aaron/discoverpixy
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Refactured comments and implemented a bugfix for the PID controller

Browse Source
This commit is contained in:
id101010
2015-06-06 18:51:57 +02:00
parent 8c264c237a
commit a04cda9fc2
4 changed files with 76 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
common/app/pixy_control.c
View File

@@ -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

4
common/app/pixy_control.h
View File

@@ -8,7 +8,7 @@
#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);
uint16_t pixy_PID_Y(uint16_t x, uint16_t w);
uint16_t pixy_PID_X(uint16_t x, uint16_t w);
#endif

4
common/app/screen_tracking.c
View File

@@ -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