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;
}

4
common/app/pixy_control.h
View File

@@ -8,7 +8,7 @@
#include<stdint.h>
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

35
common/app/screen_tracking.c
View File

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