From 6eafb98253528db14ae69584771eb9cabc262353 Mon Sep 17 00:00:00 2001
From: Zelina974 <elina.claerebout@gmail.com>
Date: Mon, 26 Feb 2024 15:28:02 +0100
Subject: [PATCH] update

---
 test.java | 85 +++++++++++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 80 insertions(+), 5 deletions(-)

diff --git a/test.java b/test.java
index 22d1a8b..6939d97 100644
--- a/test.java
+++ b/test.java
@@ -45,25 +45,100 @@ public class ftc2024_autonome_test extends LinearOpMode {
 
         YawPitchRollAngles robotOrientation;
         double Yaw   = robotOrientation.getYaw(AngleUnit.DEGREES);
-        double yaw_sortie1;
-        double yaw_sortie2;
-        double yaw_sortie3;
-        double yaw_sortie4;
+        double yaw_sortie;
+        double a;
+        double a1;
+        double b;
+        double b1;
+        double k;
+        double k1;
 
         waitForStart();
 
         while (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};
-            for(int i = 0; i< p_t_g.length; i++){
+            for(int i = 0; i< lm_p.length; i++){
                 while (opModeIsActive() && Yaw < 90){
                     lm.setPower = lm_p[i];
                     rm.setPower = rm_p[i];
                     Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
                     telemetry.addData("Yaw : ", Yaw);
                     telemetry.update();
+                    yaw_sortie = Yaw;
                 }
+                telemetry.addData("Yaw sortie", yaw_sortie);
+                telemetry.update();
+                Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                double [] x = Yaw - yaw_sortie;
+                              
+                
+                
+                /*if (i = 0) {
+                    Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                    a = yaw_sortie;
+                    a1 = Yaw;
+                }
+                if (i = 1){
+                    Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                    b = yaw_sortie;
+                    b1 = Yaw;
+                }
+                if (i = 2){
+                    Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                    double c = yaw_sortie;
+                    double c1 = Yaw;
+                }
+                if (i = 3){
+                    Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                    double d = yaw_sortie;
+                    double d1 = Yaw;
+                }
+                if (i = 4){
+                    Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                    double e = yaw_sortie;
+                    double e1 = Yaw;
+                }
+                if (i = 5){
+                    Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                    double f = yaw_sortie;
+                    double f1 = Yaw;
+                }
+                if (i = 6){
+                    Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                    double g = yaw_sortie;
+                    double g1 = Yaw;
+                }
+                if (i = 7){
+                    Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                    double h = yaw_sortie;
+                    double h1 = Yaw;
+                }
+                if (i = 8){
+                    Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                    double j = yaw_sortie;
+                    double j1 = Yaw;
+                }
+                if (i = 9){
+                    Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                    k = yaw_sortie;
+                    k1 = Yaw;
+                }
+                */
+                imu.resetYaw();
             }
+            
+
+            telemetry.addData("0.1", x[0]);
+            telemetry.addData("0.2", x[1]);
+            telemetry.addData("0.3", x[2]);
+            telemetry.addData("0.4", x[3]);
+            telemetry.addData("0.5", x[4]);
+            telemetry.addData("0.6", x[5]);
+            telemetry.addData("0.7", x[6]);
+            telemetry.addData("0.8", x[7]);
+            telemetry.addData("0.9", x[8]);
+            telemetry.addData("1", x[9]);
         }
     }
 }
\ No newline at end of file