Diff: steering_methods.h

Revision:

23:4c743746533c

Parent:

22:08d30ea47111

Child:

24:7bf492bf50f4

--- a/steering_methods.h	Wed Nov 03 16:13:02 2021 +0000
+++ b/steering_methods.h	Wed Nov 03 21:52:00 2021 +0000
@@ -34,27 +34,37 @@
             };
     }
 //  ------------ LANDMARK DETECTION ------------
+Timer land;
 int counter = 0;
 volatile bool landmark_triggered=false; // Debugging purposes only
 volatile bool restarted = false;        // Debugging purposes only
+
 void landmark_counter(void){
+    land.start();
     landmark_triggered=false; // Debugging purposes only
     restarted = false;        // Debugging purposes only
     if(counter >= 15)
         counter =0;
     turn_seg_off(seg1);
-    //if(right_landmark_sensor.read() == 1 || left_landmark_sensor.read() == 1  )
-    //{
-        landmark_triggered =true; // Debugging purpose only
-        counter++ ;
-           
-     // }  
+  //  if(right_landmark_sensor.read() == 1 || left_landmark_sensor.read() == 1  )
+  //  { 
+        if(land.read_us() > 1)
+           {
+              landmark_triggered =true; // Debugging purpose only
+            counter++ ;
+            land.stop();
+            }
+
+   //  }  
     if(right_landmark_sensor.read() == 1 && left_landmark_sensor.read() == 1 )
       {  
-      counter = 0;
-      restarted=true;       // Debugging purposes only
+      if(land.read_us() > 1)
+      {  
+        counter = 0;
+        restarted=true;  
+        }     // Debugging purposes only
       }
-
+    land.stop();
    show_decimal(counter, seg1);
     }
 
@@ -92,7 +102,7 @@
 
        //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
@@ -100,7 +110,7 @@
         
         //Calculate the controller output
         //Angle in radians
-        float servo_angle = (0.04+KP*err + KD*errChange*TIME_DERIVATIVE + KI_STEERING*errorArea*TIME_INTEGRAL);
+        float servo_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);
         servo_motor_pwm.write(current_duty_cycle);