Fixed pid controller and refactored code

common/app/pixy_control.c

@@ -16,65 +16,57 @@
 #include<stdint.h>
 
 // PID tuning factors
-#define REG_PID_KP      (0.23f)
+#define REG_PID_KP      (0.5f)
-#define REG_PID_KI      (1.2f)
+#define REG_PID_KI      (0.001f)
-#define REG_PID_KD      (0.01f)
+#define REG_PID_KD      (0.001f)
-#define REG_PID_TA      (0.02f)
+#define REG_PID_TA      (0.01f)
 
 void int_init(void){  
     // TODO Init ports and outputs if needed.
 }
 
 // PID controller implementatoin for the y-axis
-uint16_t pixy_PID_Y(uint16_t x, uint16_t w)
+int16_t pixy_PID_Y(int16_t x, int16_t w)
 {
     float e;
     static float esum;
-    static float ealt;
+    static float eold;
-    uint16_t y;
+    float y;
 
-    // Calculate controller offset
+    e = (float)(x - w);                         // calculate the controller offset
-    e = x - w;
 
     //----PID-control-------------------------------------------------------------------------
     esum = esum + e;                            // add e to the current sum
 
-    esum = (esum > 1000)  ? 1000 : esum;        // check upper boundary and limit size
+    y += REG_PID_KP * e;                        // add the proportional part to the output
-    esum = (esum < 0) ? 0 : esum;               // check lower boundary and limit size
+    y += REG_PID_KI * REG_PID_TA * esum;        // add the integral part to the output
-
+    y += REG_PID_KD * (e - eold) / REG_PID_TA;  // add the differential part to the output
-    // PID controller equation
-    y = REG_PID_KP * e + REG_PID_KI * REG_PID_TA * esum + REG_PID_KD * (e - ealt)/REG_PID_TA;
     //----------------------------------------------------------------------------------------
 
-    // save old value
+    eold = e;                                   // save the previous value
-    ealt = e;
 
-    return y;
+    return (int16_t) y;
 }
 
 // PID controller implementation for the x-axis
-uint16_t pixy_PID_X(uint16_t x, uint16_t w)
+int16_t pixy_PID_X(int16_t x, int16_t w)
 {
     float e;
     static float esum;
-    static float ealt;
+    static float eold;
-    uint16_t y;
+    float y;
 
-    // Calculate controller offset
+    e = (float)(x - w);                         // calculate the controller offset
-    e = x - w;
 
     //----PID-control-------------------------------------------------------------------------
     esum = esum + e;                            // add e to the current sum
 
-    esum = (esum > 1000)  ? 1000 : esum;        // check upper boundary and limit size
+    y += REG_PID_KP * e;                        // add the proportional part to the output
-    esum = (esum < 0) ? 0 : esum;               // check lower boundary and limit size
+    y += REG_PID_KI * REG_PID_TA * esum;        // add the integral part to the output
-
+    y += REG_PID_KD * (e - eold) / REG_PID_TA;  // add the differential part to the output
-    // PID controller equation
-    y = REG_PID_KP * e + REG_PID_KI * REG_PID_TA * esum + REG_PID_KD * (e - ealt)/REG_PID_TA;
     //----------------------------------------------------------------------------------------
 
-    // save old value
+    eold = e;                                   // save the previous value
-    ealt = e;
 
-    return y;
+    return (int16_t) y;
 }

common/app/pixy_control.h

@@ -8,7 +8,7 @@
 #include<stdint.h>
 
 void int_init(void);
-uint16_t pixy_PID_Y(uint16_t x, uint16_t w);
+int16_t pixy_PID_Y(int16_t x, int16_t w);
-uint16_t pixy_PID_X(uint16_t x, uint16_t w);
+int16_t pixy_PID_X(int16_t x, int16_t w);
 
 #endif

common/app/screen_tracking.c

@@ -81,12 +81,19 @@ typedef struct {
 } TRACKING_CONFIG_STRUCT;
 
 //Methods for our tracking implementation ahead
+static int16_t servo_x = 0;
+static int16_t servo_y = 0;
 
 //Method/Callback to start our tracking
 void tracking_our_start(void* tracking_config) {
 	//Activate pixy's data send program
 	int32_t response;
 	int return_value;
+
+    servo_x = servo_y = 500;                // set a default value of 500
+    pixy_rcs_set_position(0, servo_x);      //  
+    pixy_rcs_set_position(1, servo_y);      //
+
 	return_value = pixy_command("runprog", INT8(0), END_OUT_ARGS,  &response, END_IN_ARGS);
 }
 
@@ -100,11 +107,31 @@ void tracking_our_stop(void* tracking_config) {
 
 //Method/Callback to calculate one step of our tracking
 void tracking_our_update(void* tracking_config, struct Block* blocks, int num_blocks) {
+
+    if(num_blocks <= 0){                    // Check if there are blocks available
+        return;                             // When there are none, do nothing
+    }
+    
     uint16_t x = blocks[0].x;               // Get x coordinate of the biggest object
     uint16_t y = blocks[0].y;               // Get y coordinate of the biggest object
 
-    pixy_rcs_set_position(0, pixy_PID_X((FRAME_WIDTH / 2), x));  // track x
+    int16_t xset = 0;                       
-    pixy_rcs_set_position(1, pixy_PID_Y((FRAME_HEIGHT / 2), y)); // track y
+    int16_t yset = 0;
+
+    xset = (servo_x + pixy_PID_X((FRAME_WIDTH / 2), x));    // calculate the PID output for x
+    yset = (servo_y - pixy_PID_Y((FRAME_HEIGHT / 2), y));   // calculate the PID output for y
+    
+    xset = (xset < 0) ? 0 : xset;           // x lower boundary check
+    xset = (xset > 1000) ? 1000 : xset;     // x upper boundary check
+    
+    yset = (yset < 0) ? 0 : yset;           // y lower boundary check
+    yset = (yset > 1000) ? 1000 : yset;     // y upper boundary check
+
+    servo_x = xset;                         // update the global, static variable for x
+    servo_y = yset;                         // update the global, statuc variable for y
+
+    pixy_rcs_set_position(0, servo_x);      // set the new x position
+    pixy_rcs_set_position(1, servo_y);      // set the new y position
 }
 
 //Variable which stores all the callbacks and settings for our tracking implementation