File content as of revision 9:5f032ca6c9b5:

//#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;
    
}