diff --git a/ftc2024-autonome.java b/ftc2024-autonome.java index 726c582..ad0ac98 100644 --- a/ftc2024-autonome.java +++ b/ftc2024-autonome.java @@ -97,7 +97,22 @@ public class ftc2024_autonome extends LinearOpMode { telemetry.update(); } } - else { + 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); + } + + if(false){ double[][] operations = { {-1.0,1.0}, // vectors {1.0,1.0},