Diff: main.cpp

Revision:

0:d2693f27f344

Child:

1:b928ca54cd1a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 14 00:03:54 2015 +0000
@@ -0,0 +1,299 @@
+/**********************************************************************
+* 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);
+}