File content as of revision 4:eb6d6d25355c:

/**********************************************************************
* 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);
}


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
    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
    
    //raw 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 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 = 100;
    const int32_t MIN_DUTY_CYCLE = -100;
    
    const int32_t TARGET_DUTY_CYCLE = 35;
    
    
    /***************************************************
    |  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.
    //for values below Kc = 10 and Pc was measured at 0.33
    
    //set PID terms to 0 if not used/needed
    //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; //6
    int32_t ki = 10; //.0576, divide by 1000 later
    int32_t kd = 500; //156.25 
    
    //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
    
    //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
    
    //Lost bot error signal
    const uint8_t ERROR_OFF_TRACK = 0xFF; //b11111111
    
    
    #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 ~1.6ms
    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)
        {
            //measure dt with scope
            loop_pulse = !loop_pulse;
            
            //get raw ir sensor data
            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;
                
                case(ERROR_OFF_TRACK):
                    //requires reset
                    motor_shield.set_pwm_duty_cycle(r_motor_driver, 0.0f);
                    motor_shield.set_pwm_duty_cycle(l_motor_driver, 0.0f);
                    while(1);
                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);
                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;
    }
}