diff --git a/ftc2024-carlike.java b/ftc2024-carlike.java index f6b5302..de9142b 100644 --- a/ftc2024-carlike.java +++ b/ftc2024-carlike.java @@ -23,271 +23,273 @@ import org.firstinspires.ftc.robotcore.external.navigation.Position; import org.firstinspires.ftc.robotcore.external.navigation.Velocity; //test -@TeleOp(name="WeRobot: FTC2024 Carlike", group="WeRobot") +@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); - } - /* - * 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"); + + // Le moteur de droite + private DcMotor rm; + + // Le moteur de gauche + private DcMotor lm; + + private DcMotor moissoneuse; + + private DcMotor lmelevator; + + private DcMotor rmelevator; + + private DcMotor box; + + private DcMotor rotation; + + private IMU imu; /* - * 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"); - /* - * 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; - } - } - - - 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(); + private double helloexp(double t) { + return (Math.exp(5 * t) - 1) / (Math.exp(5) - 1); + } + + private void tele(string name, string data) { + telemetry.addData(name, data); + telemetry.update(); + } + + /* + * 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; + + telemetry.addData("Status", "Initialized"); + + telemetry.update(); + + lm = hardwareMap.get(DcMotor.class, "blm"); + + rm = hardwareMap.get(DcMotor.class, "brm"); + + moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse"); + + lmelevator = hardwareMap.get(DcMotor.class, "ltrselv"); + lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + rmelevator = hardwareMap.get(DcMotor.class, "rtrselv"); + rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + rotation = hardwareMap.get(DcMotor.class, "elvRot"); + rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + box = hardwareMap.get(DcMotor.class, "boxRot"); + box.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + imu = hardwareMap.get(IMU.class, "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; + tele("mode", "normal"); + } else if (mode == "tank") { + lpower = -y; + 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) { + 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(); + } } - } }