Diff: steering_methods.h

Revision:

26:54ce9f642477

Parent:

25:8bd029d58251

Child:

27:0fa9d61c5fc6

--- a/steering_methods.h	Sat Nov 06 16:31:02 2021 +0000
+++ b/steering_methods.h	Mon Nov 08 21:08:41 2021 +0000
@@ -90,39 +90,21 @@
     
     if(steering_enabled == true ){
         //Read each sensor and calculate the sensor difference
-        float sensor_difference = left_distance_sensor.read() -
+        
+
+    float sensor_difference = left_distance_sensor.read() -
                                   right_distance_sensor.read();
-                                left_sens =   left_distance_sensor.read();
-                                right_sens = right_distance_sensor.read()*1.065;
+                               
         //calculate the feedback term for the controller. distance (meters) from
         //track to sensor
         feedback = (sensor_difference/SEN_DIFF_TO_DIST); //
-       if(abs(feedback) < 0.035 && counter ==0)
-               {
-                    KP = 1.3;
-                    KD = 0.0856;
-                }
-        else if (abs(feedback) > 0.035 && counter ==0)
-            {
-                 KP = 2;
-               
-                }
-        else if (counter == 0 && lap ==2 )
-        {
-            KP=3.3; 
-            KD=0.35;
-        }
-        else 
-           {
-               KP = 5.5;
-               KD = 0.6;
-               }
           
         //calculate the error term for the controller. distance (meters) from
         //desired position to sensor
          err = REF - feedback; //
 
        //Area of the error: TimeStep*err (width*length of a rectangle)
+       
        if(clamp == false)
         errorArea= TI_STEERING*err + errorAreaPrev; 
           
@@ -136,8 +118,7 @@
         //Calculate the duty cycle for the servo motor
         current_duty_cycle = rad2dc(servo_angle);
         
-          
-             //--- integral anti-windup ---
+               //--- integral anti-windup ---
         if (servo_angle > 0.1 || servo_angle < 0.05){
             if (errorArea > 0.0){
                 clamp = true;
@@ -145,7 +126,8 @@
          } else {
              clamp = false;
             }
-     
+            
+        
         servo_motor_pwm.write(current_duty_cycle);
         
       }
@@ -154,10 +136,7 @@
       
             servo_motor_pwm.write(current_duty_cycle);
           };
-    //Save the error for the next calculation.
-    prev_duty = current_duty_cycle;
-    err_prev = err;
-    errorAreaPrev = errorArea;
+ 
 }