diff --git a/FTC2024WeRobotControl.java b/FTC2024WeRobotControl.java index cf5e932..25b1bcb 100644 --- a/FTC2024WeRobotControl.java +++ b/FTC2024WeRobotControl.java @@ -38,6 +38,9 @@ public class FTC2024WeRobotControl { * the width size of the tiles on the ground in metres */ private final double ground_tiles_width = 61.0e-2; // metres + // + private final double defaultspeed = 0.6; + private final double defaultanglespeed = 0.4; private ElapsedTime timer; @@ -102,7 +105,7 @@ public class FTC2024WeRobotControl { } public void forward(double n_tiles){ - this.forward(n_tiles,1); + this.forward(n_tiles,this.defaultspeed); } /* @@ -119,7 +122,7 @@ public class FTC2024WeRobotControl { } public void backward(double n_tiles){ - this.backward(n_tiles,1); + this.backward(n_tiles,this.defaultspeed); } /* @@ -167,7 +170,7 @@ public class FTC2024WeRobotControl { Parent.rm.setPower(0); } public void rotate(double angle){ - this.rotate(angle,1.0); + this.rotate(angle,this.defaultanglespeed); } public void test_forward_10_and_rotate_20deg(){ diff --git a/ftc2024-carlike.java b/ftc2024-carlike.java index b38cef9..6dda021 100644 --- a/ftc2024-carlike.java +++ b/ftc2024-carlike.java @@ -26,268 +26,323 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity; @TeleOp(name = "WeRobot: FTC2024 Carlike", group = "WeRobot") public class Werobot_FTC2024_carlike extends LinearOpMode { - // Le moteur de droite - private DcMotor rm; + // Le moteur de droite + private DcMotor rm; - // Le moteur de gauche - private DcMotor lm; + // Le moteur de gauche + private DcMotor lm; - private DcMotor moissoneuse; + private DcMotor moissoneuse; - private DcMotorEx lmelevator; + private DcMotorEx lmelevator; - private DcMotorEx rmelevator; + private DcMotorEx rmelevator; - private DcMotorEx box; + private DcMotorEx box; - private DcMotorEx rotation; + private DcMotorEx rotation; - private IMU 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 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); + } - private void tele(string name, string data) { - telemetry.addData(name, data); - telemetry.update(); - } + 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; + /* + * 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 + // l'axe y du joystick gauche de la manette - double y; + double y; - // variation 1 du left trigger + // variation 1 du left trigger - double t; + double t; - // variation 2 du left trigger + // variation 2 du left trigger - double t2; + double t2; - // variation 3 du left trigger + // variation 3 du left trigger - double t3; + double t3; - // le mode du robot + // le mode du robot - String mode = "normal"; + String mode = "elina"; - // b est il déjà préssé? + // b est il déjà préssé? - boolean already_b = false; + boolean already_b = false; - // a est il déjà préssé? + boolean already_a = false; - boolean already_a = false; + boolean already_x = false; + + boolean already_y = false; - // x est il déjà préssé? + boolean already_up = false; - boolean already_x = false; + boolean already_down = false; + + boolean sinking = false; - boolean already_up = false; + telemetry.addData("Status", "Initialized"); - boolean already_down = false; + // telemetry.update(); - telemetry.addData("Status", "Initialized"); + lm = hardwareMap.get(DcMotor.class, "blm"); - telemetry.update(); + rm = hardwareMap.get(DcMotor.class, "brm"); - lm = hardwareMap.get(DcMotor.class, "blm"); + moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse"); - rm = hardwareMap.get(DcMotor.class, "brm"); + lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv"); + lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse"); + rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv"); + rmelevator.setDirection(DcMotor.Direction.REVERSE); + rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv"); - lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rotation = hardwareMap.get(DcMotorEx.class, "elvRot"); + rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv"); - rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + box = hardwareMap.get(DcMotorEx.class, "boxRot"); + box.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rotation = hardwareMap.get(DcMotorEx.class, "elvRot"); - rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + imu = hardwareMap.get(IMU.class, "imu"); + // rotation positions: 20° pos initiale par rapport au sol + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rotation.setVelocity(700, AngleUnit.DEGREES); + rotation.setTargetPosition(40*288/360); + + + + + imu.initialize( - box = hardwareMap.get(DcMotorEx.class, "boxRot"); - box.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + // 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(); - imu = hardwareMap.get(IMU.class, "imu"); + // make sure the imu gyro is calibrated before continuing. + // while (!isStopRequested() && !imu.isGyroCalibrated()) + // { + // sleep(50); + // idle(); + // } - imu.initialize( + /* + * 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(); + while (opModeIsActive()){ + telemetry.addData("rotation target Pos", rotation.getTargetPosition()); + telemetry.addData("rotation Pos", rotation.getCurrentPosition()); + telemetry.addData("rotation velocity", rotation.getVelocity()); + telemetry.update(); + } + /* + * 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 = "elina"; + } + 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");*/ + 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_trigger>0.1) { + lpower /= 3; + rpower /= 3; + } + lm.setPower(lpower); + rm.setPower(rpower); - // 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) { - 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 = false; - } - if ((gamepad1.dpad_up && !already_up)^(gamepad1.dpad_down && !already_down)){ - lmelevator.setVelocity(100); - rmelevator.setVelocity(100); - Long targetPosLong = (Long) Math.round(288*3.45); + 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 = false; + } + + if (sinking && Math.abs(lmelevator.getCurrentPosition()-90)<=5 && Math.abs(rmelevator.getCurrentPosition()-90)<=5){ + lmelevator.setVelocity(100); + rmelevator.setVelocity(100); + lmelevator.setTargetPosition(0); + rmelevator.setTargetPosition(0); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + if ((gamepad1.dpad_up && !already_up)^(gamepad1.dpad_down && !already_down)){ + lmelevator.setVelocity(600); + rmelevator.setVelocity(600); + Long targetPosLong = (Long) Math.round(288*3.4); int targetPos = targetPosLong.intValue(); - if (gamepad1.dpad_down){ - targetPos = 0; - already_down = true; - }else{ - already_up = true; - } + if (gamepad1.dpad_down){ + targetPos = 90; + already_down = true; + sinking = true; + }else{ + already_up = true; + sinking = false; + } lmelevator.setTargetPosition(targetPos); - rmelevator.setTargetPosition(targetPos); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - }else if (!gamepad1.dpad_up && already_up){ - already_up = false; - }else if (!gamepad1.dpad_down && already_down){ - already_down = false; - } + rmelevator.setTargetPosition(targetPos); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + }else if (!gamepad1.dpad_up && already_up){ + already_up = false; + }else if (!gamepad1.dpad_down && already_down){ + already_down = false; + } + + if (gamepad1.x && !already_x){ + box.setVelocity(200); + int targetPos = 0; + if(gamepad1.right_bumper){ + targetPos = 0; + } + box.setTargetPosition(targetPos); + box.setMode(DcMotor.RunMode.RUN_TO_POSITION); + already_x = !already_x; + } else if(!gamepad1.x && already_x){ + already_x = false; + } + + if (gamepad1.y && already_y){ + rotation.setVelocity(450); + int targetPos = 0; + if (gamepad1.right_bumper){ + targetPos = 0; + }else if (gamepad1.left_bumper){ + targetPos = 0; + } + rotation.setTargetPosition(targetPos); + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (gamepad1.right_bumper && gamepad1.left_bumper){ + // launch the plane + } +<<<<<<< HEAD telemetry.addData("x", x); telemetry.addData("y", y); telemetry.addData("lpow", lpower); @@ -297,15 +352,36 @@ public class Werobot_FTC2024_carlike extends LinearOpMode { // 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.addData("x", x); + telemetry.addData("y", y); + telemetry.addData("lpow", lpower); + telemetry.addData("rpow", rpower); + telemetry.addData("ltrigg", t); + telemetry.addData("t2", t2); + // Create an object to receive the IMU angles + YawPitchRollAngles robotOrientation; + robotOrientation = imu.getRobotYawPitchRollAngles(); +>>>>>>> cdd50be8b9f970f1db99003d4c401bf7dafa25b6 + + // 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); + +<<<<<<< HEAD telemetry.update(); } } -} \ No newline at end of file +} +======= + telemetry.update(); + } + } +} +>>>>>>> cdd50be8b9f970f1db99003d4c401bf7dafa25b6 diff --git a/ftc2024_autonome_api.java b/ftc2024_autonome_api.java index 7126dec..4424484 100644 --- a/ftc2024_autonome_api.java +++ b/ftc2024_autonome_api.java @@ -32,7 +32,11 @@ public class Ftc2024_autonome_api extends LinearOpMode { public void runOpMode() { lm = hardwareMap.get(DcMotor.class, "blm"); rm = hardwareMap.get(DcMotor.class, "brm"); - harvestmotor = hardwareMap.get(DcMotor.class, "flm"); +<<<<<<< HEAD + harvestmotor = hardwareMap.get(DcMotor.class, "moissonneuse"); +======= + harvestmotor = hardwareMap.get(DcMotor.class, "moissoneuse"); +>>>>>>> a8ae6d3 (hello) rm.setDirection(DcMotor.Direction.REVERSE); diff --git a/ftc_new.java b/ftc_new.java index 46716b6..c4b877e 100644 --- a/ftc_new.java +++ b/ftc_new.java @@ -329,10 +329,7 @@ public class Werobot_FTC2024_carlike extends LinearOpMode { if (gamepad1.right_bumper && gamepad1.left_bumper){ // launch the plane } - - - telemetry.addData("x", x); telemetry.addData("y", y); telemetry.addData("lpow", lpower); diff --git a/list.py b/list.py deleted file mode 100644 index ecb2683..0000000 --- a/list.py +++ /dev/null @@ -1,12 +0,0 @@ - - -def calcul_suite(n): - res1 = [1]; - for i in range(n): - res2 = 0; - for j in res1: - res2+=j; - res2+=1; - res1.append(res2); - return res1[n-1]; - diff --git a/test.java b/test.java index 3b9b3f8..35baae3 100644 --- a/test.java +++ b/test.java @@ -49,7 +49,7 @@ public class ftc2024_autonome_test extends LinearOpMode { waitForStart(); - while (opModeIsActive()){ + if (opModeIsActive()){ double [] lm_p = {0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1}; double [] rm_p = {-0.1,-0.2,-0.3,-0.4,-0.5,-0.6,-0.7,-0.8,-0.9,-1}; double [] y = new double[lm_p.length]; @@ -99,4 +99,4 @@ public class ftc2024_autonome_test extends LinearOpMode { } } } -} \ No newline at end of file +}