From 94118433a0ec0d0e1cbb7d0d8dd8c08b3d63f471 Mon Sep 17 00:00:00 2001
From: Zelina974 <elina.claerebout@gmail.com>
Date: Mon, 26 Feb 2024 14:10:42 +0100
Subject: [PATCH] change

---
 ftc2024-autonome.java | 132 ++++++++++++++++++++++++------------------
 test.java             |  68 ++++++++++++++++++++++
 2 files changed, 145 insertions(+), 55 deletions(-)

diff --git a/ftc2024-autonome.java b/ftc2024-autonome.java
index ebc6510..9366d1a 100644
--- a/ftc2024-autonome.java
+++ b/ftc2024-autonome.java
@@ -17,7 +17,6 @@ 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;
@@ -63,7 +62,7 @@ public class ftc2024_autonome extends LinearOpMode {
         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;
+        boolean mode = true;
         
         YawPitchRollAngles robotOrientation;
         robotOrientation = imu.getRobotYawPitchRollAngles();
@@ -72,7 +71,7 @@ public class ftc2024_autonome extends LinearOpMode {
         double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES);
         double Roll  = robotOrientation.getRoll(AngleUnit.DEGREES);
 
-		double yaw_sortie;
+        double yaw_sortie;
         
         // Wait for the game to start (driver presses PLAY)
         waitForStart();
@@ -80,69 +79,92 @@ public class ftc2024_autonome extends LinearOpMode {
         runtime.reset();
         if (mode){
             //mode Elina
-            while (opModeIsActive() && Yaw <= 90.0) {
-                lm.setPower(0.5);
-                rm.setPower(-0.5);
-				robotOrientation = imu.getRobotYawPitchRollAngles();
+            /*while (opModeIsActive() && Yaw <= -90.0) {
+                lm.setPower(0.2);
+                rm.setPower(-0.2);
+                robotOrientation = imu.getRobotYawPitchRollAngles();
                 Yaw   = robotOrientation.getYaw(AngleUnit.DEGREES);
                 telemetry.addData("Yaw : ", Yaw);
                 telemetry.update();
-				yaw_sortie = Yaw;
+                
+            }*/
+            while (opModeIsActive() && Yaw < 90) {
+                lm.setPower(0.2);
+                rm.setPower(-0.2);
+                robotOrientation = imu.getRobotYawPitchRollAngles();
+                Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                telemetry.addData("Leg 1", runtime.seconds());
+                telemetry.addData("Yaw", Yaw);
+                telemetry.update();
             }
-			telemetry.addData("yaw_sortie", yaw_sortie);
             runtime.reset();
-            while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) {
+            yaw_sortie = Yaw;
+            
+            telemetry.update();
+            
+            while (opModeIsActive()) {
+                lm.setPower(0.1);
+                rm.setPower(0.1);
+                robotOrientation = imu.getRobotYawPitchRollAngles();
+                Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                telemetry.addData("yaw_sortie", yaw_sortie);
+                telemetry.addData("Yaw", Yaw);
+                telemetry.update();
+                
+            }
+            /*while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) {
                 lm.setPower(0.1);
                 rm.setPower(0.1);
                 telemetry.addData("Leg 2", runtime.seconds());
                 telemetry.update();
-            }
+            }*/
         }
         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}
-            };
-            //mode Aurelien
-        for(int i = 0; i<operations.length; i++){
-            double[] vec = operations[i];
-            double x = vec[0];
-            double y = vec[1];
-            double total_dist = (double) Math.sqrt(Math.pow(y,2)+Math.pow(x,2));
-            double time = time_for_dist(speed, total_dist);
-            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;
-            double lmvalue = (a/vmean);
-            double rmvalue = (b/vmean);
-            runtime.reset();
-                
-            while (opModeIsActive() && (runtime.seconds() <= time)) {
-                lm.setPower(lmvalue);
-                rm.setPower(rmvalue);
-                telemetry.addData("Runtime Seconds", runtime.seconds());
-                telemetry.addData("current_operation",operations[i]);
-                telemetry.addData("current_op_id",i);
 
-                telemetry.update();
+            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}
+                };
+                //mode Aurelien
+                for(int i = 0; i<operations.length; i++){
+                    double[] vec = operations[i];
+                    double x = vec[0];
+                    double y = vec[1];
+                    double total_dist = (double) Math.sqrt(Math.pow(y,2)+Math.pow(x,2));
+                    double time = time_for_dist(speed, total_dist);
+                    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;
+                    double lmvalue = (a/vmean);
+                    double rmvalue = (b/vmean);
+                    runtime.reset();
+                    while (opModeIsActive() && (runtime.seconds() <= time)) {
+                        lm.setPower(lmvalue);
+                        rm.setPower(rmvalue);
+                        telemetry.addData("Runtime Seconds", runtime.seconds());
+                        telemetry.addData("current_operation",operations[i]);
+                        telemetry.addData("current_op_id",i);
+
+                    
+                        telemetry.update();
+                    }
                 }
             }
         }
@@ -154,9 +176,9 @@ public class ftc2024_autonome extends LinearOpMode {
 
         telemetry.update();
         // run until the end of the match (driver presses STOP
-
     }
 
-    }
+}
+
 }
 
diff --git a/test.java b/test.java
index 8b13789..22d1a8b 100644
--- a/test.java
+++ b/test.java
@@ -1 +1,69 @@
+package org.firstinspires.ftc.teamcode;
 
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+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.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+@Autonomous
+
+public class ftc2024_autonome_test extends LinearOpMode {
+    private DcMotor rm;
+    private DcMotor lm;
+    private IMU imu;
+
+    @Override
+
+    public void runOpMode() {
+        lm = hardwareMap.get (DcMotor.class, "lm");
+        rm = hardwareMap.get (DcMotor.class, "rm");
+
+        rm.setDirection(DcMotorSimple.Direction.REVERSE);
+        
+        imu = hardwareMap.get(IMU.class, "imu");
+        imu.initialize(
+            new IMU.Parameters(
+                new RevHubOrientationOnRobot(
+                    RevHubOrientationOnRobot.LogoFacingDirection.UP,
+                    RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
+                )
+            )
+            );
+        imu.resetYaw();
+
+        YawPitchRollAngles robotOrientation;
+        double Yaw   = robotOrientation.getYaw(AngleUnit.DEGREES);
+        double yaw_sortie1;
+        double yaw_sortie2;
+        double yaw_sortie3;
+        double yaw_sortie4;
+
+        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++){
+                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();
+                }
+            }
+        }
+    }
+}
\ No newline at end of file