Diff: main.cpp

Revision:

17:c643b6b4a96f

Parent:

16:11ba5d6f42ba

Child:

19:85eb7991e2ab

--- a/main.cpp	Wed Feb 25 17:56:16 2015 +0000
+++ b/main.cpp	Wed Feb 25 19:35:14 2015 +0000
@@ -12,6 +12,18 @@
     float current_servo_position = 0;
     float current_left_motor_speed = 0;
     float current_right_motor_speed = 0;
+    
+    float dt = 0.00001;
+    float integral = 0;
+    float derivative = 0;
+    float output = 0;
+    
+    // gains on prop, int, der
+    // subject to change, need to fine tune
+    float kp = 1.8960;
+    float ki = 0.6170;
+    float kd = 1.5590;
+    
     bool rear_motor_enable_flag = true;
     bool linescan_ping_pong = false;
     bool linescan_enable = true;
@@ -22,6 +34,11 @@
     int sum_black = 0;
     int violence_level = 0;
     
+    int set_point = 64;
+    int previous error = 0;
+    int error = 0;
+
+    
     //uint16_t  MyImage0Buffer[2][128];
     //uint16_t  MyImage1Buffer[2][128];
 
@@ -127,6 +144,19 @@
                     // value of center of black (single line)
                     black_center_value= sum_black / black_value_count;
                     
+                    /* ******* PID ALGORITHM *******
+                    
+                    error = set_point - black_center_value;
+                    integral = integral + error*dt;
+                    derivative = (error - previous_error)/dt;
+                    output = kp*error + ki*integral + kd*derivative;
+                    previous error = error;
+                    
+                    GOTTA DO SOME TESTS/SIMULATIONS TO CALIBRATE SERVO ADJUSTMENTS
+                    
+                    
+                       *****************************
+                    */
 
                     // need to turn left
                     if (black_center_value < 64) {