werobot/ftc2024-robotcode
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Merge branch 'master' of github.com:GZod01/ftc2024-robotcode

Browse source
This commit is contained in:
GZod01 2024-03-17 15:58:00 +01:00
commit 8fd698c3b6
1 changed files with 258 additions and 262 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

520
ftc2024-carlike.java
View file

@ -23,271 +23,267 @@ import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.Velocity; import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
//test //test
@TeleOp(name="WeRobot: FTC2024 Carlike", group="WeRobot") @TeleOp(name = "WeRobot: FTC2024 Carlike", group = "WeRobot")
public class Werobot_FTC2024_carlike extends LinearOpMode { public class Werobot_FTC2024_carlike extends LinearOpMode {
/*
* Le moteur de droite // Le moteur de droite
*/ private DcMotor rm;
private DcMotor rm;
/* // Le moteur de gauche
* Le moteur de gauche private DcMotor lm;
*/
private DcMotor lm; private DcMotor moissoneuse;
/*
* La moissoneuse private DcMotor lmelevator;
*/
private DcMotor moissoneuse; private DcMotor rmelevator;
/*
* L'IMU private DcMotor box;
*/
private IMU imu; private DcMotor rotation;
/*
* La fonction pour faire des exponentielles spécifiques private IMU imu;
* @param double t => le nombre dont on veut faire l'exponentielle
* @return double une_exponentielle_très_spéciale_de_t
*/
private double helloexp(double t){
return (Math.exp(5*t)-1)/(Math.exp(5)-1);
}
/*
* La fonction du thread principal
*/
@Override
public void runOpMode() throws InterruptedException {
/*
* l'axe x du joystick gauche de la manette
*/
float x;
/*
* l'axe y du joystick gauche de la manette
*/
double y;
/*
* variation 1 du left trigger
*/
double t;
/*
* variation 2 du left trigger
*/
double t2;
/*
* variation 3 du left trigger
*/
double t3;
/*
* le mode du robot
*/
String mode = "normal";
/*
* b est il déjà préssé?
*/
boolean already_b = false;
/*
* a est il déjà préssé?
*/
boolean already_a = false;
/*
* x est il déjà préssé?
*/
boolean already_x = false;
/*
* ajout de la donnée status sur telemetry, initialisé
*/
telemetry.addData("Status", "Initialized");
/*
* mise a jour de la telemetry
*/
telemetry.update();
/*
* récupération du moteur gauche pour {@link lm}
*/
lm = hardwareMap.get(DcMotor.class, "blm");
/*
* récupération du moteur droit pour {@link rm}
*/
rm = hardwareMap.get(DcMotor.class, "brm");
/*
* récupération du moteur de moissoneuse pour {@link moissoneuse}
*/
moissoneuse = hardwareMap.get(DcMotor.class, "flm");
/* /*
* récupération de l'imu pour {@link imu} * La fonction pour faire des exponentielles spécifiques
*
* @param double t => le nombre dont on veut faire l'exponentielle
*
* @return double une_exponentielle_très_spéciale_de_t
*/ */
imu = hardwareMap.get(IMU.class, "imu"); private double helloexp(double t) {
/* return (Math.exp(5 * t) - 1) / (Math.exp(5) - 1);
* initialisation de l'imu }
*/
imu.initialize( private void tele(string name, string data) {
/* telemetry.addData(name, data);
* paramètres de l'imu telemetry.update();
*/ }
new IMU.Parameters(
/* /*
* orientation initiale du robot * La fonction du thread principal
*/ */
new RevHubOrientationOnRobot( @Override
/* public void runOpMode() throws InterruptedException {
* logo vers le haut // l'axe x du joystick gauche de la manette
*/ float x;
RevHubOrientationOnRobot.LogoFacingDirection.UP,
/* // l'axe y du joystick gauche de la manette
* usb vers l'avant
*/ double y;
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
) // variation 1 du left trigger
)
); double t;
/*
* réinitialisation du yaw de l'imu // variation 2 du left trigger
*/
imu.resetYaw(); double t2;
//telemetry.addData("Mode", "calibrating...");
//telemetry.update(); // variation 3 du left trigger
// make sure the imu gyro is calibrated before continuing. double t3;
//while (!isStopRequested() && !imu.isGyroCalibrated())
//{ // le mode du robot
// sleep(50);
// idle(); String mode = "normal";
//}
// b est il déjà préssé?
/*
* ajout de la donnée en "mode": "en attente de démarrage" sur telemetry boolean already_b = false;
*/
telemetry.addData("Mode", "waiting for start"); // a est il déjà préssé?
//telemetry.addData("imu calib status", imu.getCalibrationStatus().toString());
/* boolean already_a = false;
* mise à jour de la telemetry
*/ // x est il déjà préssé?
telemetry.update();
/* boolean already_x = false;
* en attente du démarrage
*/ telemetry.addData("Status", "Initialized");
waitForStart();
telemetry.update();
/*
* le robot a démarré, le tant que le robot est activé et donc qu'il n'a pas été stoppé: lm = hardwareMap.get(DcMotor.class, "blm");
*/
while (opModeIsActive()) { rm = hardwareMap.get(DcMotor.class, "brm");
/*
* définition de {@link x} sur la valeur de x du joystick gauche du gamepad 1 moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse");
*/
x = gamepad1.left_stick_x; lmelevator = hardwareMap.get(DcMotor.class, "ltrselv");
/* lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
* définition de {@link y} sur la valeur de y du joystick gauche du gamepad 1 lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
*/
y = gamepad1.left_stick_y; rmelevator = hardwareMap.get(DcMotor.class, "rtrselv");
/* rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
* définition de {@link t} sur la valeur du trigger droit du gamepad 1 rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
*/
t= gamepad1.right_trigger; rotation = hardwareMap.get(DcMotor.class, "elvRot");
/* rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
* définition de {@link t2} par utilisation de la fonction {@link helloexp} sur {@link t} rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
*/
t2 = helloexp(t); box = hardwareMap.get(DcMotor.class, "boxRot");
/* box.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
* définition de {@link t3} par utilisation de la fonction {@link helloexp} sur la norme du vecteur du joystick gauche du gamepad 1 (racine carrée de {@link x} au carré plus {@link y} au carré box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
*/
t3 = helloexp(Math.sqrt(Math.pow(x,2)+Math.pow(y,2))); imu = hardwareMap.get(IMU.class, "imu");
/*
* ajout de la donnée "statut":"entrain de courrir" sur telemetry imu.initialize(
*/
telemetry.addData("Status", "Running"); // paramètres de l'imu
/* new IMU.Parameters(
* si le bouton a du gamepad 1 est appuyé et {already_a} est faux // orientation initiale du robot
*/ new RevHubOrientationOnRobot(
if(gamepad1.a && !already_a){ //logo vers le haut
if(mode=="normal"){ RevHubOrientationOnRobot.LogoFacingDirection.UP,
mode="tank"; //usb vers l'avant
}else if(mode=="tank"){ RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)));
mode = "essaifranck"; // réinitialisation du yaw de l'imu
}else if (mode == "essaifranck"){ imu.resetYaw();
mode = "elina"; // telemetry.addData("Mode", "calibrating...");
}else{ // telemetry.update();
mode="normal";
} // make sure the imu gyro is calibrated before continuing.
already_a = true; // while (!isStopRequested() && !imu.isGyroCalibrated())
} // {
if(!gamepad1.a && already_a){ // sleep(50);
already_a = false; // idle();
} // }
double lpower = 0.0;
double rpower = 0.0; /*
if(mode=="normal"){ * ajout de la donnée en "mode": "en attente de démarrage" sur telemetry
double ysign = y>0?1.0:(y<0?-1.0:0.0); */
double xsign = x>0?1.0:(x<0?-1.0:0.0); telemetry.addData("Mode", "waiting for start");
lpower = -ysign * t + (xsign-2*x)*t; // telemetry.addData("imu calib status", imu.getCalibrationStatus().toString());
rpower = ysign * t + (xsign-2*x)*t; /*
} * mise à jour de la telemetry
else if (mode=="tank"){ */
lpower = -y; telemetry.update();
rpower = gamepad1.right_stick_y; /*
} * en attente du démarrage
else if (mode=="essaifranck"){ */
double a = (-y+x)/Math.pow(2,1/2); waitForStart();
double b = (-y-x)/Math.pow(2,1/2);
double vmean = (Math.abs(a)+Math.abs(b))/2; /*
lpower = (a/vmean)*t2; * le robot a démarré, le tant que le robot est activé et donc qu'il n'a pas été
rpower = (b/vmean)*t2; * stoppé:
} */
else if (mode=="elina"){ while (opModeIsActive()) {
double a = (-y+x)/Math.pow(2,1/2); /*
double b = (-y-x)/Math.pow(2,1/2); * définition de {@link x} sur la valeur de x du joystick gauche du gamepad 1
double vmean = (Math.abs(a)+Math.abs(b))/2; */
lpower = (a/vmean)*t3; x = gamepad1.left_stick_x;
rpower = (b/vmean)*t3; /*
} * définition de {@link y} sur la valeur de y du joystick gauche du gamepad 1
if(!(gamepad1.left_bumper)){ */
lpower/=3; y = gamepad1.left_stick_y;
rpower/=3; /*
} * définition de {@link t} sur la valeur du trigger droit du gamepad 1
lm.setPower(lpower); */
rm.setPower(rpower); t = gamepad1.right_trigger;
/*
if(gamepad1.b && !already_b){ * définition de {@link t2} par utilisation de la fonction {@link helloexp} sur
already_b = !already_b; * {@link t}
if(moissoneuse.getPower() == 1.0){ */
moissoneuse.setPower(0); t2 = helloexp(t);
}else{ /*
moissoneuse.setPower(1); * définition de {@link t3} par utilisation de la fonction {@link helloexp} sur
} * la norme du vecteur du joystick gauche du gamepad 1 (racine carrée de {@link
} * x} au carré plus {@link y} au carré
if(!gamepad1.b && already_b){ */
already_b = !already_b; t3 = helloexp(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)));
} /*
if(gamepad1.x && !already_x){ * ajout de la donnée "statut":"entrain de courrir" sur telemetry
already_x = !already_x; */
if(moissoneuse.getPower() == -1.0){ telemetry.addData("Status", "Running");
moissoneuse.setPower(0); /*
}else{ * si le bouton a du gamepad 1 est appuyé et {already_a} est faux
moissoneuse.setPower(-1); */
} if (gamepad1.a && !already_a) {
if(!gamepad1.x && already_x){ if (mode == "normal") {
already_x = !already_x; mode = "tank";
} } else if (mode == "tank") {
} mode = "essaifranck";
} else if (mode == "essaifranck") {
mode = "elina";
telemetry.addData("x",x); } else {
telemetry.addData("y",y); mode = "normal";
telemetry.addData("lpow",lpower); }
telemetry.addData("rpow",rpower); already_a = true;
telemetry.addData("ltrigg",t); }
telemetry.addData("t2",t2); if (!gamepad1.a && already_a) {
telemetry.addData("mode",mode); already_a = false;
// Create an object to receive the IMU angles }
YawPitchRollAngles robotOrientation; double lpower = 0.0;
robotOrientation = imu.getRobotYawPitchRollAngles(); double rpower = 0.0;
if (mode == "normal") {
// Now use these simple methods to extract each angle double ysign = y > 0 ? 1.0 : (y < 0 ? -1.0 : 0.0);
// (Java type double) from the object you just created: double xsign = x > 0 ? 1.0 : (x < 0 ? -1.0 : 0.0);
double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); lpower = -ysign * t + (xsign - 2 * x) * t;
double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES); rpower = ysign * t + (xsign - 2 * x) * t;
double Roll = robotOrientation.getRoll(AngleUnit.DEGREES); tele("mode", "normal");
telemetry.addData("yaw",Yaw); } else if (mode == "tank") {
lpower = -y;
telemetry.update(); rpower = gamepad1.right_stick_y;
tele("mode", "tank");
} else if (mode == "essaifranck") {
double a = (-y + x) / Math.pow(2, 1 / 2);
double b = (-y - x) / Math.pow(2, 1 / 2);
double vmean = (Math.abs(a) + Math.abs(b)) / 2;
lpower = (a / vmean) * t2;
rpower = (b / vmean) * t2;
tele("mode", "essai Franck");
} else if (mode == "elina") {
double a = (-y + x) / Math.pow(2, 1 / 2);
double b = (-y - x) / Math.pow(2, 1 / 2);
double vmean = (Math.abs(a) + Math.abs(b)) / 2;
lpower = (a / vmean) * t3;
rpower = (b / vmean) * t3;
tele("mode", "Elina");
}
if (!(gamepad1.left_bumper)) {
lpower /= 3;
rpower /= 3;
}
lm.setPower(lpower);
rm.setPower(rpower);
if (gamepad1.b && !already_b) {
double moissoneuseSpeed = 1.0;
if (gamepad1.right_bumper){
moissoneuseSpeed = -1.0;
}
already_b = !already_b;
if (moissoneuse.getPower() == moissoneuseSpeed) {
moissoneuse.setPower(0);
} else {
moissoneuse.setPower(moissoneuseSpeed);
}
}
if (!gamepad1.b && already_b) {
already_b = !already_b;
}
telemetry.addData("x", x);
telemetry.addData("y", y);
telemetry.addData("lpow", lpower);
telemetry.addData("rpow", rpower);
telemetry.addData("ltrigg", t);
telemetry.addData("t2", t2);
telemetry.addData("mode", mode);
// Create an object to receive the IMU angles
YawPitchRollAngles robotOrientation;
robotOrientation = imu.getRobotYawPitchRollAngles();
// Now use these simple methods to extract each angle
// (Java type double) from the object you just created:
double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES);
double Roll = robotOrientation.getRoll(AngleUnit.DEGREES);
telemetry.addData("yaw", Yaw);
telemetry.update();
}
} }
}
} }