diff --git a/ftc2024-autonome.java b/ftc2024-autonome.java index ad0ac98..6dda8d5 100644 --- a/ftc2024-autonome.java +++ b/ftc2024-autonome.java @@ -17,6 +17,7 @@ import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; 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; @@ -88,7 +89,7 @@ public class ftc2024_autonome extends LinearOpMode { telemetry.update(); yaw_sortie = Yaw; } - telemetry.addData("yaw_sortie", yaw_sortie) + telemetry.addData("yaw_sortie", yaw_sortie); runtime.reset(); while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) { lm.setPower(0.1); @@ -98,50 +99,50 @@ public class ftc2024_autonome extends LinearOpMode { } } else{ - while(opModeIsActive()){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - if(Math.abs(Yaw-90.0)<=0.01){ - break; - } - else if((Yaw - 90.0) <0){ - lm.setPower((Math.abs(Yaw-90.0)/90)*0.5); - rm.setPower(-(Math.abs(Yaw-90.0)/90)*0.5); - } - else{ - rm.setPower((Math.abs(Yaw-90.0)/90)*0.5); - lm.setPower(-(Math.abs(Yaw-90.0)/90)*0.5); - } - + while(opModeIsActive()){ + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + if(Math.abs(Yaw-90.0)<=0.01){ + break; + } + else if((Yaw - 90.0) <0){ + lm.setPower((Math.abs(Yaw-90.0)/90)*0.5); + rm.setPower(-(Math.abs(Yaw-90.0)/90)*0.5); + } + else{ + rm.setPower((Math.abs(Yaw-90.0)/90)*0.5); + lm.setPower(-(Math.abs(Yaw-90.0)/90)*0.5); + } + } if(false){ - double[][] operations = { - {-1.0,1.0}, // vectors - {1.0,1.0}, - {-1.0,1.0}, - {-1.0,-1.0}, - {1.0,-1.0} + 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; i