diff --git a/ftc2024-autonome.java b/ftc2024-autonome.java index faa7561..da0e464 100644 --- a/ftc2024-autonome.java +++ b/ftc2024-autonome.java @@ -17,82 +17,82 @@ import com.qualcomm.robotcore.util.ElapsedTime; @Autonomous public class ftc2024_autonome extends LinearOpMode { - private DcMotor rm; - private DcMotor lm; - private ElapsedTime runtime = new ElapsedTime(); - + private DcMotor rm; + private DcMotor lm; + private ElapsedTime runtime = new ElapsedTime(); + - public double time_for_dist(double speed, double dist){ - return (double) (dist/speed); - } + public double time_for_dist(double speed, double dist){ + return (double) (dist/speed); + } - @Override - public void runOpMode() { - lm = hardwareMap.get(DcMotor.class, "blm"); - rm = hardwareMap.get(DcMotor.class, "brm"); - rm.setDirection(DcMotorSimple.Direction.REVERSE); - telemetry.addData("Status", "Initialized"); - telemetry.update(); - double tour_par_minute = 300.0; - double wheel_width = 9.0e-2; - double wheel_rayon = (wheel_width)/2; - double wheel_perimeter = wheel_rayon*2*Math.PI; - double speed = (tour_par_minute/60)*wheel_perimeter;//dist per second - boolean mode = false; - // Wait for the game to start (driver presses PLAY) - waitForStart(); + @Override + public void runOpMode() { + lm = hardwareMap.get(DcMotor.class, "blm"); + rm = hardwareMap.get(DcMotor.class, "brm"); + rm.setDirection(DcMotorSimple.Direction.REVERSE); + telemetry.addData("Status", "Initialized"); + telemetry.update(); + double tour_par_minute = 300.0; + double wheel_width = 9.0e-2; + double wheel_rayon = (wheel_width)/2; + double wheel_perimeter = wheel_rayon*2*Math.PI; + double speed = (tour_par_minute/60)*wheel_perimeter;//dist per second + boolean mode = false; + // Wait for the game to start (driver presses PLAY) + waitForStart(); - runtime.reset(); - if (mode){ - //mode Elina - while (opModeIsActive() && (runtime.seconds() <= 41e-2*Math.PI/4/speed)) { - lm.setPower(1); - rm.setPower(-1); - telemetry.addData("Leg 1", runtime.seconds()); - telemetry.update(); - } - runtime.reset(); - while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) { - lm.setPower(1); - rm.setPower(1); - telemetry.addData("Leg 2", runtime.seconds()); - telemetry.update(); - } - } - else { - double[][] operations = { - {-1.0,1.0}, // vectors - {1.0,1.0}, - {-1.0,1.0}, - {-1.0,-1.0}, - {1.0,-1.0} - }; - //mode Aurelien - for(int i = 0; i0?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); + //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.getPowerFloat == (float) 1){ - moissoneuse.setPower(0); - }else{ - moissoneuse.setPower(1); - } - } - if(!gamepad1.b && already_b){ - already_b = !already_b; - } - if(gamepad1.a && !already_a){ - already_a = !already_a; - if(moissoneuse.getPowerFloat == (float) -1){ - moissoneuse.setPower(0); - }else{ - moissoneuse.setPower(-1); - } - if(!gamepad1.a && already_a){ - already_a = !already_a; - } - } + 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.a && !already_a){ + already_a = !already_a; + if(moissoneuse.getPower() == -1.0){ + moissoneuse.setPower(0); + }else{ + moissoneuse.setPower(-1); + } + if(!gamepad1.a && already_a){ + already_a = !already_a; + } + } - 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(); + 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); + // 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(); + } + } }