File content as of revision 34:cb9a0cec2feb:

//#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 ------------
Timer land;

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) && land.read_us() > 10  )
    { 
              landmark_triggered =true; // Debugging purpose only
              (counter > 6 ? counter = 7 : counter++);
            land.stop();
            }

   //  }  
    if(right_landmark_sensor.read() == 1 && left_landmark_sensor.read() == 1)
      {  
        lap++;
        counter = 0;
        restarted=true;  
            // Debugging purposes only
      }
    land.stop();
   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;
float errorArea;
float errorAreaPrev =0.0;
float feedback;
float err;
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)
{
    
    
    if(steering_enabled == true ){
        //Read each sensor and calculate the sensor difference
        

    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;
        KI_STEERINg = 0.8;
        
        }*/
        //calculate the feedback term for the controller. distance (meters) from
        //track to sensor
        feedback = (sensor_difference/SEN_DIFF_TO_DIST); //
          
        //calculate the error term for the controller. distance (meters) from
        //desired position to sensor
         err = REF - feedback; //
         KICK_ERR = -1 * feedback;
       //Area of the error: TimeStep*err (width*length of a rectangle)
       
      // if(clamp == false)
        errorArea= TI_STEERING*err + errorAreaPrev; 
          
        //Calculate the derivative of the error term for the controller.
        //  e'(t) = (error(t) - error(t-1))/dt
        float errChange = (err - err_prev)/(TI_STEERING);
        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*KICK_CHNG_ERR*TIME_DERIVATIVE + KI_STEERING*errorArea*TIME_INTEGRAL);
        //Calculate the duty cycle for the servo motor
        current_duty_cycle = rad2dc(servo_angle);
        
               //--- integral anti-windup ---
        if (servo_angle > 0.1 || servo_angle < 0.05){
            if (errorArea > 0.0){
                clamp = true;
                }
         } else {
             clamp = false;
            }
            
         float strCheck = rad2dc(0);
        servo_motor_pwm.write(current_duty_cycle);
        
        err_prev = err;
        PREV_KICK_ERR = KICK_ERR;
        errorAreaPrev = errorArea;
        
      }
      else {
            current_duty_cycle = rad2dc(M_PI/4);
      
            servo_motor_pwm.write(current_duty_cycle);
          };
        
}