Diff: steering_methods.h

Revision:

31:d570f957e083

Parent:

28:1c2eb25d624e

--- a/steering_methods.h	Fri Nov 19 22:53:54 2021 +0000
+++ b/steering_methods.h	Mon Nov 22 21:44:10 2021 +0000
@@ -54,7 +54,7 @@
             }
 
    //  }  
-    if(right_landmark_sensor.read() == 1 && left_landmark_sensor.read() == 1&& land.read_us() > 4)
+    if(right_landmark_sensor.read() == 1 && left_landmark_sensor.read() == 1)
       {  
         lap++;
         counter = 0;
@@ -85,6 +85,7 @@
 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;
+Timer k;
 void steering_control(void)
 {
     
@@ -96,6 +97,25 @@
     float sensor_difference = left_distance_sensor.read() -
                                   right_distance_sensor.read();
     
+        REF = 0.0;
+        if(counter ==1 || counter == 2 )
+            REF = 0.01;
+        if(counter == 3)
+            {
+                k.start();
+                REF = -0.01;
+                if(k.read_ms() > 500)
+                    REF = 0.0;
+                    k.stop();
+                }
+        if(counter == 6 || counter == 7)
+           {
+                REF=0.02;
+                
+            }
+
+        
+     
        /*if (counter == 1 && lap == 3){
         KP = 6; 
         KD = 1;
@@ -121,7 +141,7 @@
         KICK_CHNG_ERR = (KICK_ERR - PREV_KICK_ERR)/(TI_STEERING);
         //Calculate the controller output
         //Angle in radians
-        float servo_angle = (OFFSET_ANGLE+KP*err + KD*errChange*TIME_DERIVATIVE + KI_STEERING*errorArea*TIME_INTEGRAL);
+        float servo_angle = (OFFSET_ANGLE+KP*err + KD*KICK_CHNG_ERR*TIME_DERIVATIVE + KI_STEERING*errorArea*TIME_INTEGRAL);
         //Calculate the duty cycle for the servo motor
         current_duty_cycle = rad2dc(servo_angle);