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

Fixed pid controller and refactored code

Browse Source
This commit is contained in:
id101010
2015-06-07 13:13:49 +02:00
parent ef467d0fee
commit 802d3df373
3 changed files with 55 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
common/app/pixy_control.c
View File

@@ -16,65 +16,57 @@
#include<stdint.h>
// 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;
}