Diff: main.cpp

Revision:

1:b928ca54cd1a

Parent:

0:d2693f27f344

Child:

2:2c35ad38bf00

diff -r d2693f27f344 -r b928ca54cd1a main.cpp
--- a/main.cpp	Mon Dec 14 00:03:54 2015 +0000
+++ b/main.cpp	Thu Dec 17 02:29:28 2015 +0000
@@ -2,25 +2,33 @@
 * 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"
 
 
-float ramp(float val, float val_old);
+//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;
-DigitalOut run_led(LED_GREEN, 1); //leds have inverted logic
-DigitalOut stop_led(LED_RED, 0);
+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;
-    run_led = !run_led;
-    stop_led = !stop_led;
+    start_stop_led = (start_stop_led ^ 3);
 }
 
 
@@ -28,179 +36,175 @@
 {
     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;
     
-    const float MAX_DUTY_CYCLE = 1.0;
-    const float MIN_DUTY_CYCLE = 0.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;
     
-    //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;
+    //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;
     
-    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;
+    //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
     
-    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;
+    //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.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)
         {
+            loop_pulse = !loop_pulse;
+            
             ir_val = ~(ir_bus.read());
             
             //scale feedback
             switch(ir_val)
             {
-                case(ZONE_8):
-                if(fb < 0)
-                {
-                  fb = -8;
-                }
-                else
-                {
-                  fb = 8;
-                }                    
+                case(ERROR_SIGNAL_P7):
+                    current_error = 7;
                 break;
-                
-                case(ZONE_7):
-                fb = 7;
+              
+                case(ERROR_SIGNAL_P6):
+                    current_error = 6;
                 break;
               
-                case(ZONE_6):
-                fb = 6;
+                case(ERROR_SIGNAL_P5):
+                    current_error = 5;
                 break;
               
-                case(ZONE_5):
-                fb = 5;
+                case(ERROR_SIGNAL_P4):
+                    current_error = 4;
+                break;
+              
+                case(ERROR_SIGNAL_P3):
+                    current_error = 3;
                 break;
               
-                case(ZONE_4):
-                fb = 4;
+                case(ERROR_SIGNAL_P2):
+                    current_error = 2;
                 break;
               
-                case(ZONE_3):
-                fb = 3;
-                break;
-              
-                case(ZONE_2):
-                fb = 2;
+                case(ERROR_SIGNAL_P1):
+                    current_error = 1;
                 break;
               
-                case(ZONE_1):
-                fb = 1;
+                case(ERROR_SIGNAL_00):
+                    current_error = 0;
                 break;
               
-                case(ZONE_0):
-                fb = 0;
+                case(ERROR_SIGNAL_N1):
+                    current_error = -1;
                 break;
               
-                case(ZONE_N1):
-                fb = -1;
+                case(ERROR_SIGNAL_N2):
+                    current_error = -2;
                 break;
               
-                case(ZONE_N2):
-                fb = -2;
+                case(ERROR_SIGNAL_N3):
+                    current_error = -3;
                 break;
               
-                case(ZONE_N3):
-                fb = -3;
+                case(ERROR_SIGNAL_N4):
+                    current_error = -4;
                 break;
               
-                case(ZONE_N4):
-                fb = -4;
+                case(ERROR_SIGNAL_N5):
+                    current_error = -5;
                 break;
               
-                case(ZONE_N5):
-                fb = -5;
+                case(ERROR_SIGNAL_N6):
+                    current_error = -6;
                 break;
               
-                case(ZONE_N6):
-                fb = -6;
+                case(ERROR_SIGNAL_N7):
+                    current_error = -7;
                 break;
-              
-                case(ZONE_N7):
-                fb = -7;
-                break;
-              
-                case(ZONE_LT0):
-                fb = 8;
+                
+                case(ERROR_LT0):
+                    current_error = 7;
                 break;
                 
-                case(ZONE_LT1):
-                fb = 8;
+                case(ERROR_LT1):
+                    current_error = 7;
                 break;
                 
-                case(ZONE_LT2):
-                fb = 8;
+                case(ERROR_LT2):
+                    current_error = 7;
                 break;
-              
-                case(ZONE_RT0):
-                fb = -8;
+                
+                case(ERROR_RT0):
+                    current_error = -7;
                 break;
                 
-                case(ZONE_RT1):
-                fb = -8;
+                case(ERROR_RT1):
+                    current_error = -7;
                 break;
                 
-                case(ZONE_RT2):
-                fb = -8;
+                case(ERROR_RT2):
+                    current_error = -7;
                 break;
-              
-                default:       
-                fb = fb;
+         
+                default:
+                    //do circles because I am a lost bot 
+                    current_error = 7;  
+                break;    
             }
             
-            //get error signal
-            current_error = SP-fb;
             
             //get integral term, if statement helps w/wind-up
             if(current_error == 0)
@@ -224,76 +228,74 @@
             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 = (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 = MAX_DUTY_CYCLE;
+                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);
             }
-            
-            if(r_duty_cycle < MIN_DUTY_CYCLE)
+            else
             {
-                r_duty_cycle = MIN_DUTY_CYCLE;
+                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);
             }
             
-            //get new duty cycle for left motor  
-            l_duty_cycle = ((KP*current_error) + (KI*integral) + (KD*derivative));
-            if(l_duty_cycle > MAX_DUTY_CYCLE)
+            //update direction and duty cycle for left motor
+            if(l_duty_cycle < 0)
             {
-                l_duty_cycle = MAX_DUTY_CYCLE;
-            }
-            
-            if(l_duty_cycle < MIN_DUTY_CYCLE)
-            {
-                l_duty_cycle = MIN_DUTY_CYCLE;
+                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);
             }
-            
-            //ramp motors
-            do
+            else
             {
-                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));
+                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::BRAKE);
-        motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::BRAKE);
+        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;
     }
 }
 
 
-float ramp(float val, float val_old)
+int32_t clamp_duty_cycle(int32_t max, int32_t min, int32_t dc)
 {
-  if(val > val_old)
-  {
-    if((val - val_old) >= 0.1)
-    {
-      val_old += 0.1;
-    }   
-    else
+    if(dc > max)
     {
-      val_old = val;
-    }                                   
-  }
-  else if(val < val_old)
-  {
-    if((val_old - val) >= 0.1)
-    {
-      val_old -= 0.1;
-    }   
-    else
+        dc = max;
+    }
+    
+    if(dc < min)
     {
-      val_old = val;
-    }   
-  }
-  else
-  {
-    val_old = val;
-  }
-  
-  return(val_old);
-}  
+        dc = min;
+    }
+    
+    return(dc);
+}
\ No newline at end of file