File content as of revision 1:b928ca54cd1a:

/**********************************************************************
* 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"


//helper functions to make code in main loop clearer/readable
int32_t clamp_duty_cycle(int32_t max, int32_t min, int32_t dc);


//state variables for ISR
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);
}


int main(void)
{
    start_stop_button.fall(&start_stop);
    
    start_stop_led = 2; // D7 on D6 off = red
    
    BusIn ir_bus(P4_0, P4_1, P4_2, P4_3, P4_4, P4_5, P4_6, P4_7);
    DigitalOut ir_bus_enable(D3, 1); //active high enable
    
    DigitalOut loop_pulse(D8, 0);
    
    Max14871_Shield motor_shield(D14, D15, true);// Default config

    Max14871_Shield::max14871_motor_driver_t r_motor_driver = Max14871_Shield::MD4;
    Max14871_Shield::max14871_motor_driver_t l_motor_driver = Max14871_Shield::MD3;
    
    int32_t r_duty_cycle = 0;
    int32_t l_duty_cycle = 0;
    
    const int32_t MAX_DUTY_CYCLE = 100;
    const int32_t MIN_DUTY_CYCLE = -100;
    
    //set PID terms to 0 if not used/needed
    const int32_t KP = 7;
    const int32_t KI = 0;
    const int32_t KD = 0;
    const int32_t TARGET_DUTY_CYCLE = 50; //starts bot off at 80% duty cycle
    
    //raw sensor data
    uint8_t ir_val = 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
    
    //special case error signals, 90 degree turns
    const uint8_t ERROR_LT0 = 0xE0; //b11100000
    const uint8_t ERROR_LT1 = 0xF0; //b11110000
    const uint8_t ERROR_LT2 = 0xF8; //b11111000
    const uint8_t ERROR_RT0 = 0x07; //b00000111
    const uint8_t ERROR_RT1 = 0x0F; //b00001111
    const uint8_t ERROR_RT2 = 0x1F; //b00011111
    
    int32_t current_error = 0;
    int32_t previous_error = 0;
    
    int32_t integral = 0;
    int32_t derivative = 0;
    
    
    for(;;)
    {
        //wait for start_stop button press
        while(!run);
        
        //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)
        {
            loop_pulse = !loop_pulse;
            
            ir_val = ~(ir_bus.read());
            
            //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;
                
                case(ERROR_LT0):
                    current_error = 7;
                break;
                
                case(ERROR_LT1):
                    current_error = 7;
                break;
                
                case(ERROR_LT2):
                    current_error = 7;
                break;
                
                case(ERROR_RT0):
                    current_error = -7;
                break;
                
                case(ERROR_RT1):
                    current_error = -7;
                break;
                
                case(ERROR_RT2):
                    current_error = -7;
                break;
         
                default:
                    //do circles because I am a lost bot 
                    current_error = 7;  
                break;    
            }
            
            
            //get integral term, if statement helps w/wind-up
            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 + KD*derivative));
            //clamp to limits
            r_duty_cycle  = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, r_duty_cycle );
       
    
            //get new duty cycle for left motor  
            l_duty_cycle  = (TARGET_DUTY_CYCLE + (KP*current_error + KI*integral + KD*derivative));
            //clamp to limits
            l_duty_cycle  = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, l_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);
                motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f);
            }
            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);
                motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f);
            }
            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;
    }
}


int32_t clamp_duty_cycle(int32_t max, int32_t min, int32_t dc)
{
    if(dc > max)
    {
        dc = max;
    }
    
    if(dc < min)
    {
        dc = min;
    }
    
    return(dc);
}