Comment refactoring, updated PID values

common/app/pixy_control.c

@@ -13,12 +13,13 @@
  *
  */
 #include<pixy_control.h>
+#include<stdint.h>
 
 // PID tuning factors
-#define REG_PID_KP      1
-#define REG_PID_KI      1
-#define REG_PID_KD      1
-#define REG_PID_TA      1
+#define REG_PID_KP      (0.23f)
+#define REG_PID_KI      (1.2f)
+#define REG_PID_KD      (0.01f)
+#define REG_PID_TA      (0.02f)
 
 #define REG_PI_KP       1
 #define REG_PI_KI       1
@@ -28,7 +29,7 @@ void int_init(void){
     // TODO Init ports and outputs if needed.
 }
 
-// PI controller
+// PI controller implementatino
 uint16_t pixy_PI(uint16_t x, uint16_t w)
 {
     uint16_t        y = 0;
@@ -38,8 +39,7 @@ uint16_t pixy_PI(uint16_t x, uint16_t w)
     // Calculate controller offset
     e = x - w;
 
-    //----PID-control--------------------------------
-
+    //----PID-control-------------------------------------------------
     // Integrate and check boundaries if necassary
     esum = esum + e;
     //esum = (esum < -400) ? -400 : esum;
@@ -47,14 +47,13 @@ uint16_t pixy_PI(uint16_t x, uint16_t w)
 
     // PI controller equation
     y = ( REG_PI_KP * e ) + ( REG_PI_KI * REG_PI_TA * esum);
-
-    //-----------------------------------------------
+    //----------------------------------------------------------------
 
     return y;
 }
 
-// PID controller
-uint16_t control_SpeedPID(uint16_t x, uint16_t w)
+// PID controller implementation
+uint16_t pixy_PID(uint16_t x, uint16_t w)
 {
     float e;
     static float esum;
@@ -64,21 +63,18 @@ uint16_t control_SpeedPID(uint16_t x, uint16_t w)
     // Calculate controller offset
     e = x - w;
 
-    //----PID-control--------------------------------
+    //----PID-control-------------------------------------------------------------------------
+    esum = esum + e;                            // add e to the current sum
 
-    // Integrate and check boundaries if necessary
-    esum = esum + e;
-    //esum = (esum > 50)? 50 : esum;
-    //esum = (esum < -50)? -50 : esum;
+    esum = (esum > 65535)  ? 65535 : esum;      // check upper boundary and limit size
+    esum = (esum < 0) ? 0 : esum;               // check lower boundary and limit size
 
     // 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
     ealt = e;
 
     return y;
 }
-

common/app/pixy_control.h

@@ -5,6 +5,8 @@
 #ifndef _CONTROL_H_
 #define _CONTROL_H_
 
+#include<stdint.h>
+
 void int_init(void);
 uint16_t pixy_PID(uint16_t x, uint16_t w);
 uint16_t pixy_PI(uint16_t x, uint16_t w);

common/app/screen_tracking.c

@@ -1,4 +1,5 @@
 #include "screen_tracking.h"
+#include "pixy_control.h"
 #include "button.h"
 #include "checkbox.h"
 #include "tft.h"
@@ -99,8 +100,11 @@ 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) {
-	//TODO: Implement tracking!
-	//Calculate new servo pos and set the new servo pos
+    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((FRAME_WIDTH / 2), x));  // track x
+    pixy_rcs_set_position(1, pixy_PID((FRAME_HEIGHT / 2), y)); // track y
 }
 
 //Variable which stores all the callbacks and settings for our tracking implementation