From e018a75cd3feb9696a462b428e7df5323f38bccb Mon Sep 17 00:00:00 2001 From: id101010 Date: Tue, 2 Jun 2015 16:25:45 +0200 Subject: [PATCH 1/3] Implemented basic pi and pid controller --- common/app/pixy_control.c | 84 +++++++++++++++++++++++++++++++++++++++ common/app/pixy_control.h | 12 ++++++ 2 files changed, 96 insertions(+) create mode 100644 common/app/pixy_control.c create mode 100644 common/app/pixy_control.h diff --git a/common/app/pixy_control.c b/common/app/pixy_control.c new file mode 100644 index 0000000..fef3fc2 --- /dev/null +++ b/common/app/pixy_control.c @@ -0,0 +1,84 @@ +/* + * pixy_control.c + * + * Notation + * -------- + * + * x : Sollwert (Führgrösse) + * w : Istwert (Reglergrösse) + * esum : Integralteil + * e : Regelabweichung + * y : Stellgrösse + * + * + */ +#include + +// 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_PI_KP 1 +#define REG_PI_KI 1 +#define REG_PI_TA 1 + +void int_init(void){ + // TODO Init ports and outputs if needed. +} + +// PI controller +uint16_t pixy_PI(uint16_t x, uint16_t w) +{ + uint16_t y = 0; + float e = 0; + static float esum; + + // Calculate controller offset + e = x - w; + + //----PID-control-------------------------------- + + // Integrate and check boundaries if necassary + esum = esum + e; + //esum = (esum < -400) ? -400 : esum; + //esum = (esum > 400) ? 400 : esum; + + // 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) +{ + float e; + static float esum; + static float ealt; + uint16_t y; + + // Calculate controller offset + e = x - w; + + //----PID-control-------------------------------- + + // Integrate and check boundaries if necessary + esum = esum + e; + //esum = (esum > 50)? 50 : esum; + //esum = (esum < -50)? -50 : esum; + + // 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; +} + diff --git a/common/app/pixy_control.h b/common/app/pixy_control.h new file mode 100644 index 0000000..da895e7 --- /dev/null +++ b/common/app/pixy_control.h @@ -0,0 +1,12 @@ +/* + * pixy_control.h + */ + +#ifndef _CONTROL_H_ +#define _CONTROL_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); + +#endif From 8c264c237a0b9a6ea29c4bb0b189b87d1ede94e5 Mon Sep 17 00:00:00 2001 From: id101010 Date: Sat, 6 Jun 2015 18:19:28 +0200 Subject: [PATCH 2/3] Comment refactoring, updated PID values --- common/app/pixy_control.c | 38 ++++++++++++++++-------------------- common/app/pixy_control.h | 2 ++ common/app/screen_tracking.c | 8 ++++++-- 3 files changed, 25 insertions(+), 23 deletions(-) diff --git a/common/app/pixy_control.c b/common/app/pixy_control.c index fef3fc2..a6b240e 100644 --- a/common/app/pixy_control.c +++ b/common/app/pixy_control.c @@ -13,12 +13,13 @@ * */ #include +#include // 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; @@ -63,22 +62,19 @@ uint16_t control_SpeedPID(uint16_t x, uint16_t w) // Calculate controller offset e = x - w; - - //----PID-control-------------------------------- - - // Integrate and check boundaries if necessary - esum = esum + e; - //esum = (esum > 50)? 50 : esum; - //esum = (esum < -50)? -50 : esum; + + //----PID-control------------------------------------------------------------------------- + esum = esum + e; // add e to the current sum + + 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; } - diff --git a/common/app/pixy_control.h b/common/app/pixy_control.h index da895e7..47112a2 100644 --- a/common/app/pixy_control.h +++ b/common/app/pixy_control.h @@ -5,6 +5,8 @@ #ifndef _CONTROL_H_ #define _CONTROL_H_ +#include + void int_init(void); uint16_t pixy_PID(uint16_t x, uint16_t w); uint16_t pixy_PI(uint16_t x, uint16_t w); diff --git a/common/app/screen_tracking.c b/common/app/screen_tracking.c index a1066d2..fd12ada 100644 --- a/common/app/screen_tracking.c +++ b/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 From a04cda9fc203cbff8588bd43da8899bb03c2018a Mon Sep 17 00:00:00 2001 From: id101010 Date: Sat, 6 Jun 2015 18:51:57 +0200 Subject: [PATCH 3/3] Refactured comments and implemented a bugfix for the PID controller --- common/app/pixy_control.c | 60 +++++++++++++++---------------- common/app/pixy_control.h | 4 +-- common/app/screen_tracking.c | 4 +-- discovery/src/ll_tft.c | 68 ++++++++++++++++++++++-------------- 4 files changed, 76 insertions(+), 60 deletions(-) diff --git a/common/app/pixy_control.c b/common/app/pixy_control.c index a6b240e..aee56db 100644 --- a/common/app/pixy_control.c +++ b/common/app/pixy_control.c @@ -21,39 +21,12 @@ #define REG_PID_KD (0.01f) #define REG_PID_TA (0.02f) -#define REG_PI_KP 1 -#define REG_PI_KI 1 -#define REG_PI_TA 1 - void int_init(void){ // TODO Init ports and outputs if needed. } -// PI controller implementatino -uint16_t pixy_PI(uint16_t x, uint16_t w) -{ - uint16_t y = 0; - float e = 0; - static float esum; - - // Calculate controller offset - e = x - w; - - //----PID-control------------------------------------------------- - // Integrate and check boundaries if necassary - esum = esum + e; - //esum = (esum < -400) ? -400 : esum; - //esum = (esum > 400) ? 400 : esum; - - // PI controller equation - y = ( REG_PI_KP * e ) + ( REG_PI_KI * REG_PI_TA * esum); - //---------------------------------------------------------------- - - return y; -} - -// PID controller implementation -uint16_t pixy_PID(uint16_t x, uint16_t w) +// PID controller implementatoin for the y-axis +uint16_t pixy_PID_Y(uint16_t x, uint16_t w) { float e; static float esum; @@ -66,7 +39,34 @@ uint16_t pixy_PID(uint16_t x, uint16_t w) //----PID-control------------------------------------------------------------------------- esum = esum + e; // add e to the current sum - esum = (esum > 65535) ? 65535 : esum; // check upper boundary and limit size + 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 diff --git a/common/app/pixy_control.h b/common/app/pixy_control.h index 47112a2..8b8e47b 100644 --- a/common/app/pixy_control.h +++ b/common/app/pixy_control.h @@ -8,7 +8,7 @@ #include void int_init(void); -uint16_t pixy_PID(uint16_t x, uint16_t w); -uint16_t pixy_PI(uint16_t x, uint16_t w); +uint16_t pixy_PID_Y(uint16_t x, uint16_t w); +uint16_t pixy_PID_X(uint16_t x, uint16_t w); #endif diff --git a/common/app/screen_tracking.c b/common/app/screen_tracking.c index fd12ada..c6699d7 100644 --- a/common/app/screen_tracking.c +++ b/common/app/screen_tracking.c @@ -103,8 +103,8 @@ void tracking_our_update(void* tracking_config, struct Block* blocks, int num_bl 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 + 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 diff --git a/discovery/src/ll_tft.c b/discovery/src/ll_tft.c index 37037b2..aac5b2f 100644 --- a/discovery/src/ll_tft.c +++ b/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,61 +359,69 @@ 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){ - GPIOB->BSRRH = GPIO_Pin_0; - } else { - GPIOB->BSRRL = GPIO_Pin_0; + if(state){ // if state is true + GPIOB->BSRRH = GPIO_Pin_0; // set the backlight output + } else { // else + GPIOB->BSRRL = GPIO_Pin_0; // reset the backlight } } +// Port operations on the screen RS PIN static void tft_reset(bool state) { - if(state){ - GPIOB->BSRRH = GPIO_Pin_0; - } else { - GPIOB->BSRRL = GPIO_Pin_0; + if(state){ // if state is ture + GPIOB->BSRRH = GPIO_Pin_0; // Set the reset pin + } else { // else + 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_RAM = reg_value; + TFT_REG = reg_adr; // set adress + 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; - return TFT_RAM; + TFT_REG = reg_adr; // set adress + 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); - end = ((yend & 0x00FF) << 8); - ystart_end = (start | end); + start = (ystart & 0x00FF); // Start adress of the window + end = ((yend & 0x00FF) << 8); // End adress of the window + ystart_end = (start | end); // Calculate y endpoint - tft_write_reg(TFT_SSD1289_REG_44, ystart_end); - tft_write_reg(TFT_SSD1289_REG_45, 319-xend); - tft_write_reg(TFT_SSD1289_REG_46, 319-xstart); + tft_write_reg(TFT_SSD1289_REG_44, ystart_end); // Send y size + tft_write_reg(TFT_SSD1289_REG_45, 319-xend); // Send x start + tft_write_reg(TFT_SSD1289_REG_46, 319-xstart); // Send x end } +// Reset a Window void tft_reset_window() { - tft_write_reg(0x44,239<<8); - tft_write_reg(0x45,0); - tft_write_reg(0x46,319); + // Commands according to the datasheet + tft_write_reg(0x44, 239 << 8); + tft_write_reg(0x45, 0); + tft_write_reg(0x46, 319); } /* * ---------------------- 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(y1==y2){ @@ -497,12 +510,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_RAM = color; + tft_set_cursor(x,y); // Set the cursor position + 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; @@ -537,6 +552,7 @@ void ll_tft_draw_rectangle(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, u tft_write_reg(0x11,0x6018); // Set adresspointer direction normal again } +// 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;