Diff: steering_methods.h

Revision:

28:1c2eb25d624e

Parent:

27:0fa9d61c5fc6

Child:

31:d570f957e083

--- a/steering_methods.h	Mon Nov 15 21:18:37 2021 +0000
+++ b/steering_methods.h	Wed Nov 17 21:37:29 2021 +0000
@@ -84,6 +84,7 @@
 float err;
 float left_sens; float right_sens;
 volatile bool clamp;
+float OFFSET_ANGLE=0.0; float KICK_ERR; float PREV_KICK_ERR; float KICK_CHNG_ERR;
 void steering_control(void)
 {
     
@@ -94,7 +95,13 @@
 
     float sensor_difference = left_distance_sensor.read() -
                                   right_distance_sensor.read();
-                               
+    
+       /*if (counter == 1 && lap == 3){
+        KP = 6; 
+        KD = 1;
+        KI_STEERINg = 0.8;
+        
+        }*/
         //calculate the feedback term for the controller. distance (meters) from
         //track to sensor
         feedback = (sensor_difference/SEN_DIFF_TO_DIST); //
@@ -102,7 +109,7 @@
         //calculate the error term for the controller. distance (meters) from
         //desired position to sensor
          err = REF - feedback; //
-
+         KICK_ERR = -1 * feedback;
        //Area of the error: TimeStep*err (width*length of a rectangle)
        
       // if(clamp == false)
@@ -111,10 +118,10 @@
         //Calculate the derivative of the error term for the controller.
         //  e'(t) = (error(t) - error(t-1))/dt
         float errChange = (err - err_prev)/(TI_STEERING);
-        
+        KICK_CHNG_ERR = (KICK_ERR - PREV_KICK_ERR)/(TI_STEERING);
         //Calculate the controller output
         //Angle in radians
-        float servo_angle = (0.15+KP*err + KD*errChange*TIME_DERIVATIVE + KI_STEERING*errorArea*TIME_INTEGRAL);
+        float servo_angle = (OFFSET_ANGLE+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);
         
@@ -127,8 +134,12 @@
              clamp = false;
             }
             
+         float strCheck = rad2dc(0);
+        servo_motor_pwm.write(current_duty_cycle);
         
-        servo_motor_pwm.write(current_duty_cycle);
+        err_prev = err;
+        PREV_KICK_ERR = KICK_ERR;
+        errorAreaPrev = errorArea;
         
       }
       else {
@@ -136,7 +147,7 @@
       
             servo_motor_pwm.write(current_duty_cycle);
           };
- 
+        
 }