File content as of revision 0:d2693f27f344:

/**********************************************************************
* Simple line following bot with PID to demo MAXREFDES89#
* PID feedback comes from infrared led sensor from Parallax
* https://www.parallax.com/product/28034
**********************************************************************/

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


float ramp(float val, float val_old);

bool run = false;
DigitalOut run_led(LED_GREEN, 1); //leds have inverted logic
DigitalOut stop_led(LED_RED, 0);

InterruptIn start_stop_button(SW3);

void start_stop()
{
    run = !run;
    run_led = !run_led;
    stop_led = !stop_led;
}


int main(void)
{
    start_stop_button.fall(&start_stop);
    
    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
    
    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;
    
    const float MAX_DUTY_CYCLE = 1.0;
    const float MIN_DUTY_CYCLE = 0.0;
    
    //set point is for position relative to line, not duty cycle of pwm
    const float SP = 0.0;
    const float KP = 1.0;
    const float KI = 0.0;
    const float KD = 0.0;
    //const float OFFSET = 0;
    
    //raw sensor data
    uint8_t ir_val = 0;
    
    const uint8_t ZONE_8 = 0xFF;
    const uint8_t ZONE_7 = 0xFE;
    const uint8_t ZONE_6 = 0xFC;
    const uint8_t ZONE_5 = 0xFD;
    const uint8_t ZONE_4 = 0xF9;
    const uint8_t ZONE_3 = 0xFB;
    const uint8_t ZONE_2 = 0xF3;
    const uint8_t ZONE_1 = 0xF7;
    const uint8_t ZONE_0 = 0xE7;
    const uint8_t ZONE_N1 = 0xEF;
    const uint8_t ZONE_N2 = 0xCF;
    const uint8_t ZONE_N3 = 0xDF;
    const uint8_t ZONE_N4 = 0x9F;
    const uint8_t ZONE_N5 = 0xBF;
    const uint8_t ZONE_N6 = 0x3F;
    const uint8_t ZONE_N7 = 0x7F;
    const uint8_t ZONE_LT0 = 0xE0;
    const uint8_t ZONE_LT1 = 0xF0;
    const uint8_t ZONE_LT2 = 0xF8;
    const uint8_t ZONE_RT0 = 0x07;
    const uint8_t ZONE_RT1 = 0x0F;
    const uint8_t ZONE_RT2 = 0x1F;
    
    float r_duty_cycle = 0.0;
    float r_duty_cycle_old = r_duty_cycle;
  
    float l_duty_cycle = 0.0;
    float l_duty_cycle_old = l_duty_cycle;
  
    float fb = 0;
    float current_error = 0;
    float previous_error = 0;
    float integral = 0;
    float derivative = 0;
    
    for(;;)
    {
        //wait for start_stop button press
        while(!run);
        
        //mode is set to forward, but duty cycle is still 0.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)
        {
            ir_val = ~(ir_bus.read());
            
            //scale feedback
            switch(ir_val)
            {
                case(ZONE_8):
                if(fb < 0)
                {
                  fb = -8;
                }
                else
                {
                  fb = 8;
                }                    
                break;
                
                case(ZONE_7):
                fb = 7;
                break;
              
                case(ZONE_6):
                fb = 6;
                break;
              
                case(ZONE_5):
                fb = 5;
                break;
              
                case(ZONE_4):
                fb = 4;
                break;
              
                case(ZONE_3):
                fb = 3;
                break;
              
                case(ZONE_2):
                fb = 2;
                break;
              
                case(ZONE_1):
                fb = 1;
                break;
              
                case(ZONE_0):
                fb = 0;
                break;
              
                case(ZONE_N1):
                fb = -1;
                break;
              
                case(ZONE_N2):
                fb = -2;
                break;
              
                case(ZONE_N3):
                fb = -3;
                break;
              
                case(ZONE_N4):
                fb = -4;
                break;
              
                case(ZONE_N5):
                fb = -5;
                break;
              
                case(ZONE_N6):
                fb = -6;
                break;
              
                case(ZONE_N7):
                fb = -7;
                break;
              
                case(ZONE_LT0):
                fb = 8;
                break;
                
                case(ZONE_LT1):
                fb = 8;
                break;
                
                case(ZONE_LT2):
                fb = 8;
                break;
              
                case(ZONE_RT0):
                fb = -8;
                break;
                
                case(ZONE_RT1):
                fb = -8;
                break;
                
                case(ZONE_RT2):
                fb = -8;
                break;
              
                default:       
                fb = fb;
            }
            
            //get error signal
            current_error = SP-fb;
            
            //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 = ((KP*current_error) + (KI*integral) + (KD*derivative));
            if(r_duty_cycle > MAX_DUTY_CYCLE)
            {
                r_duty_cycle = MAX_DUTY_CYCLE;
            }
            
            if(r_duty_cycle < MIN_DUTY_CYCLE)
            {
                r_duty_cycle = MIN_DUTY_CYCLE;
            }
            
            //get new duty cycle for left motor  
            l_duty_cycle = ((KP*current_error) + (KI*integral) + (KD*derivative));
            if(l_duty_cycle > MAX_DUTY_CYCLE)
            {
                l_duty_cycle = MAX_DUTY_CYCLE;
            }
            
            if(l_duty_cycle < MIN_DUTY_CYCLE)
            {
                l_duty_cycle = MIN_DUTY_CYCLE;
            }
            
            //ramp motors
            do
            {
                r_duty_cycle_old = ramp(r_duty_cycle, r_duty_cycle_old);
                motor_shield.set_pwm_duty_cycle(r_motor_driver, r_duty_cycle_old);
                
                l_duty_cycle_old = ramp(l_duty_cycle, l_duty_cycle_old); 
                motor_shield.set_pwm_duty_cycle(l_motor_driver, l_duty_cycle_old);
            }             
            while((r_duty_cycle != r_duty_cycle_old) || (l_duty_cycle != l_duty_cycle_old));
        }
      
        //shutdown  
        motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::BRAKE);
        motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::BRAKE);
    }
}


float ramp(float val, float val_old)
{
  if(val > val_old)
  {
    if((val - val_old) >= 0.1)
    {
      val_old += 0.1;
    }   
    else
    {
      val_old = val;
    }                                   
  }
  else if(val < val_old)
  {
    if((val_old - val) >= 0.1)
    {
      val_old -= 0.1;
    }   
    else
    {
      val_old = val;
    }   
  }
  else
  {
    val_old = val;
  }
  
  return(val_old);
}