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

Comment refactoring, updated PID values

Browse Source
This commit is contained in:
id101010
2015-06-06 18:19:28 +02:00
parent e018a75cd3
commit 8c264c237a
3 changed files with 25 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
common/app/pixy_control.c
View File

@@ -13,12 +13,13 @@
*
*/
#include<pixy_control.h>
#include<stdint.h>
// 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;
}