Diff: steering_methods.h

Revision:

20:7dcdadbd7bc0

Parent:

19:65fecaa2a387

Child:

22:08d30ea47111

--- a/steering_methods.h	Thu Oct 28 00:28:29 2021 +0000
+++ b/steering_methods.h	Thu Oct 28 21:41:03 2021 +0000
@@ -43,12 +43,12 @@
     if(counter >= 15)
         counter =0;
     turn_seg_off(seg1);
-    if(right_landmark_sensor.read() == 1 || left_landmark_sensor.read() == 1  )
-    {
+    //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 )
       {  
       counter = 0;
@@ -94,15 +94,16 @@
         
         //Calculate the controller output
         //Angle in radians
-        float servo_angle = (KP*err + KD*errChange*TIME_DERIVATIVE)/10;
+        float servo_angle = (0.09+KP*err + KD*errChange*TIME_DERIVATIVE)/7;
         //Calculate the duty cycle for the servo motor
         current_duty_cycle = rad2dc(servo_angle);
-        if( abs(current_duty_cycle - prev_duty) >= 0.0000103){
+        if( abs(current_duty_cycle - prev_duty) >= 0.0000093){ // this is a filter
             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.