From 864306c9bf8df67aecb36a2df906e736525093dc Mon Sep 17 00:00:00 2001
From: GZod01 <gzod01@gzod01.fr>
Date: Fri, 8 Mar 2024 14:29:30 +0100
Subject: [PATCH] helloworld

---
 ftc2024-autonome.java | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/ftc2024-autonome.java b/ftc2024-autonome.java
index 197075d..2d7e641 100644
--- a/ftc2024-autonome.java
+++ b/ftc2024-autonome.java
@@ -44,22 +44,22 @@ public class ftc2024_autonome extends LinearOpMode {
      */
     public double runPid(double angle2go){
         robotOrientation = imu.getRobotYawPitchRollAngles();
-	angle2go = (angle2go*180)/Math.PI
-	double yaw = robotOrientation.getYaw(AngleUnit.RADIANS);
-	double lastPos;
-	// command -1.0<command<1.0
-	double command = (angle2go-yaw)/2*Math.PI;
+	angle2go = (angle2go*180)/Math.PI;
+	double P = 0.5;
 	while (opModeIsActive()){
 	    yaw = robotOrientation.getYaw(AngleUnit.RADIANS);
 	    double currentPos = yaw;
-	    double error = currentPos-lastPos;
-	    lastPos = currentPos;
-	    command += P * error;
+	    double error = yaw-currentpos;
+	    double command = (P * error)/Math.PI;
+	    command = clamp(command,.6); 
 	    lm.setPower(command);
 	    rm.setPower(-command);
 	}
 
     }
+    double clamp(double value, double max) {
+        return Math.max(-max, Math.min(max, value));
+    }
 
     @Override
     public void runOpMode() {