Diff: steering_methods.h

Revision:

22:08d30ea47111

Parent:

20:7dcdadbd7bc0

Child:

23:4c743746533c

--- a/steering_methods.h	Fri Oct 29 18:42:13 2021 +0000
+++ b/steering_methods.h	Wed Nov 03 16:13:02 2021 +0000
@@ -71,6 +71,8 @@
     // Global declearation
 float err_prev = 0;
 float prev_duty = 0;
+float errorArea;
+float errorAreaPrev =0.0;
 void steering_control(void)
 {
     
@@ -78,7 +80,7 @@
     if(steering_enabled == true ){
         //Read each sensor and calculate the sensor difference
         float sensor_difference = left_distance_sensor.read() -
-                                  right_distance_sensor.read()*1.4;
+                                  right_distance_sensor.read();
                                   
         //calculate the feedback term for the controller. distance (meters) from
         //track to sensor
@@ -87,29 +89,31 @@
         //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)
+        errorArea= TI_STEERING*err + errorAreaPrev; 
+               
+            
         //Calculate the derivative of the error term for the controller.
         //  e'(t) = (error(t) - error(t-1))/dt
-        float errChange = (err - err_prev)/(0.02);
+        float errChange = (err - err_prev)/(TI_STEERING);
         
         //Calculate the controller output
         //Angle in radians
-        float servo_angle = (0.09+KP*err + KD*errChange*TIME_DERIVATIVE)/7;
+        float servo_angle = (0.04+KP*err + KD*errChange*TIME_DERIVATIVE + KI_STEERING*errorArea*TIME_INTEGRAL);
         //Calculate the duty cycle for the servo motor
         current_duty_cycle = rad2dc(servo_angle);
-        if( abs(current_duty_cycle - prev_duty) >= 0.0000093){ // this is a filter
-            servo_motor_pwm.write(current_duty_cycle);
-            };
+        servo_motor_pwm.write(current_duty_cycle);
+        
       }
       else {
             current_duty_cycle = rad2dc(M_PI/4);
-    
             servo_motor_pwm.write(current_duty_cycle);
           };
     //Save the error for the next calculation.
     prev_duty = current_duty_cycle;
     err_prev = err;
-    
+    errorAreaPrev = errorArea;
 }