diff --git a/.ftc2024_autonome_api.java.un~ b/.ftc2024_autonome_api.java.un~ new file mode 100644 index 0000000..b8fdabd Binary files /dev/null and b/.ftc2024_autonome_api.java.un~ differ diff --git a/.ftc_new.java.un~ b/.ftc_new.java.un~ new file mode 100644 index 0000000..b0464be Binary files /dev/null and b/.ftc_new.java.un~ differ diff --git a/ftc2024_autonome_api.java b/ftc2024_autonome_api.java index c37f0d6..0ae9749 100644 --- a/ftc2024_autonome_api.java +++ b/ftc2024_autonome_api.java @@ -82,12 +82,15 @@ public class Ftc2024_autonome_api extends LinearOpMode { switch (autonomous_mode) { default: robot.boxElv(); + robot.harvest(1); robot.forward(0.5); + robot.harvest(0); robot.rotate((-90)); robot.forward(1.5); robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case B2D: robot.boxElv(); @@ -97,6 +100,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case R4D: robot.boxElv(); @@ -106,6 +110,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case R2D: robot.boxElv(); @@ -115,6 +120,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case B4N: robot.boxElv(); @@ -132,6 +138,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case B2N: robot.boxElv(); @@ -140,6 +147,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(); robot.forward(1); robot.harvest(0); + robot.forward(1); robot.rotate(180); robot.forward(1); robot.rotate(-90); @@ -149,6 +157,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case R4N: robot.boxElv(); @@ -166,6 +175,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case R2N: robot.boxElv(); @@ -174,6 +184,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(); robot.forward(1); robot.harvest(0); + robot.forward(1); robot.rotate(180); robot.forward(1); robot.rotate(90); @@ -183,8 +194,9 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; } } } -} \ No newline at end of file +} diff --git a/ftc2024_autonome_api.java~ b/ftc2024_autonome_api.java~ new file mode 100644 index 0000000..8d90a84 Binary files /dev/null and b/ftc2024_autonome_api.java~ differ diff --git a/ftc_new.java b/ftc_new.java index 25b0002..5948748 100644 --- a/ftc_new.java +++ b/ftc_new.java @@ -26,7 +26,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity; @TeleOp(name = "WeRobot: FTC2024 NEW! Carlike", group = "WeRobot") public class WEROBOT_FTC2024_New_carlike extends LinearOpMode { public enum RobotMode { - ESSAIFRANCK, ELINA, NORMAL, TANK; + ESSAIFRANCK, ELINA, NORMAL, TANK; } private DcMotorEx rm; @@ -48,516 +48,516 @@ public class WEROBOT_FTC2024_New_carlike extends LinearOpMode { * @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); } public void nextMode() { - RobotMode toNextMode; - switch (this.mode) { - case ESSAIFRANCK: - toNextMode = RobotMode.ELINA; - break; - case ELINA: - toNextMode = RobotMode.NORMAL; - break; - case NORMAL: - toNextMode = RobotMode.TANK; - break; - default: - toNextMode = RobotMode.ESSAIFRANCK; - break; - } - this.mode = toNextMode; + RobotMode toNextMode; + switch (this.mode) { + case ESSAIFRANCK: + toNextMode = RobotMode.ELINA; + break; + case ELINA: + toNextMode = RobotMode.NORMAL; + break; + case NORMAL: + toNextMode = RobotMode.TANK; + break; + default: + toNextMode = RobotMode.ESSAIFRANCK; + break; + } + this.mode = toNextMode; } public boolean isBetween(double elem, double mini, double maxi){ - return Math.abs(elem - (((maxi-mini)/2)+mini))<=(maxi-mini)/2; + return Math.abs(elem - (((maxi-mini)/2)+mini))<=(maxi-mini)/2; } // La fonction du thread principal @Override public void runOpMode() throws InterruptedException { - double boxRot = 0; - int signeBR; + double boxRot = 0; + int signeBR; - float x; - double y; + float x; + double y; - double t; - double t2; - double t3; + double t; + double t2; + double t3; - boolean already_b = false; - boolean already_a = false; - boolean already_x = false; - boolean already_y = false; - boolean already_up = false; - boolean already_down = false; - boolean already_ps = false; - - boolean already_paddown = false; - boolean already_padup = false; - boolean already_padright = false; - boolean already_padleft = false; - + boolean already_b = false; + boolean already_a = false; + boolean already_x = false; + boolean already_y = false; + boolean already_up = false; + boolean already_down = false; + boolean already_ps = false; - boolean sinking = false; - boolean manualMode = false; - boolean firstLaunch = true; - - telemetry.addData("Status", "Initialized"); - telemetry.addData("Version", "5.123"); - - lm = hardwareMap.get(DcMotorEx.class, "blm"); - - rm = hardwareMap.get(DcMotorEx.class, "brm"); - rm.setDirection(DcMotor.Direction.REVERSE); - - lm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - rm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - lm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - - moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse"); - moissoneuse.setDirection(DcMotor.Direction.REVERSE); - - lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv"); - lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv"); - rmelevator.setDirection(DcMotor.Direction.REVERSE); - rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - rotation = hardwareMap.get(DcMotorEx.class, "elvRot"); - rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - box = hardwareMap.get(DcMotorEx.class, "boxRot"); - box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - box.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - avion = hardwareMap.get(Servo.class, "avion"); - // box.setPositionPIDFCoefficients(5.0); - - // rotation positions: 20° pos initiale par rapport au sol - // while (runtime.seconds()<0.5){ - // rotation.setPower(0.5); - // } - - telemetry.addData("Mode", "waiting for start"); - - // rotation.setVelocity(1700); - // rotation.setTargetPosition(-1000); - // rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - telemetry.addData("rotation target Pos", rotation.getTargetPosition()); - telemetry.addData("rotation Pos", rotation.getCurrentPosition()); - telemetry.update(); - - waitForStart(); - rotation.setVelocity(200); - rotation.setTargetPosition(0); - rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - while (opModeIsActive()) { - - x = gamepad1.left_stick_x; - 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))); - - telemetry.addData("Status", "Running"); - - - if (gamepad1.right_bumper && gamepad1.left_bumper){ - avion.setDirection(Servo.Direction.REVERSE); - // telemetry.addData("servoPos",avion.getPosition()); - // avion.setPosition(0); - avion.setPosition(0.75); - // continue; - } - // Choix mode conduite / actif en manuel et auto - if (gamepad1.a && !already_a) { - nextMode(); - already_a = true; - } - if (!gamepad1.a && already_a) { - already_a = false; - } - double lpower = 0.0; - double rpower = 0.0; - double vmean; - double a; - double b; - - switch (mode) { - - case NORMAL: - double ysign = Math.signum(y); - double xsign = Math.signum(x); - lpower = ysign * t + (-xsign - 2 * x) * t; - rpower = ysign * t + (xsign - 2 * x) * t; - break; + boolean already_paddown = false; + boolean already_padup = false; + boolean already_padright = false; + boolean already_padleft = false; - case TANK: - lpower = -y; - rpower = -gamepad1.right_stick_y; - break; + boolean sinking = false; + boolean manualMode = false; + boolean firstLaunch = true; - case ESSAIFRANCK: - // Code ci-dessous OK - // lpower = (1 + x); //(a / vmean) * t2; - // rpower = 2-lpower;//(b / vmean) * t2; - // lpower*=t2*((Math.signum(y)==0)?1:-Math.signum(y)); // sigNum(0) - // rpower*=t2*((Math.signum(y)==0)?1:-Math.signum(y)); - // Fin code OK - - int ltargetPos, rtargetPos; - ltargetPos = rtargetPos = 0; - - int lmpos = lm.getCurrentPosition(); - int rmpos = rm.getCurrentPosition(); - int step = 100; - // double rapp = step/theta; - double signe = Math.signum(-y); - signe = (signe == 0)?1.0:signe; - if(Math.abs(x)<=0.1 && Math.abs(y)<=0.1){ - ltargetPos = rtargetPos = 0; - }else if (Math.abs(x)<=0.1){ - ltargetPos = rtargetPos = 100; - }else if (isBetween(x,0.1,0.9)){ - rtargetPos = step; - ltargetPos = 2*step; - }else if (x>=0.9){ - ltargetPos = step; - rtargetPos = -step; - }else if (x<=-0.9){ - ltargetPos = -step; - rtargetPos = step; - }else if (isBetween(x, -0.9,-0.1)){ - rtargetPos = 2*step; - ltargetPos = step; - } - ltargetPos*=signe; - rtargetPos*=signe; - - // if (x>0.1){ - // theta = Math.abs(Math.PI/2 - Math.atan2(-y,x)); - // theta = (theta > Math.PI/2)?(Math.PI - theta):theta; - // ltargetPos = (int) (Math.floor(theta*37.0 + step)*signe); - // rtargetPos = (int) (Math.floor(step)*signe); - // } - // else if (x < -0.1){ - // theta = Math.abs(Math.abs(Math.atan2(-y,x)) - Math.PI/2); - // rtargetPos = (int) (Math.floor(theta*37.0 + step)*signe); - // ltargetPos = (int) (Math.floor(step)*signe); - // } - // else { - // rtargetPos = step*((int) Math.signum(-y)); - // ltargetPos = step*((int) Math.signum(-y)); - // } - - telemetry.addData("ltargetPos avant cut",ltargetPos); - telemetry.addData("rtargetPos avant cut",rtargetPos); - - // ltargetPos = ((Math.abs(ltargetPos)> step)?(int) Math.signum(ltargetPos)*(step + Math.abs(ltargetPos)%step ):ltargetPos); - // rtargetPos = ((Math.abs(rtargetPos)> step)?(int) Math.signum(rtargetPos)*(step + Math.abs(rtargetPos)%step):rtargetPos); - - lm.setTargetPosition(lmpos + ltargetPos); - rm.setTargetPosition(rmpos + rtargetPos); - lm.setVelocity(2800.0*t2); - rm.setVelocity(2800.0*t2); - - - // telemetry.addData("rapp",rapp); - telemetry.addData("ltargetPos",ltargetPos); - telemetry.addData("rtargetPos",rtargetPos); - - lm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - - - break; + telemetry.addData("Status", "Initialized"); + telemetry.addData("Version", "5.123"); - case ELINA: - a = (-y + x) / Math.pow(2, 1 / 2); - b = (-y - x) / Math.pow(2, 1 / 2); - vmean = (Math.abs(a) + Math.abs(b)) / 2; - lpower = (a / vmean) * t3; - rpower = (b / vmean) * t3; - break; + lm = hardwareMap.get(DcMotorEx.class, "blm"); - } + rm = hardwareMap.get(DcMotorEx.class, "brm"); + rm.setDirection(DcMotor.Direction.REVERSE); - if (gamepad1.left_trigger > 0.1) { - lpower /= 3; - rpower /= 3; - } + lm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + lm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - // CODE OK - // lm.setPower(lpower/1.5); - // rm.setPower(rpower/1.5); - // Fin code OK + moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse"); + moissoneuse.setDirection(DcMotor.Direction.REVERSE); - // activation moissonneuse -- actif en manuel et auto - 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; - } + lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv"); + lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - // activation elevateur - 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 = 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); + rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv"); + rmelevator.setDirection(DcMotor.Direction.REVERSE); + rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - } else if (!gamepad1.dpad_up && already_up) { - already_up = false; - } else if (!gamepad1.dpad_down && already_down) { - already_down = false; - } + rotation = hardwareMap.get(DcMotorEx.class, "elvRot"); + rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - if (gamepad1.ps && !already_ps) { - manualMode = !manualMode; - already_ps = true; - } else if (!gamepad1.ps && already_ps) { - already_ps = false; - } + box = hardwareMap.get(DcMotorEx.class, "boxRot"); + box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + box.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - // activation rotation - if (manualMode) { - gamepad1.setLedColor(1.0, 0.0, 0.0,255); - // lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - // rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - rotation.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - box.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - telemetry.addData("mode","MANUEL"); - // Elevator manual mode - if (gamepad1.dpad_up) { - // rmelevator.setPower(0.3); - // lmelevator.setPower(0.3); - lmelevator.setVelocity(400); - rmelevator.setVelocity(400); - int lpos = rmelevator.getCurrentPosition(); - int rpos = lmelevator.getCurrentPosition(); - lmelevator.setTargetPosition(lpos + 15); - rmelevator.setTargetPosition(rpos + 15); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } else if (gamepad1.dpad_down) { - // lmelevator.setPower(-0.3); - // rmelevator.setPower(-0.3); - lmelevator.setVelocity(400); - rmelevator.setVelocity(400); - int lpos = rmelevator.getCurrentPosition(); - int rpos = lmelevator.getCurrentPosition(); - lmelevator.setTargetPosition(lpos - 15); - rmelevator.setTargetPosition(rpos - 15); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } else if (gamepad1.y) { - double power = -0.3; - if (gamepad1.right_bumper) { - power = -power; - } - rotation.setPower(power); - } else { - // lmelevator.setPower(0); - // rmelevator.setPower(0); - rotation.setPower(0); - // lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - // rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - } - // Box manual mode - if (gamepad1.dpad_left) { - box.setPower(0.3); - box.setPower(0.3); - } else if (gamepad1.dpad_right) { - box.setPower(-0.3); - box.setPower(-0.3); - } - else { - box.setPower(0); - box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - } - // Accrochage final - if (gamepad1.x){ - // lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - // rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - lmelevator.setVelocity(600); - rmelevator.setVelocity(600); - lmelevator.setTargetPosition(40); - rmelevator.setTargetPosition(40); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } + avion = hardwareMap.get(Servo.class, "avion"); + // box.setPositionPIDFCoefficients(5.0); - } else { - gamepad1.setLedColor(0.0, 0.0, 0.0,10); - telemetry.addData("mode","AUTOMATIQUE"); - - if (!gamepad1.dpad_right && already_padright) { - already_padright = false; - } - if (gamepad1.dpad_right && !already_padright) { - already_padright = true; - // POSITION INITIALE - rotation.setVelocity(600); - rotation.setTargetPosition(0); - - lmelevator.setVelocity(600); - lmelevator.setTargetPosition(0); - rmelevator.setVelocity(600); - rmelevator.setTargetPosition(0); - - rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - if (!gamepad1.dpad_left && already_padleft) { - already_padleft = false; - } - if (gamepad1.dpad_left && !already_padleft) { - already_padleft = true; - // POSITION ROULAGE / Rammase Pixel dans boite - rotation.setVelocity(600); - rotation.setTargetPosition(-50); - - lmelevator.setVelocity(600); - lmelevator.setTargetPosition(150); - rmelevator.setVelocity(600); - rmelevator.setTargetPosition(150); - - rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - + // rotation positions: 20° pos initiale par rapport au sol + // while (runtime.seconds()<0.5){ + // rotation.setPower(0.5); + // } - if (!gamepad1.dpad_up && already_padup) { - already_y = false; - } - if (gamepad1.dpad_up && !already_padup) { - already_padup = true; - // POSITION CHASSE-NEIGE - rotation.setVelocity(600); - rotation.setTargetPosition(110); - - lmelevator.setVelocity(600); - lmelevator.setTargetPosition(555); - rmelevator.setVelocity(600); - rmelevator.setTargetPosition(555); - - rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - - // int pos = rotation.getCurrentPosition(); - // rotation.setVelocity(200); - // if (gamepad1.right_bumper) { - // // rotation.setTargetPosition(pos - 25); - // rotation.setTargetPosition(-100); // vertical si pos origine = 0 - // } else if (gamepad1.left_bumper) { - // // rotation.setTargetPosition(pos + 25); - // rotation.setTargetPosition(1000); // position basse - // } else { - // rotation.setTargetPosition(0); - // } - // rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - if (!gamepad1.dpad_down && already_paddown) { - already_a = false; - } - if (gamepad1.dpad_down && !already_paddown) { - already_a = true; - // POSITION BASSE - rotation.setVelocity(600); - rotation.setTargetPosition(800); - - lmelevator.setVelocity(600); - lmelevator.setTargetPosition(0); - rmelevator.setVelocity(600); - rmelevator.setTargetPosition(0); - - rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - } + telemetry.addData("Mode", "waiting for start"); - + // rotation.setVelocity(1700); + // rotation.setTargetPosition(-1000); + // rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - //telemetry.addData("x", x); - //telemetry.addData("y", y); - //telemetry.addData("mode", mode); - //telemetry.addData("lpow", lpower); - //telemetry.addData("rpow", rpower); - //telemetry.addData("ltrigg", t); - //telemetry.addData("t2", t2); - //telemetry.addData("rotation power", boxRot); - //telemetry.addData("Position elevateur l", lmelevator.getCurrentPosition()); - //telemetry.addData("Position elevateur r", rmelevator.getCurrentPosition()); - //telemetry.addData("Position rotation", rotation.getCurrentPosition()); - //telemetry.addData("Position box", box.getCurrentPosition()); - //telemetry.addData("box velocity", rotation.getVelocity()); - //telemetry.update(); - } + telemetry.addData("rotation target Pos", rotation.getTargetPosition()); + telemetry.addData("rotation Pos", rotation.getCurrentPosition()); + telemetry.update(); + + waitForStart(); + rotation.setVelocity(200); + rotation.setTargetPosition(0); + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + while (opModeIsActive()) { + + x = gamepad1.left_stick_x; + 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))); + + telemetry.addData("Status", "Running"); + + + if (gamepad1.right_bumper && gamepad1.left_bumper){ + avion.setDirection(Servo.Direction.REVERSE); + // telemetry.addData("servoPos",avion.getPosition()); + // avion.setPosition(0); + avion.setPosition(0.75); + // continue; + } + // Choix mode conduite / actif en manuel et auto + if (gamepad1.a && !already_a) { + nextMode(); + already_a = true; + } + if (!gamepad1.a && already_a) { + already_a = false; + } + double lpower = 0.0; + double rpower = 0.0; + double vmean; + double a; + double b; + + switch (mode) { + + case NORMAL: + double ysign = Math.signum(y); + double xsign = Math.signum(x); + lpower = ysign * t + (-xsign - 2 * x) * t; + rpower = ysign * t + (xsign - 2 * x) * t; + break; + + + case TANK: + lpower = -y; + rpower = -gamepad1.right_stick_y; + break; + + case ESSAIFRANCK: + // Code ci-dessous OK + // lpower = (1 + x); //(a / vmean) * t2; + // rpower = 2-lpower;//(b / vmean) * t2; + // lpower*=t2*((Math.signum(y)==0)?1:-Math.signum(y)); // sigNum(0) + // rpower*=t2*((Math.signum(y)==0)?1:-Math.signum(y)); + // Fin code OK + + int ltargetPos, rtargetPos; + ltargetPos = rtargetPos = 0; + + int lmpos = lm.getCurrentPosition(); + int rmpos = rm.getCurrentPosition(); + int step = 100; + // double rapp = step/theta; + double signe = Math.signum(-y); + signe = (signe == 0)?1.0:signe; + if(Math.abs(x)<=0.1 && Math.abs(y)<=0.1){ + ltargetPos = rtargetPos = 0; + }else if (Math.abs(x)<=0.1){ + ltargetPos = rtargetPos = 100; + }else if (isBetween(x,0.1,0.9)){ + rtargetPos = step; + ltargetPos = 2*step; + }else if (x>=0.9){ + ltargetPos = step; + rtargetPos = -step; + }else if (x<=-0.9){ + ltargetPos = -step; + rtargetPos = step; + }else if (isBetween(x, -0.9,-0.1)){ + rtargetPos = 2*step; + ltargetPos = step; + } + ltargetPos*=signe; + rtargetPos*=signe; + + // if (x>0.1){ + // theta = Math.abs(Math.PI/2 - Math.atan2(-y,x)); + // theta = (theta > Math.PI/2)?(Math.PI - theta):theta; + // ltargetPos = (int) (Math.floor(theta*37.0 + step)*signe); + // rtargetPos = (int) (Math.floor(step)*signe); + // } + // else if (x < -0.1){ + // theta = Math.abs(Math.abs(Math.atan2(-y,x)) - Math.PI/2); + // rtargetPos = (int) (Math.floor(theta*37.0 + step)*signe); + // ltargetPos = (int) (Math.floor(step)*signe); + // } + // else { + // rtargetPos = step*((int) Math.signum(-y)); + // ltargetPos = step*((int) Math.signum(-y)); + // } + + telemetry.addData("ltargetPos avant cut",ltargetPos); + telemetry.addData("rtargetPos avant cut",rtargetPos); + + // ltargetPos = ((Math.abs(ltargetPos)> step)?(int) Math.signum(ltargetPos)*(step + Math.abs(ltargetPos)%step ):ltargetPos); + // rtargetPos = ((Math.abs(rtargetPos)> step)?(int) Math.signum(rtargetPos)*(step + Math.abs(rtargetPos)%step):rtargetPos); + + lm.setTargetPosition(lmpos + ltargetPos); + rm.setTargetPosition(rmpos + rtargetPos); + lm.setVelocity(2800.0*t2); + rm.setVelocity(2800.0*t2); + + + // telemetry.addData("rapp",rapp); + telemetry.addData("ltargetPos",ltargetPos); + telemetry.addData("rtargetPos",rtargetPos); + + lm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + + + break; + + case ELINA: + a = (-y + x) / Math.pow(2, 1 / 2); + b = (-y - x) / Math.pow(2, 1 / 2); + vmean = (Math.abs(a) + Math.abs(b)) / 2; + lpower = (a / vmean) * t3; + rpower = (b / vmean) * t3; + break; + + } + + if (gamepad1.left_trigger > 0.1) { + lpower /= 3; + rpower /= 3; + } + + // CODE OK + // lm.setPower(lpower/1.5); + // rm.setPower(rpower/1.5); + // Fin code OK + + // activation moissonneuse -- actif en manuel et auto + 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; + } + + // activation elevateur + 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 = 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; + } + + if (gamepad1.ps && !already_ps) { + manualMode = !manualMode; + already_ps = true; + } else if (!gamepad1.ps && already_ps) { + already_ps = false; + } + + // activation rotation + if (manualMode) { + gamepad1.setLedColor(1.0, 0.0, 0.0,255); + // lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + rotation.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + box.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + telemetry.addData("mode","MANUEL"); + // Elevator manual mode + if (gamepad1.dpad_up) { + // rmelevator.setPower(0.3); + // lmelevator.setPower(0.3); + lmelevator.setVelocity(400); + rmelevator.setVelocity(400); + int lpos = rmelevator.getCurrentPosition(); + int rpos = lmelevator.getCurrentPosition(); + lmelevator.setTargetPosition(lpos + 15); + rmelevator.setTargetPosition(rpos + 15); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } else if (gamepad1.dpad_down) { + // lmelevator.setPower(-0.3); + // rmelevator.setPower(-0.3); + lmelevator.setVelocity(400); + rmelevator.setVelocity(400); + int lpos = rmelevator.getCurrentPosition(); + int rpos = lmelevator.getCurrentPosition(); + lmelevator.setTargetPosition(lpos - 15); + rmelevator.setTargetPosition(rpos - 15); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } else if (gamepad1.y) { + double power = -0.3; + if (gamepad1.right_bumper) { + power = -power; + } + rotation.setPower(power); + } else { + // lmelevator.setPower(0); + // rmelevator.setPower(0); + rotation.setPower(0); + // lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + // rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + } + // Box manual mode + if (gamepad1.dpad_left) { + box.setPower(0.3); + box.setPower(0.3); + } else if (gamepad1.dpad_right) { + box.setPower(-0.3); + box.setPower(-0.3); + } + else { + box.setPower(0); + box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + } + // Accrochage final + if (gamepad1.x){ + // lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + lmelevator.setVelocity(600); + rmelevator.setVelocity(600); + lmelevator.setTargetPosition(40); + rmelevator.setTargetPosition(40); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + } else { + gamepad1.setLedColor(0.0, 0.0, 0.0,10); + telemetry.addData("mode","AUTOMATIQUE"); + + if (!gamepad1.dpad_right && already_padright) { + already_padright = false; + } + if (gamepad1.dpad_right && !already_padright) { + already_padright = true; + // POSITION INITIALE + rotation.setVelocity(600); + rotation.setTargetPosition(0); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(0); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(0); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (!gamepad1.dpad_left && already_padleft) { + already_padleft = false; + } + if (gamepad1.dpad_left && !already_padleft) { + already_padleft = true; + // POSITION ROULAGE / Rammase Pixel dans boite + rotation.setVelocity(600); + rotation.setTargetPosition(-50); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(150); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(150); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + + if (!gamepad1.dpad_up && already_padup) { + already_y = false; + } + if (gamepad1.dpad_up && !already_padup) { + already_padup = true; + // POSITION CHASSE-NEIGE + rotation.setVelocity(600); + rotation.setTargetPosition(110); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(555); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(555); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + + // int pos = rotation.getCurrentPosition(); + // rotation.setVelocity(200); + // if (gamepad1.right_bumper) { + // // rotation.setTargetPosition(pos - 25); + // rotation.setTargetPosition(-100); // vertical si pos origine = 0 + // } else if (gamepad1.left_bumper) { + // // rotation.setTargetPosition(pos + 25); + // rotation.setTargetPosition(1000); // position basse + // } else { + // rotation.setTargetPosition(0); + // } + // rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (!gamepad1.dpad_down && already_paddown) { + already_a = false; + } + if (gamepad1.dpad_down && !already_paddown) { + already_a = true; + // POSITION BASSE + rotation.setVelocity(600); + rotation.setTargetPosition(800); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(0); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(0); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + } + + + + //telemetry.addData("x", x); + //telemetry.addData("y", y); + //telemetry.addData("mode", mode); + //telemetry.addData("lpow", lpower); + //telemetry.addData("rpow", rpower); + //telemetry.addData("ltrigg", t); + //telemetry.addData("t2", t2); + //telemetry.addData("rotation power", boxRot); + //telemetry.addData("Position elevateur l", lmelevator.getCurrentPosition()); + //telemetry.addData("Position elevateur r", rmelevator.getCurrentPosition()); + //telemetry.addData("Position rotation", rotation.getCurrentPosition()); + //telemetry.addData("Position box", box.getCurrentPosition()); + //telemetry.addData("box velocity", rotation.getVelocity()); + //telemetry.update(); + } } -} \ No newline at end of file +} diff --git a/ftc_new.java~ b/ftc_new.java~ new file mode 100644 index 0000000..25b0002 --- /dev/null +++ b/ftc_new.java~ @@ -0,0 +1,563 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import com.qualcomm.robotcore.robot.Robot; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.robot.Robot; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; + +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.Velocity; + +@TeleOp(name = "WeRobot: FTC2024 NEW! Carlike", group = "WeRobot") +public class WEROBOT_FTC2024_New_carlike extends LinearOpMode { + public enum RobotMode { + ESSAIFRANCK, ELINA, NORMAL, TANK; + } + + private DcMotorEx rm; + private DcMotorEx lm; + private DcMotor moissoneuse; + private DcMotorEx lmelevator; + private DcMotorEx rmelevator; + private DcMotorEx box; + private DcMotorEx rotation; + private Servo avion; + private ElapsedTime runtime = new ElapsedTime(); + private RobotMode mode = RobotMode.ESSAIFRANCK; + + /* + * 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); + } + + public void nextMode() { + RobotMode toNextMode; + switch (this.mode) { + case ESSAIFRANCK: + toNextMode = RobotMode.ELINA; + break; + case ELINA: + toNextMode = RobotMode.NORMAL; + break; + case NORMAL: + toNextMode = RobotMode.TANK; + break; + default: + toNextMode = RobotMode.ESSAIFRANCK; + break; + } + this.mode = toNextMode; + } + public boolean isBetween(double elem, double mini, double maxi){ + return Math.abs(elem - (((maxi-mini)/2)+mini))<=(maxi-mini)/2; + } + // La fonction du thread principal + @Override + public void runOpMode() throws InterruptedException { + + double boxRot = 0; + int signeBR; + + float x; + double y; + + double t; + double t2; + double t3; + + boolean already_b = false; + boolean already_a = false; + boolean already_x = false; + boolean already_y = false; + boolean already_up = false; + boolean already_down = false; + boolean already_ps = false; + + boolean already_paddown = false; + boolean already_padup = false; + boolean already_padright = false; + boolean already_padleft = false; + + + boolean sinking = false; + boolean manualMode = false; + boolean firstLaunch = true; + + telemetry.addData("Status", "Initialized"); + telemetry.addData("Version", "5.123"); + + lm = hardwareMap.get(DcMotorEx.class, "blm"); + + rm = hardwareMap.get(DcMotorEx.class, "brm"); + rm.setDirection(DcMotor.Direction.REVERSE); + + lm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + lm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse"); + moissoneuse.setDirection(DcMotor.Direction.REVERSE); + + lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv"); + lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv"); + rmelevator.setDirection(DcMotor.Direction.REVERSE); + rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + rotation = hardwareMap.get(DcMotorEx.class, "elvRot"); + rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + box = hardwareMap.get(DcMotorEx.class, "boxRot"); + box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + box.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + avion = hardwareMap.get(Servo.class, "avion"); + // box.setPositionPIDFCoefficients(5.0); + + // rotation positions: 20° pos initiale par rapport au sol + // while (runtime.seconds()<0.5){ + // rotation.setPower(0.5); + // } + + telemetry.addData("Mode", "waiting for start"); + + // rotation.setVelocity(1700); + // rotation.setTargetPosition(-1000); + // rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + telemetry.addData("rotation target Pos", rotation.getTargetPosition()); + telemetry.addData("rotation Pos", rotation.getCurrentPosition()); + telemetry.update(); + + waitForStart(); + rotation.setVelocity(200); + rotation.setTargetPosition(0); + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + while (opModeIsActive()) { + + x = gamepad1.left_stick_x; + 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))); + + telemetry.addData("Status", "Running"); + + + if (gamepad1.right_bumper && gamepad1.left_bumper){ + avion.setDirection(Servo.Direction.REVERSE); + // telemetry.addData("servoPos",avion.getPosition()); + // avion.setPosition(0); + avion.setPosition(0.75); + // continue; + } + // Choix mode conduite / actif en manuel et auto + if (gamepad1.a && !already_a) { + nextMode(); + already_a = true; + } + if (!gamepad1.a && already_a) { + already_a = false; + } + double lpower = 0.0; + double rpower = 0.0; + double vmean; + double a; + double b; + + switch (mode) { + + case NORMAL: + double ysign = Math.signum(y); + double xsign = Math.signum(x); + lpower = ysign * t + (-xsign - 2 * x) * t; + rpower = ysign * t + (xsign - 2 * x) * t; + break; + + + case TANK: + lpower = -y; + rpower = -gamepad1.right_stick_y; + break; + + case ESSAIFRANCK: + // Code ci-dessous OK + // lpower = (1 + x); //(a / vmean) * t2; + // rpower = 2-lpower;//(b / vmean) * t2; + // lpower*=t2*((Math.signum(y)==0)?1:-Math.signum(y)); // sigNum(0) + // rpower*=t2*((Math.signum(y)==0)?1:-Math.signum(y)); + // Fin code OK + + int ltargetPos, rtargetPos; + ltargetPos = rtargetPos = 0; + + int lmpos = lm.getCurrentPosition(); + int rmpos = rm.getCurrentPosition(); + int step = 100; + // double rapp = step/theta; + double signe = Math.signum(-y); + signe = (signe == 0)?1.0:signe; + if(Math.abs(x)<=0.1 && Math.abs(y)<=0.1){ + ltargetPos = rtargetPos = 0; + }else if (Math.abs(x)<=0.1){ + ltargetPos = rtargetPos = 100; + }else if (isBetween(x,0.1,0.9)){ + rtargetPos = step; + ltargetPos = 2*step; + }else if (x>=0.9){ + ltargetPos = step; + rtargetPos = -step; + }else if (x<=-0.9){ + ltargetPos = -step; + rtargetPos = step; + }else if (isBetween(x, -0.9,-0.1)){ + rtargetPos = 2*step; + ltargetPos = step; + } + ltargetPos*=signe; + rtargetPos*=signe; + + // if (x>0.1){ + // theta = Math.abs(Math.PI/2 - Math.atan2(-y,x)); + // theta = (theta > Math.PI/2)?(Math.PI - theta):theta; + // ltargetPos = (int) (Math.floor(theta*37.0 + step)*signe); + // rtargetPos = (int) (Math.floor(step)*signe); + // } + // else if (x < -0.1){ + // theta = Math.abs(Math.abs(Math.atan2(-y,x)) - Math.PI/2); + // rtargetPos = (int) (Math.floor(theta*37.0 + step)*signe); + // ltargetPos = (int) (Math.floor(step)*signe); + // } + // else { + // rtargetPos = step*((int) Math.signum(-y)); + // ltargetPos = step*((int) Math.signum(-y)); + // } + + telemetry.addData("ltargetPos avant cut",ltargetPos); + telemetry.addData("rtargetPos avant cut",rtargetPos); + + // ltargetPos = ((Math.abs(ltargetPos)> step)?(int) Math.signum(ltargetPos)*(step + Math.abs(ltargetPos)%step ):ltargetPos); + // rtargetPos = ((Math.abs(rtargetPos)> step)?(int) Math.signum(rtargetPos)*(step + Math.abs(rtargetPos)%step):rtargetPos); + + lm.setTargetPosition(lmpos + ltargetPos); + rm.setTargetPosition(rmpos + rtargetPos); + lm.setVelocity(2800.0*t2); + rm.setVelocity(2800.0*t2); + + + // telemetry.addData("rapp",rapp); + telemetry.addData("ltargetPos",ltargetPos); + telemetry.addData("rtargetPos",rtargetPos); + + lm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + + + break; + + case ELINA: + a = (-y + x) / Math.pow(2, 1 / 2); + b = (-y - x) / Math.pow(2, 1 / 2); + vmean = (Math.abs(a) + Math.abs(b)) / 2; + lpower = (a / vmean) * t3; + rpower = (b / vmean) * t3; + break; + + } + + if (gamepad1.left_trigger > 0.1) { + lpower /= 3; + rpower /= 3; + } + + // CODE OK + // lm.setPower(lpower/1.5); + // rm.setPower(rpower/1.5); + // Fin code OK + + // activation moissonneuse -- actif en manuel et auto + 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; + } + + // activation elevateur + 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 = 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; + } + + if (gamepad1.ps && !already_ps) { + manualMode = !manualMode; + already_ps = true; + } else if (!gamepad1.ps && already_ps) { + already_ps = false; + } + + // activation rotation + if (manualMode) { + gamepad1.setLedColor(1.0, 0.0, 0.0,255); + // lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + rotation.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + box.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + telemetry.addData("mode","MANUEL"); + // Elevator manual mode + if (gamepad1.dpad_up) { + // rmelevator.setPower(0.3); + // lmelevator.setPower(0.3); + lmelevator.setVelocity(400); + rmelevator.setVelocity(400); + int lpos = rmelevator.getCurrentPosition(); + int rpos = lmelevator.getCurrentPosition(); + lmelevator.setTargetPosition(lpos + 15); + rmelevator.setTargetPosition(rpos + 15); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } else if (gamepad1.dpad_down) { + // lmelevator.setPower(-0.3); + // rmelevator.setPower(-0.3); + lmelevator.setVelocity(400); + rmelevator.setVelocity(400); + int lpos = rmelevator.getCurrentPosition(); + int rpos = lmelevator.getCurrentPosition(); + lmelevator.setTargetPosition(lpos - 15); + rmelevator.setTargetPosition(rpos - 15); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } else if (gamepad1.y) { + double power = -0.3; + if (gamepad1.right_bumper) { + power = -power; + } + rotation.setPower(power); + } else { + // lmelevator.setPower(0); + // rmelevator.setPower(0); + rotation.setPower(0); + // lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + // rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + } + // Box manual mode + if (gamepad1.dpad_left) { + box.setPower(0.3); + box.setPower(0.3); + } else if (gamepad1.dpad_right) { + box.setPower(-0.3); + box.setPower(-0.3); + } + else { + box.setPower(0); + box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + } + // Accrochage final + if (gamepad1.x){ + // lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + lmelevator.setVelocity(600); + rmelevator.setVelocity(600); + lmelevator.setTargetPosition(40); + rmelevator.setTargetPosition(40); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + } else { + gamepad1.setLedColor(0.0, 0.0, 0.0,10); + telemetry.addData("mode","AUTOMATIQUE"); + + if (!gamepad1.dpad_right && already_padright) { + already_padright = false; + } + if (gamepad1.dpad_right && !already_padright) { + already_padright = true; + // POSITION INITIALE + rotation.setVelocity(600); + rotation.setTargetPosition(0); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(0); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(0); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (!gamepad1.dpad_left && already_padleft) { + already_padleft = false; + } + if (gamepad1.dpad_left && !already_padleft) { + already_padleft = true; + // POSITION ROULAGE / Rammase Pixel dans boite + rotation.setVelocity(600); + rotation.setTargetPosition(-50); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(150); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(150); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + + if (!gamepad1.dpad_up && already_padup) { + already_y = false; + } + if (gamepad1.dpad_up && !already_padup) { + already_padup = true; + // POSITION CHASSE-NEIGE + rotation.setVelocity(600); + rotation.setTargetPosition(110); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(555); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(555); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + + // int pos = rotation.getCurrentPosition(); + // rotation.setVelocity(200); + // if (gamepad1.right_bumper) { + // // rotation.setTargetPosition(pos - 25); + // rotation.setTargetPosition(-100); // vertical si pos origine = 0 + // } else if (gamepad1.left_bumper) { + // // rotation.setTargetPosition(pos + 25); + // rotation.setTargetPosition(1000); // position basse + // } else { + // rotation.setTargetPosition(0); + // } + // rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (!gamepad1.dpad_down && already_paddown) { + already_a = false; + } + if (gamepad1.dpad_down && !already_paddown) { + already_a = true; + // POSITION BASSE + rotation.setVelocity(600); + rotation.setTargetPosition(800); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(0); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(0); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + } + + + + //telemetry.addData("x", x); + //telemetry.addData("y", y); + //telemetry.addData("mode", mode); + //telemetry.addData("lpow", lpower); + //telemetry.addData("rpow", rpower); + //telemetry.addData("ltrigg", t); + //telemetry.addData("t2", t2); + //telemetry.addData("rotation power", boxRot); + //telemetry.addData("Position elevateur l", lmelevator.getCurrentPosition()); + //telemetry.addData("Position elevateur r", rmelevator.getCurrentPosition()); + //telemetry.addData("Position rotation", rotation.getCurrentPosition()); + //telemetry.addData("Position box", box.getCurrentPosition()); + //telemetry.addData("box velocity", rotation.getVelocity()); + //telemetry.update(); + } + } + +} \ No newline at end of file