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!
+    uint16_t x = blocks[0].x; // Get x coordinate of the biggest object
-	//Calculate new servo pos and set the new servo pos
+    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

discovery/src/ll_tft.c

@@ -273,7 +273,7 @@ static bool gpio_init()
     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD,ENABLE);
     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE,ENABLE);
     
-    // PORT_B init -------------------------------------------------------------------------------------
+    // PORT_B init 
     GPIO_InitStructure.GPIO_Pin     = GPIO_Pin_0;              
     GPIO_InitStructure.GPIO_Mode    = GPIO_Mode_OUT;
     GPIO_InitStructure.GPIO_OType   = GPIO_OType_PP;
@@ -282,7 +282,7 @@ static bool gpio_init()
     // configure PORT_B
     GPIO_Init(GPIOB, &GPIO_InitStructure);  
     
-    // PORT_D init -------------------------------------------------------------------------------------
+    // PORT_D init
     GPIO_PinAFConfig(GPIOD, GPIO_PinSource0,  GPIO_AF_FSMC); // PD0=FSMC_D2   -> DB2
     GPIO_PinAFConfig(GPIOD, GPIO_PinSource1,  GPIO_AF_FSMC); // PD1=FSMC_D3   -> DB3
     GPIO_PinAFConfig(GPIOD, GPIO_PinSource4,  GPIO_AF_FSMC); // PD4=FSMC_NOE  -> RD
@@ -305,7 +305,7 @@ static bool gpio_init()
     // configure PORT_D
     GPIO_Init(GPIOD, &GPIO_InitStructure);   
     
-    // PORT_E init --------------------------------------------------------------------------------------
+    // PORT_E init
     GPIO_PinAFConfig(GPIOE, GPIO_PinSource3,  GPIO_AF_FSMC); // PE3=FSMC_A19  -> RS
     GPIO_PinAFConfig(GPIOE, GPIO_PinSource7,  GPIO_AF_FSMC); // PE7=FSMC_D4   -> DB4
     GPIO_PinAFConfig(GPIOE, GPIO_PinSource8,  GPIO_AF_FSMC); // PE8=FSMC_D5   -> DB5  
@@ -316,6 +316,7 @@ static bool gpio_init()
     GPIO_PinAFConfig(GPIOE, GPIO_PinSource13, GPIO_AF_FSMC); // PE13=FSMC_D10 -> DB12
     GPIO_PinAFConfig(GPIOE, GPIO_PinSource14, GPIO_AF_FSMC); // PE14=FSMC_D11 -> DB13
     GPIO_PinAFConfig(GPIOE, GPIO_PinSource15, GPIO_AF_FSMC); // PE15=FSMC_D12 -> DB14
+   
     // PORT_E struct
     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9 |
                                 GPIO_Pin_10 | GPIO_Pin_11 | GPIO_Pin_12 | GPIO_Pin_13 |
@@ -325,6 +326,7 @@ static bool gpio_init()
     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
     GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
     GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
+  
     // configure PORT_E
     GPIO_Init(GPIOE, &GPIO_InitStructure);
 
@@ -334,6 +336,8 @@ static bool gpio_init()
 /*
  * ---------------------- display control functions -------------------------------------------------------
  */
+
+// Clear the whole screen by filling it with a specifig color
 void ll_tft_clear(uint16_t color) 
 {
     uint32_t n = 0;
@@ -346,6 +350,7 @@ void ll_tft_clear(uint16_t color)
     }
 }
 
+// Set the cursorposition
 static void tft_set_cursor(uint16_t xpos, uint16_t ypos)
 {
     // set cursor
@@ -354,52 +359,59 @@ static void tft_set_cursor(uint16_t xpos, uint16_t ypos)
     TFT_REG = TFT_SSD1289_REG_22;
 }
 
+// Enable / Disable the backlight
 static void tft_set_backlight(bool state) 
 {
-    if(state){
+    if(state){                      // if state is true
-        GPIOB->BSRRH = GPIO_Pin_0;
+        GPIOB->BSRRH = GPIO_Pin_0;  // set the backlight output
-    } else {
+    } else {                        // else
-        GPIOB->BSRRL = GPIO_Pin_0;    
+        GPIOB->BSRRL = GPIO_Pin_0;  // reset the backlight    
     }
 }
 
+// Port operations on the screen RS PIN
 static void tft_reset(bool state) 
 {
-    if(state){
+    if(state){                      // if state is ture
-        GPIOB->BSRRH = GPIO_Pin_0;
+        GPIOB->BSRRH = GPIO_Pin_0;  // Set the reset pin
-    } else {
+    } else {                        // else
-        GPIOB->BSRRL = GPIO_Pin_0;    
+        GPIOB->BSRRL = GPIO_Pin_0;  // reset the reset pin
     }
 }
 
+// Send a single command to the display controller
 static void tft_write_reg(uint8_t reg_adr, uint16_t reg_value)
 {
-    TFT_REG = reg_adr;
+    TFT_REG = reg_adr;      // set adress
-    TFT_RAM = reg_value;  
+    TFT_RAM = reg_value;    // send command
 }
 
+// Read a register value of the display controller
 static uint16_t tft_read_reg(uint8_t reg_adr)
 {
-    TFT_REG = reg_adr;
+    TFT_REG = reg_adr;      // set adress
-    return TFT_RAM;
+    return TFT_RAM;         // return value
 }
 
+// This sets a window for current draw functions
 static void tft_set_window(uint16_t xstart, uint16_t ystart, uint16_t xend, uint16_t yend)
 {
     uint16_t start,end;
     uint16_t ystart_end;
 
-    start = (ystart & 0x00FF);
+    start = (ystart & 0x00FF);              // Start adress of the window
-    end = ((yend & 0x00FF) << 8);
+    end = ((yend & 0x00FF) << 8);           // End adress of the window
-    ystart_end = (start | end);
+    ystart_end = (start | end);             // Calculate y endpoint 
 
-    tft_write_reg(TFT_SSD1289_REG_44, ystart_end);  
+    tft_write_reg(TFT_SSD1289_REG_44, ystart_end);      // Send y size
-    tft_write_reg(TFT_SSD1289_REG_45, 319-xend);
+    tft_write_reg(TFT_SSD1289_REG_45, 319-xend);        // Send x start
-    tft_write_reg(TFT_SSD1289_REG_46, 319-xstart);
+    tft_write_reg(TFT_SSD1289_REG_46, 319-xstart);      // Send x end
 }
 
+// Reset a Window
 void tft_reset_window()
 {
+    // Commands according to the datasheet
     tft_write_reg(0x44, 239 << 8);
     tft_write_reg(0x45, 0);
     tft_write_reg(0x46, 319);
@@ -409,6 +421,7 @@ void tft_reset_window()
  * ---------------------- draw functions -----------------------------------------------------------
  */
 
+// Draw a line on the given coordinates
 void ll_tft_draw_line(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16_t color) 
 {
     if(abs(x2-x1) > abs(y2-y1)) //line has more distance in x than y => iterate over x distance
@@ -462,12 +475,14 @@ void ll_tft_draw_line(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16
     } 
 }
 
+// Draw a single pixel on (x,y) with the given color
 void ll_tft_draw_pixel(uint16_t x,uint16_t y,uint16_t color) 
 {
-    tft_set_cursor(x,y);
+    tft_set_cursor(x,y);            // Set the cursor position
-    TFT_RAM = color;
+    TFT_RAM = color;                // Draw the pixel
 }
 
+// Draw a rectangle at the given coordinates with the given color
 void ll_tft_draw_rectangle(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16_t color) 
 {
     unsigned int tmp;
@@ -508,6 +523,7 @@ void ll_tft_draw_rectangle(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, u
     
 }
 
+// Draw a filled rectangle at the given coordinates with the given color
 void ll_tft_fill_rectangle(uint16_t x1,uint16_t y1,uint16_t x2,uint16_t y2, uint16_t color)
 {
     uint16_t area;