Diff: main.cpp

Revision:

3:d268f6e06b7a

Parent:

2:2c35ad38bf00

Child:

4:eb6d6d25355c

--- a/main.cpp	Thu Dec 17 02:46:09 2015 +0000
+++ b/main.cpp	Mon Dec 21 00:40:11 2015 +0000
@@ -54,11 +54,27 @@
     const int32_t MAX_DUTY_CYCLE = 100;
     const int32_t MIN_DUTY_CYCLE = -100;
     
+    
+    /***************************************************
+    |  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.
+    
     //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; //starts bot off at 80% duty cycle
+    const int32_t TARGET_DUTY_CYCLE = 50;
     
     //raw sensor data
     uint8_t ir_val = 0;
@@ -90,6 +106,9 @@
     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
+    
     int32_t current_error = 0;
     int32_t previous_error = 0;
     
@@ -198,10 +217,14 @@
                 case(ERROR_RT2):
                     current_error = -7;
                 break;
+                
+                case(ERROR_OFF_TRACK):
+                    //do circles cause I am lost
+                    current_error = 7;
+                break;
          
                 default:
-                    //do circles because I am a lost bot 
-                    current_error = 7;  
+                    current_error = previous_error;  
                 break;    
             }
             
@@ -228,13 +251,13 @@
             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) + 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));
+            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 );