From 802d3df37353a209b2cd8c706548713215a11035 Mon Sep 17 00:00:00 2001 From: id101010 Date: Sun, 7 Jun 2015 13:13:49 +0200 Subject: [PATCH 1/2] Fixed pid controller and refactored code --- common/app/pixy_control.c | 52 +++++++++++++++--------------------- common/app/pixy_control.h | 4 +-- common/app/screen_tracking.c | 35 +++++++++++++++++++++--- 3 files changed, 55 insertions(+), 36 deletions(-) diff --git a/common/app/pixy_control.c b/common/app/pixy_control.c index aee56db..8e0d8e9 100644 --- a/common/app/pixy_control.c +++ b/common/app/pixy_control.c @@ -16,65 +16,57 @@ #include // 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) +#define REG_PID_KP (0.5f) +#define REG_PID_KI (0.001f) +#define REG_PID_KD (0.001f) +#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; - uint16_t y; + static float eold; + float y; - // Calculate controller offset - e = x - w; + e = (float)(x - w); // calculate the controller offset //----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; + y += REG_PID_KP * e; // add the proportional part to the output + 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 //---------------------------------------------------------------------------------------- - // save old value - ealt = e; + eold = e; // save the previous value - 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; - uint16_t y; + static float eold; + float y; - // Calculate controller offset - e = x - w; + e = (float)(x - w); // calculate the controller offset //----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; + y += REG_PID_KP * e; // add the proportional part to the output + 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 //---------------------------------------------------------------------------------------- - // save old value - ealt = e; + eold = e; // save the previous value - return y; + return (int16_t) y; } diff --git a/common/app/pixy_control.h b/common/app/pixy_control.h index 8b8e47b..3eb5f55 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_Y(uint16_t x, uint16_t w); -uint16_t pixy_PID_X(uint16_t x, uint16_t w); +int16_t pixy_PID_Y(int16_t x, int16_t w); +int16_t pixy_PID_X(int16_t x, int16_t w); #endif diff --git a/common/app/screen_tracking.c b/common/app/screen_tracking.c index c6699d7..1a3daf5 100644 --- a/common/app/screen_tracking.c +++ b/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) { - 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 + 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 + + int16_t xset = 0; + 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 From 3d98ca93bcc4f946636ee70ca2914edd9a7da44f Mon Sep 17 00:00:00 2001 From: id101010 Date: Sun, 7 Jun 2015 13:41:07 +0200 Subject: [PATCH 2/2] Minor changes --- common/app/pixy_control.c | 20 ++++++++++---------- common/app/screen_tracking.c | 4 ++-- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/common/app/pixy_control.c b/common/app/pixy_control.c index 8e0d8e9..5786e7a 100644 --- a/common/app/pixy_control.c +++ b/common/app/pixy_control.c @@ -28,10 +28,10 @@ void int_init(void){ // PID controller implementatoin for the y-axis int16_t pixy_PID_Y(int16_t x, int16_t w) { - float e; - static float esum; - static float eold; - float y; + float e = 0; + static float esum = 0; + static float eold = 0; + float y = 0; e = (float)(x - w); // calculate the controller offset @@ -45,16 +45,16 @@ int16_t pixy_PID_Y(int16_t x, int16_t w) eold = e; // save the previous value - return (int16_t) y; + return (int16_t)y; } // PID controller implementation for the x-axis int16_t pixy_PID_X(int16_t x, int16_t w) { - float e; - static float esum; - static float eold; - float y; + float e = 0; + static float esum = 0; + static float eold = 0; + float y = 0; e = (float)(x - w); // calculate the controller offset @@ -68,5 +68,5 @@ int16_t pixy_PID_X(int16_t x, int16_t w) eold = e; // save the previous value - return (int16_t) y; + return (int16_t)y; } diff --git a/common/app/screen_tracking.c b/common/app/screen_tracking.c index 1a3daf5..508c8b3 100644 --- a/common/app/screen_tracking.c +++ b/common/app/screen_tracking.c @@ -91,8 +91,8 @@ void tracking_our_start(void* tracking_config) { 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); // + pixy_rcs_set_position(0, servo_x); // set default + pixy_rcs_set_position(1, servo_y); // set default return_value = pixy_command("runprog", INT8(0), END_OUT_ARGS, &response, END_IN_ARGS); }