diff --git a/ftc2024-carlike.java b/ftc2024-carlike.java index 30526ba..f6b5302 100644 --- a/ftc2024-carlike.java +++ b/ftc2024-carlike.java @@ -25,155 +25,269 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity; //test @TeleOp(name="WeRobot: FTC2024 Carlike", group="WeRobot") public class Werobot_FTC2024_carlike extends LinearOpMode { + /* + * Le moteur de droite + */ private DcMotor rm; + /* + * Le moteur de gauche + */ private DcMotor lm; + /* + * La moissoneuse + */ private DcMotor moissoneuse; + /* + * L'IMU + */ private IMU 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 + */ private double helloexp(double t){ - return (Math.exp(5*t)-1)/(Math.exp(5)-1); + return (Math.exp(5*t)-1)/(Math.exp(5)-1); } + /* + * La fonction du thread principal + */ @Override public void runOpMode() throws InterruptedException { - float x; - double y; - double t; - double t2; - double t3; - String mode = "normal"; - boolean already_b = false; - boolean already_a = false; - boolean already_x = false; - telemetry.addData("Status", "Initialized"); - telemetry.update(); - lm = hardwareMap.get(DcMotor.class, "blm"); - rm = hardwareMap.get(DcMotor.class, "brm"); - moissoneuse = hardwareMap.get(DcMotor.class, "flm"); + /* + * 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} + */ + imu = hardwareMap.get(IMU.class, "imu"); + /* + * initialisation de l'imu + */ + imu.initialize( + /* + * paramètres de l'imu + */ + new IMU.Parameters( + /* + * orientation initiale du robot + */ + new RevHubOrientationOnRobot( + /* + * logo vers le haut + */ + RevHubOrientationOnRobot.LogoFacingDirection.UP, + /* + * usb vers l'avant + */ + RevHubOrientationOnRobot.UsbFacingDirection.FORWARD + ) + ) + ); + /* + * réinitialisation du yaw de l'imu + */ + imu.resetYaw(); + //telemetry.addData("Mode", "calibrating..."); + //telemetry.update(); + + // make sure the imu gyro is calibrated before continuing. + //while (!isStopRequested() && !imu.isGyroCalibrated()) + //{ + // sleep(50); + // idle(); + //} + + /* + * ajout de la donnée en "mode": "en attente de démarrage" sur telemetry + */ + telemetry.addData("Mode", "waiting for start"); + //telemetry.addData("imu calib status", imu.getCalibrationStatus().toString()); + /* + * mise à jour de la telemetry + */ + telemetry.update(); + /* + * en attente du démarrage + */ + waitForStart(); + + /* + * le robot a démarré, le tant que le robot est activé et donc qu'il n'a pas été stoppé: + */ + while (opModeIsActive()) { + /* + * définition de {@link x} sur la valeur de x du joystick gauche du gamepad 1 + */ + x = gamepad1.left_stick_x; + /* + * définition de {@link y} sur la valeur de y du joystick gauche du gamepad 1 + */ + y = gamepad1.left_stick_y; + /* + * définition de {@link t} sur la valeur du trigger droit du gamepad 1 + */ + t= gamepad1.right_trigger; + /* + * définition de {@link t2} par utilisation de la fonction {@link helloexp} sur {@link t} + */ + t2 = helloexp(t); + /* + * 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é + */ + t3 = helloexp(Math.sqrt(Math.pow(x,2)+Math.pow(y,2))); + /* + * ajout de la donnée "statut":"entrain de courrir" sur telemetry + */ + telemetry.addData("Status", "Running"); + /* + * si le bouton a du gamepad 1 est appuyé et {already_a} est faux + */ + if(gamepad1.a && !already_a){ + if(mode=="normal"){ + mode="tank"; + }else if(mode=="tank"){ + mode = "essaifranck"; + }else if (mode == "essaifranck"){ + mode = "elina"; + }else{ + mode="normal"; + } + already_a = true; + } + if(!gamepad1.a && already_a){ + already_a = false; + } + double lpower = 0.0; + double rpower = 0.0; + if(mode=="normal"){ + double ysign = y>0?1.0:(y<0?-1.0:0.0); + double xsign = x>0?1.0:(x<0?-1.0:0.0); + lpower = -ysign * t + (xsign-2*x)*t; + rpower = ysign * t + (xsign-2*x)*t; + } + else if (mode=="tank"){ + lpower = -y; + rpower = gamepad1.right_stick_y; + } + 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; + } + 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; + } + if(!(gamepad1.left_bumper)){ + lpower/=3; + rpower/=3; + } + lm.setPower(lpower); + rm.setPower(rpower); + + if(gamepad1.b && !already_b){ + already_b = !already_b; + if(moissoneuse.getPower() == 1.0){ + moissoneuse.setPower(0); + }else{ + moissoneuse.setPower(1); + } + } + if(!gamepad1.b && already_b){ + already_b = !already_b; + } + if(gamepad1.x && !already_x){ + already_x = !already_x; + if(moissoneuse.getPower() == -1.0){ + moissoneuse.setPower(0); + }else{ + moissoneuse.setPower(-1); + } + if(!gamepad1.x && already_x){ + already_x = !already_x; + } + } - imu = hardwareMap.get(IMU.class, "imu"); - imu.initialize( - new IMU.Parameters( - new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.UP, - RevHubOrientationOnRobot.UsbFacingDirection.FORWARD - ) - ) - ); - imu.resetYaw(); - //telemetry.addData("Mode", "calibrating..."); - //telemetry.update(); + 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(); - // make sure the imu gyro is calibrated before continuing. - //while (!isStopRequested() && !imu.isGyroCalibrated()) - //{ - // sleep(50); - // idle(); - //} + // 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.addData("Mode", "waiting for start"); - //telemetry.addData("imu calib status", imu.getCalibrationStatus().toString()); - telemetry.update(); - waitForStart(); - - //test - while (opModeIsActive()) { - x = gamepad1.left_stick_x; - y = gamepad1.left_stick_y; - t= gamepad1.right_trigger; - t2 = helloexp(t); - t3 = helloexp(Math.sqrt(Math.pow(x,2)+Math.pow(y,2))); - telemetry.addData("Status", "Running"); - if(gamepad1.a && !already_a){ - if(mode=="normal"){ - mode="tank"; - }else if(mode=="tank"){ - mode = "essaifranck"; - }else if (mode == "essaifranck"){ - mode = "elina"; - }else{ - mode="normal"; - } - already_a = true; - } - if(!gamepad1.a && already_a){ - already_a = false; - } - double lpower = 0.0; - double rpower = 0.0; - if(mode=="normal"){ - double ysign = y>0?1.0:(y<0?-1.0:0.0); - double xsign = x>0?1.0:(x<0?-1.0:0.0); - lpower = -ysign * t + (xsign-2*x)*t; - rpower = ysign * t + (xsign-2*x)*t; - } - else if (mode=="tank"){ - lpower = -y; - rpower = gamepad1.right_stick_y; - } - 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; - } - 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; - } - if(!(gamepad1.left_bumper)){ - lpower/=3; - rpower/=3; - } - lm.setPower(lpower); - rm.setPower(rpower); - - if(gamepad1.b && !already_b){ - already_b = !already_b; - if(moissoneuse.getPower() == 1.0){ - moissoneuse.setPower(0); - }else{ - moissoneuse.setPower(1); - } - } - if(!gamepad1.b && already_b){ - already_b = !already_b; - } - if(gamepad1.x && !already_x){ - already_x = !already_x; - if(moissoneuse.getPower() == -1.0){ - moissoneuse.setPower(0); - }else{ - moissoneuse.setPower(-1); - } - if(!gamepad1.x && already_x){ - already_x = !already_x; - } - } - - - 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(); - } + telemetry.update(); + } } }