Diff: steering_methods.h

Revision:

2:c857935f928e

Child:

6:d2bd68ba99c9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/steering_methods.h	Mon Oct 25 01:28:53 2021 +0000
@@ -0,0 +1,104 @@
+//#include "steering_header.h"
+/* 
+    Include methods and functions
+*/
+
+// --------- LANDMARK COUNTER --------
+static int decimal_to_bin[16][BITS] = {
+    {0,0,0,0}, // zero
+    {0,0,0,1}, // one 
+    {0,0,1,0}, //two
+    {0,0,1,1}, //three 
+    {0,1,0,0}, // four 
+    {0,1,0,1}, //five 
+    {0,1,1,0}, // six 
+    {0,1,1,1}, //seven
+    {1,0,0,0}, //eight 
+    {1,0,0,1}, //nine 
+    {1,0,1,0}, //ten 
+    {1,0,1,1}, //11 
+    {1,1,0,0}, //12 
+    {1,1,0,1}, //13 
+    {1,1,1,0}, //14
+    {1,1,1,1}, //15 
+    } ;
+void turn_seg_off(DigitalOut segment[]){
+    for(int i =0; i<BITS; i++){
+        segment[i] = 0; 
+        }
+    }
+    void show_decimal(int number, DigitalOut segment[]) {
+        turn_seg_off(segment);
+        for(int i =0; i < BITS; i++){
+            segment[i] = decimal_to_bin[number][i]; 
+            };
+    }
+//  ------------ LANDMARK DETECTION ------------
+int counter = 0;
+void landmark_counter(void){
+    if(counter >= 15)
+        counter =0;
+    turn_seg_off(seg1);
+    if(right_landmark_sensor.read() == 1 || left_landmark_sensor.read() == 1  )
+        counter++ ;
+    if(right_landmark_sensor.read() == 1 && left_landmark_sensor.read() == 1 )
+        counter = 0;
+        
+   show_decimal(counter, seg1);
+    }
+
+  
+ float rad2dc (float angle){
+    angle = std::max(MIN_TURN_ANGLE, std::min(angle, MAX_TURN_ANGLE));
+    float x = (angle - MIN_TURN_ANGLE)/(MAX_TURN_ANGLE - MIN_TURN_ANGLE);
+    float dutyCycle = 0.05*x+0.05;
+    return dutyCycle;
+    }
+
+float current_duty_cycle=0.075;
+//  -------------- PD CONTROLLER --------------
+    // Global declearation
+float err_prev = 0;
+float prev_duty =0;
+void steering_control(void)
+{
+    
+    float err;
+    if(steering_enabled == true ){
+        //Read each sensor and calculate the sensor difference
+        float sensor_difference = left_distance_sensor.read() -
+                                  right_distance_sensor.read();
+                                  
+        //calculate the feedback term for the controller. distance (meters) from
+        //track to sensor
+        float feedback = (sensor_difference/SEN_DIFF_TO_DIST); //
+        
+        //calculate the error term for the controller. distance (meters) from
+        //desired position to sensor
+         err = REF - feedback; //
+        
+        //Calculate the derivative of the error term for the controller.
+        //  e'(t) = (error(t) - error(t-1))/dt
+        float errChange = (err - err_prev)/(TI);
+        
+        //Calculate the controller output
+        //Angle in radians
+        float servo_angle = (KP*err + KD*errChange*TIME_DERIVATIVE)/10;
+        //Calculate the duty cycle for the servo motor
+        current_duty_cycle = rad2dc(servo_angle);
+        if( abs(current_duty_cycle - prev_duty) >= 0.001){
+            servo_motor_pwm.write(current_duty_cycle);
+            }
+      }
+      else if(steering_enabled == false){
+            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;
+    
+}
+
+ 
+