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