File content as of revision 6:dfbcdc63d4f8:

/**********************************************************************
* Simple line following bot with PID to demo MAXREFDES89#
* PID feedback comes from infrared led sensor from Parallax
* https://www.parallax.com/product/28034
*
* The following link is a good example for a PID line follower
*
* http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html
*
**********************************************************************/


#include "mbed.h"
#include "max14871_shield.h"


//comment out following line for normal operation
//#define TUNE_PID 1

//state variables for ISR
volatile bool run = false;
BusOut start_stop_led(D6, D7);

//Input to trigger interrupt off of
InterruptIn start_stop_button(SW3);

//callback fx for ISR
void start_stop()
{
    run = !run;
    start_stop_led = (start_stop_led ^ 3);
}

//Function for reading QTR-RC infrared sensor from Pololu
uint8_t get_ir_bus(BusInOut &ir_bus, uint16_t *ir_vals, uint16_t &sample_cnt);

//constants used with Pololu sensor
const uint16_t MAX_SAMPLE_TIME = 1000;
const uint8_t NUM_SENSORS = 8;

int main(void)
{
    
    //Trigger interrupt on falling edge of SW3 and cal start_stop fx
    start_stop_button.fall(&start_stop);
    
    //state indicator, default red for stop
    start_stop_led = 2; // D7 on D6 off = red
    
    //Connect IR sensor to port 4
    BusInOut ir_bus(P4_0, P4_1, P4_2, P4_3, P4_4, P4_5, P4_6, P4_7);
    
    uint16_t ir_vals[8]; 
    uint16_t samples = 0;
    
    //binary sensor data
    uint8_t ir_val = 0;
    
    //used to measure dt
    DigitalOut loop_pulse(D8, 0);
    
    //MAXREFDES89# object
    Max14871_Shield motor_shield(D14, D15, true);// Default config
        
    // local vars with names that are more meaningful and easier to use than the class name with scope operator
    Max14871_Shield::max14871_motor_driver_t r_motor_driver = Max14871_Shield::MD4;
    Max14871_Shield::max14871_motor_driver_t l_motor_driver = Max14871_Shield::MD3;
    
    motor_shield.set_current_regulation_mode(r_motor_driver, Max14871_Shield::RIPPLE_25_EXTERNAL_REF, 2.0);
    motor_shield.set_current_regulation_mode(l_motor_driver, Max14871_Shield::RIPPLE_25_EXTERNAL_REF, 2.0);
    
    int32_t r_duty_cycle = 0;
    int32_t l_duty_cycle = 0;
    
    const int32_t MAX_DUTY_CYCLE = 80;
    const int32_t MIN_DUTY_CYCLE = -80;
    
    const int32_t TARGET_DUTY_CYCLE = 75;  //was 37
    
    
    /***************************************************
    |  Control Type |   KP   |     KI     |     KD     |
    |---------------|--------|------------|------------|
    |        P      | 0.5Kc  |      0     |      0     |
    |---------------------------------------------------
    |       PI      | 0.45Kc | 1.2KpdT/Pc |      0     |
    |---------------|--------|------------|------------|
    |       PD      | 0.80Kc |      0     | KpPc/(8dT) |
    |---------------|--------|------------|------------|
    |      PID      | 0.60Kc |  2KpdT/Pc  | KpPc/(8dT) |
    ***************************************************/
    
    //Kc = critical gain, gain with just P control at which system becomes marginally stable
    //Pc = period of oscillation for previous scenario.
    
    //Set PID terms to 0 if not used/needed
    //For values below Kc = 10 and Pc was measured at 0.33 calculated starting points given in comments below
    //Ended up decreasing integral term, increasing derivative term and small decrease in proportional term.
    int32_t kp = 5; //was 4
    int32_t ki = 0; //.0576, divide by 1000 later, was 2
    int32_t kd = 0; //156.25, was 250
    
    //initialize vars
    int32_t current_error = 0;
    int32_t previous_error = current_error;
    int32_t integral = 0;
    int32_t derivative = 0;
    
    //raw sensor data scaled to a useable range for error signal
    //            SP - feedback     raw sensor ir_val
    const uint8_t ERROR_SIGNAL_P7 = 0xFE; //b11111110
    const uint8_t ERROR_SIGNAL_P6 = 0xFC; //b11111100
    const uint8_t ERROR_SIGNAL_P5 = 0xFD; //b11111101
    const uint8_t ERROR_SIGNAL_P4 = 0xF9; //b11111001
    const uint8_t ERROR_SIGNAL_P3 = 0xFB; //b11111011
    const uint8_t ERROR_SIGNAL_P2 = 0xF3; //b11110011
    const uint8_t ERROR_SIGNAL_P1 = 0xF7; //b11110111
    const uint8_t ERROR_SIGNAL_00 = 0xE7; //b11100111, feedback = SP
    const uint8_t ERROR_SIGNAL_N1 = 0xEF; //b11101111
    const uint8_t ERROR_SIGNAL_N2 = 0xCF; //b11001111
    const uint8_t ERROR_SIGNAL_N3 = 0xDF; //b11011111
    const uint8_t ERROR_SIGNAL_N4 = 0x9F; //b10011111
    const uint8_t ERROR_SIGNAL_N5 = 0xBF; //b10111111
    const uint8_t ERROR_SIGNAL_N6 = 0x3F; //b00111111
    const uint8_t ERROR_SIGNAL_N7 = 0x7F; //b01111111
    
    #ifdef TUNE_PID
        char response = 'N';
        
        printf("\nDo you want to change the PID coefficents? 'Y' or 'N': ");
        scanf("%c", &response);
        
        if((response == 'Y') || (response == 'y'))
        {
            printf("\nCurrent kp = %d, please enter new kp = ", kp);
            scanf("%d", &kp);
            
            printf("\nCurrent ki = %d, please enter new ki = ", ki);
            scanf("%d", &ki);
            
            printf("\nCurrent kd = %d, please enter new kd = ", kd);
            scanf("%d", &kd);
        }
        
        printf("\nThe PID coefficents are: ");
        printf("\nkp = %d", kp);
        printf("\nki = %d", ki);
        printf("\nkd = %d\n", kd);
    #endif//TUNE_PID
    
    
    //loop time is ~2.86ms
    for(;;)
    {   
        //wait for start_stop button press
        while(!run)
        {
            //get raw ir sensor data
            ir_val = ~(get_ir_bus(ir_bus, ir_vals, samples)); 
            
            printf("\nir_state = 0x%2x \t samples = %d\n\n", ir_val, samples);
            for(uint8_t idx = 0; idx < 8; idx++)
            {
                printf("%.4d \t", ir_vals[idx]);
            }
            printf("\n\n");
            
            wait(1.0);
        }
        
        //mode is set to forward, but duty cycle is still 0
        motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
        motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
        
        while(run)
        {
            //measure dt with scope
            loop_pulse = !loop_pulse;
            
            //get raw ir sensor data
            ir_val = ~(get_ir_bus(ir_bus, ir_vals, samples)); 
            
            //scale feedback
            switch(ir_val)
            {
                case(ERROR_SIGNAL_P7):
                    current_error = 7;
                break;
              
                case(ERROR_SIGNAL_P6):
                    current_error = 6;
                break;
              
                case(ERROR_SIGNAL_P5):
                    current_error = 5;
                break;
              
                case(ERROR_SIGNAL_P4):
                    current_error = 4;
                break;
              
                case(ERROR_SIGNAL_P3):
                    current_error = 3;
                break;
              
                case(ERROR_SIGNAL_P2):
                    current_error = 2;
                break;
              
                case(ERROR_SIGNAL_P1):
                    current_error = 1;
                break;
              
                case(ERROR_SIGNAL_00):
                    current_error = 0;
                break;
              
                case(ERROR_SIGNAL_N1):
                    current_error = -1;
                break;
              
                case(ERROR_SIGNAL_N2):
                    current_error = -2;
                break;
              
                case(ERROR_SIGNAL_N3):
                    current_error = -3;
                break;
              
                case(ERROR_SIGNAL_N4):
                    current_error = -4;
                break;
              
                case(ERROR_SIGNAL_N5):
                    current_error = -5;
                break;
              
                case(ERROR_SIGNAL_N6):
                    current_error = -6;
                break;
              
                case(ERROR_SIGNAL_N7):
                    current_error = -7;
                break;

                default:
                    current_error = previous_error;  
                break;    
            }
            
            /*get integral term, if statement helps w/wind-up
            
              from url in file banner
            
              If integral wind-up is a problem two common solutions are 
              (1) zero the integral, that is set the variable integral 
                  equal to zero, every time the error is zero or the 
                  error changes sign.  
              (2) "Dampen" the integral by multiplying the accumulated 
                  integral by a factor less than one when a new integral 
                  is calculated. 
            */
            
            if(current_error == 0)
            {
                integral = 0;
            }
            else if(((current_error < 0) && (previous_error > 0)) || 
                  ((current_error > 0) && (previous_error < 0)))
            {
                integral = 0;
            }
            else
            {
                integral = (integral + current_error);
            }
            
            //get derivative term
            derivative = (current_error - previous_error);
            
            //save current error for next loop
            previous_error = current_error;
            
            //get new duty cycle for right motor
            r_duty_cycle = (TARGET_DUTY_CYCLE + (kp*current_error + ((ki*integral)/1000) + kd*derivative));
            
            //clamp to limits
            if((r_duty_cycle > MAX_DUTY_CYCLE) || (r_duty_cycle < MIN_DUTY_CYCLE))
            {
                if(r_duty_cycle > MAX_DUTY_CYCLE)
                {
                    r_duty_cycle = MAX_DUTY_CYCLE;
                }
                else
                {
                    r_duty_cycle = MIN_DUTY_CYCLE;
                }
            }
       
    
            //get new duty cycle for left motor  
            l_duty_cycle  = (TARGET_DUTY_CYCLE - (kp*current_error + ((ki*integral)/1000) + kd*derivative));
            
            //clamp to limits
            if((l_duty_cycle > MAX_DUTY_CYCLE) || (l_duty_cycle < MIN_DUTY_CYCLE))
            {
                if(l_duty_cycle > MAX_DUTY_CYCLE)
                {
                    l_duty_cycle = MAX_DUTY_CYCLE;
                }
                else
                {
                    l_duty_cycle = MIN_DUTY_CYCLE;
                }
            }
            
            
            //update direction and duty cycle for right motor
            if(r_duty_cycle < 0)
            {
                r_duty_cycle = (r_duty_cycle * -1);
                motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::REVERSE);
            }
            else
            {
                motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
            }
            motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f);
            
            //update direction and duty cycle for left motor
            if(l_duty_cycle < 0)
            {
                l_duty_cycle = (l_duty_cycle * -1);
                motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::REVERSE);
            }
            else
            {
                motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
            }
            motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f);
        }
      
        //shutdown  
        motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::COAST);
        motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::COAST);
        
        //reset all data to initial state
        r_duty_cycle = 0;
        l_duty_cycle = 0;
        
        current_error = 0;
        previous_error = current_error;
    
        integral = 0;
        derivative = 0;
    }
}


//Function for reading QTR-RC infrared sensor from Pololu
uint8_t get_ir_bus(BusInOut &ir_bus, uint16_t *ir_vals, uint16_t &sample_cnt)
{
    Timer ir_timer; 
    uint8_t ir_state = 0;
    uint8_t idx = 0;
    uint16_t sample_time = 0;
    
    for(idx = 0; idx < NUM_SENSORS; idx++)
    {
        //set initial val to max for comparison later
        ir_vals[idx] = MAX_SAMPLE_TIME;
        
        //build write val for discharging caps based on number of sensors
        ir_state |= (1 << idx);
    }
    
    //set bus to output
    ir_bus.output();
    //discharge caps
    ir_bus.write(ir_state);
    
    wait_us(10);
    
    //set bus to input
    ir_bus.input();
    //ensure no pull-up-down resistors to interfere with reading
    ir_bus.mode(PullNone);
    
    //clear sample count and timer
    sample_cnt = 0;
    ir_timer.reset();
    ir_timer.start();
    
    do
    {
        sample_cnt++;
        //get sample time from start in micro seconds
        sample_time = ir_timer.read_us();
        //get bus state
        ir_state = ir_bus.read();
        
        for(idx = 0; idx < NUM_SENSORS; idx++)
        {
            //if pin state switched to 'low', record sample time
            if(!(ir_state & (1 << idx)) && (ir_vals[idx] > sample_time))
            {
                ir_vals[idx] = sample_time;
            }
        }
    }
    while(sample_time < MAX_SAMPLE_TIME);
    
    ir_timer.stop();
    
    return(ir_state);
}