Diff: main.cpp

Revision:

4:eb6d6d25355c

Parent:

3:d268f6e06b7a

Child:

5:c673430c8a32

--- a/main.cpp	Mon Dec 21 00:40:11 2015 +0000
+++ b/main.cpp	Wed Jan 06 22:57:35 2016 +0000
@@ -9,16 +9,17 @@
 *
 **********************************************************************/
 
+
 #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);
+//comment out following line for normal operation
+//#define TUNE_PID 1
 
 
 //state variables for ISR
-bool run = false;
+volatile bool run = false;
 BusOut start_stop_led(D6, D7);
 
 //Input to trigger interrupt off of
@@ -34,26 +35,41 @@
 
 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     |
@@ -69,19 +85,23 @@
     
     //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
-    const int32_t KP = 16;
-    const int32_t KI = 0;
-    const int32_t KD = 0;
-    const int32_t TARGET_DUTY_CYCLE = 50;
+    //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 
     
-    //raw sensor data
-    uint8_t ir_val = 0;
+    //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
+    //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
@@ -109,15 +129,35 @@
     //Lost bot error signal
     const uint8_t ERROR_OFF_TRACK = 0xFF; //b11111111
     
-    int32_t current_error = 0;
-    int32_t previous_error = 0;
     
-    int32_t integral = 0;
-    int32_t derivative = 0;
+    #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);
         
@@ -127,8 +167,10 @@
         
         while(run)
         {
+            //measure dt with scope
             loop_pulse = !loop_pulse;
             
+            //get raw ir sensor data
             ir_val = ~(ir_bus.read());
             
             //scale feedback
@@ -193,7 +235,7 @@
                 case(ERROR_SIGNAL_N7):
                     current_error = -7;
                 break;
-                
+        
                 case(ERROR_LT0):
                     current_error = 7;
                 break;
@@ -219,17 +261,28 @@
                 break;
                 
                 case(ERROR_OFF_TRACK):
-                    //do circles cause I am lost
-                    current_error = 7;
-                break;
-         
+                    //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
             
-            //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;
@@ -251,15 +304,37 @@
             previous_error = current_error;
             
             //get new duty cycle for right motor
-            r_duty_cycle = (TARGET_DUTY_CYCLE - (KP*current_error + (KI*integral) + KD*derivative));
+            r_duty_cycle = (TARGET_DUTY_CYCLE - (kp*current_error + ((ki*integral)/1000) + kd*derivative));
+            
             //clamp to limits
-            r_duty_cycle  = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, r_duty_cycle );
+            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) + KD*derivative));
+            l_duty_cycle  = (TARGET_DUTY_CYCLE + (kp*current_error + ((ki*integral)/1000) + kd*derivative));
+            
             //clamp to limits
-            l_duty_cycle  = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, l_duty_cycle );
+            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
@@ -306,19 +381,3 @@
         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);
-}
\ No newline at end of file