Merge remote-tracking branch 'origin/dev_aaron' into emulator

common/app/pixy_control.c

@@ -0,0 +1,80 @@
+/*
+ * pixy_control.c
+ *
+ * Notation
+ * --------
+ *
+ * x    :   Sollwert (Führgrösse)
+ * w    :   Istwert (Reglergrösse)
+ * esum :   Integralteil
+ * e    :   Regelabweichung
+ * y    :   Stellgrösse
+ *
+ *
+ */
+#include<pixy_control.h>
+#include<stdint.h>
+
+// PID tuning factors
+#define REG_PID_KP      (0.23f)
+#define REG_PID_KI      (1.2f)
+#define REG_PID_KD      (0.01f)
+#define REG_PID_TA      (0.02f)
+
+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)
+{
+    float e;
+    static float esum;
+    static float ealt;
+    uint16_t y;
+
+    // Calculate 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
+    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;
+}
+
+// PID controller implementation for the x-axis
+uint16_t pixy_PID_X(uint16_t x, uint16_t w)
+{
+    float e;
+    static float esum;
+    static float ealt;
+    uint16_t y;
+
+    // Calculate 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
+    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

@@ -0,0 +1,14 @@
+/* 
+ * pixy_control.h
+ */
+
+#ifndef _CONTROL_H_
+#define _CONTROL_H_
+
+#include<stdint.h>
+
+void int_init(void);
+uint16_t pixy_PID_Y(uint16_t x, uint16_t w);
+uint16_t pixy_PID_X(uint16_t x, uint16_t w);
+
+#endif

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_X((FRAME_WIDTH / 2), x));  // track x
+    pixy_rcs_set_position(1, pixy_PID_Y((FRAME_HEIGHT / 2), y)); // track y
 }
 
 //Variable which stores all the callbacks and settings for our tracking implementation