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

Updated fileheaders and styled files using astyle.

Browse Source
This commit is contained in:
t-moe
2015-06-08 11:00:52 +02:00
parent 7e61a129a3
commit fceb2d15b2
63 changed files with 6045 additions and 4691 deletions
Download Patch File Download Diff File Expand all files Collapse all files

489
common/app/screen_tracking.c
View File

@@ -1,3 +1,22 @@
/**************************************************************************************************************************************
* Project: discoverpixy
* Website: https://github.com/t-moe/discoverpixy
* Authors: Aaron Schmocker, Timo Lang
* Institution: BFH Bern University of Applied Sciences
* File: common/app/screen_tracking.c
*
* Version History:
* Date Autor Email SHA Changes
* 2015-05-16 timolang@gmail.com e46314b Added Tracking Screen and implemented "Reference Tracking" and "Color Region Selection"
* 2015-05-25 timolang@gmail.com 8088014 Updated Tracking Screen so that the implementations are separated into different method groups.
* 2015-06-06 aaron@duckpond.ch 8c264c2 Comment refactoring, updated PID values
* 2015-06-06 aaron@duckpond.ch a04cda9 Refactured comments and implemented a bugfix for the PID controller
* 2015-06-07 aaron@duckpond.ch 802d3df Fixed pid controller and refactored code
* 2015-06-07 aaron@duckpond.ch 3d98ca9 Minor changes
* 2015-06-07 timolang@gmail.com c87220d Renamed pixy_helper to pixy_frame. Updated docu of appliaction. added doxygen comments to pixy_{frame,control}.h
*
**************************************************************************************************************************************/
#include "screen_tracking.h"
#include "pixy_control.h"
#include "button.h"
@@ -14,14 +33,16 @@ static CHECKBOX_STRUCT c_frame_toggle; //Checkbox to toggle video data on/off
static TOUCH_AREA_STRUCT a_area; //Touch area for the color region selection
//Callback for when the user presses the "back" button
static void b_back_cb(void* button) {
gui_screen_back(); //navigate back to the previous screen
static void b_back_cb(void* button)
{
gui_screen_back(); //navigate back to the previous screen
}
static volatile bool frame_visible = false; //Whether or not the video data should be displayed
static void c_frame_toggle_cb(void *checkbox, bool checked) {
frame_visible=checked; //Set the visibility of the frame to the checked state of the checkbox
//Frame will be drawn in the main loop below
static void c_frame_toggle_cb(void* checkbox, bool checked)
{
frame_visible = checked; //Set the visibility of the frame to the checked state of the checkbox
//Frame will be drawn in the main loop below
}
static enum {detecting, init, tracking, preselecting, abortselecting, selecting, selected, error} state; //Current state of the screen state machine
@@ -31,12 +52,13 @@ static POINT_STRUCT point2; //End point of the rectangle selected by the user (c
static bool point1_valid; //Whether or not we have a valid first point
//Callback for when the user presses the "select color" button
static void b_select_cb(void* button) {
if(state==selecting) { //we're currently selecting a color region
state = abortselecting; //Abort selecting!!
} else if (state==tracking) { //we're currently watching the tracking
state = preselecting; //start selecting
}
static void b_select_cb(void* button)
{
if (state == selecting) { //we're currently selecting a color region
state = abortselecting; //Abort selecting!!
} else if (state == tracking) { //we're currently watching the tracking
state = preselecting; //start selecting
}
}
//Video Region properties
@@ -50,34 +72,38 @@ static void b_select_cb(void* button) {
//Callback for when the user touches the frame area to select a color region.
//Note: It doesn't matter in which direction the user draws the rectangle, we'll normalize the coordinates later
static void touchCB(void* touchArea, TOUCH_ACTION triggeredAction) {
POINT_STRUCT p = touch_get_last_point();
switch(triggeredAction) {
case PEN_DOWN: //The user just put down the pen
point1.x = p.x-FRAME_START_X; //Calculate x-Coordinate relative to frame start
point1.y = p.y-FRAME_START_Y; //Calculate y-Coordinate relative to frame start
point1_valid= true; //The point1 is now valid
break;
case PEN_UP: //The user took the pen away
if(point1_valid) { //only execute if point1 is valid
point2.x = p.x-FRAME_START_X; //Calculate x-Coordinate relative to frame start
point2.y = p.y-FRAME_START_Y; //Calculate y-Coordinate relative to frame start
state = selected;
}
break;
}
static void touchCB(void* touchArea, TOUCH_ACTION triggeredAction)
{
POINT_STRUCT p = touch_get_last_point();
switch (triggeredAction) {
case PEN_DOWN: //The user just put down the pen
point1.x = p.x - FRAME_START_X; //Calculate x-Coordinate relative to frame start
point1.y = p.y - FRAME_START_Y; //Calculate y-Coordinate relative to frame start
point1_valid = true; //The point1 is now valid
break;
case PEN_UP: //The user took the pen away
if (point1_valid) { //only execute if point1 is valid
point2.x = p.x - FRAME_START_X; //Calculate x-Coordinate relative to frame start
point2.y = p.y - FRAME_START_Y; //Calculate y-Coordinate relative to frame start
state = selected;
}
break;
}
}
//Prototype for tracking start/stop methods
typedef void (*TRACKING_VOID_CALLBACK)(void* tracking_config);
//Prototype for tracking update method
typedef void (*TRACKING_BLOCK_CALLBACK)(void* tracking_config, struct Block* blocks, int num_blocks );
typedef void (*TRACKING_BLOCK_CALLBACK)(void* tracking_config, struct Block* blocks, int num_blocks);
//Structure to save callbacks and settings of a tracking implementation
typedef struct {
TRACKING_VOID_CALLBACK start;
TRACKING_VOID_CALLBACK stop;
TRACKING_BLOCK_CALLBACK update;
TRACKING_VOID_CALLBACK start;
TRACKING_VOID_CALLBACK stop;
TRACKING_BLOCK_CALLBACK update;
} TRACKING_CONFIG_STRUCT;
//Methods for our tracking implementation ahead
@@ -85,45 +111,48 @@ 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;
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); // set default
pixy_rcs_set_position(0, servo_x); // set default
pixy_rcs_set_position(1, servo_y); // set default
return_value = pixy_command("runprog", INT8(0), END_OUT_ARGS, &response, END_IN_ARGS);
return_value = pixy_command("runprog", INT8(0), END_OUT_ARGS, &response, END_IN_ARGS);
}
//Method/Callback to stop our tracking
void tracking_our_stop(void* tracking_config) {
//Stop pixy's data send programm
int32_t response;
int return_value;
return_value = pixy_command("stop", END_OUT_ARGS, &response, END_IN_ARGS);
void tracking_our_stop(void* tracking_config)
{
//Stop pixy's data send programm
int32_t response;
int return_value;
return_value = pixy_command("stop", END_OUT_ARGS, &response, END_IN_ARGS);
}
//Method/Callback to calculate one step of our tracking
void tracking_our_update(void* tracking_config, struct Block* blocks, int num_blocks) {
void tracking_our_update(void* tracking_config, struct Block* blocks, int num_blocks)
{
if(num_blocks <= 0){ // Check if there are blocks available
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 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
@@ -136,245 +165,257 @@ void tracking_our_update(void* tracking_config, struct Block* blocks, int num_bl
//Variable which stores all the callbacks and settings for our tracking implementation
static TRACKING_CONFIG_STRUCT tracking_our = {
tracking_our_start,
tracking_our_stop,
tracking_our_update
tracking_our_start,
tracking_our_stop,
tracking_our_update
};
//Methods for reference tracking implementation ahead
//Method/Callback to start reference tracking
void tracking_reference_start(void* tracking_config) {
//Run reference tracking
int32_t response;
int return_value;
return_value = pixy_command("runprog", INT8(2), END_OUT_ARGS, &response, END_IN_ARGS);
void tracking_reference_start(void* tracking_config)
{
//Run reference tracking
int32_t response;
int return_value;
return_value = pixy_command("runprog", INT8(2), END_OUT_ARGS, &response, END_IN_ARGS);
}
//Method/Callback to stop reference tracking
void tracking_reference_stop(void* tracking_config) {
//Stop reference tracking
int32_t response;
int return_value;
return_value = pixy_command("stop", END_OUT_ARGS, &response, END_IN_ARGS);
void tracking_reference_stop(void* tracking_config)
{
//Stop reference tracking
int32_t response;
int return_value;
return_value = pixy_command("stop", END_OUT_ARGS, &response, END_IN_ARGS);
}
//Method/Callback to calculate one step of the reference tracking
void tracking_reference_update(void* tracking_config, struct Block* blocks, int num_blocks) {
//Nothing to do here. Pixy does it all.
void tracking_reference_update(void* tracking_config, struct Block* blocks, int num_blocks)
{
//Nothing to do here. Pixy does it all.
}
//Variable which stores all the callbacks and settings for the reference tracking implementation
static TRACKING_CONFIG_STRUCT tracking_reference = {
tracking_reference_start,
tracking_reference_stop,
tracking_reference_update
tracking_reference_start,
tracking_reference_stop,
tracking_reference_update
};
//Pointer to the currently active tracking implementation. See also tracking_set_mode
static TRACKING_CONFIG_STRUCT* tracking_current;
//Method to set the current tracking implementation. This function is exported and should be called before getting the screen
void tracking_set_mode(enum Tracking_Implementation impl) {
//Depending on the enum value let tracking_current point to a different setting/callback structure
switch(impl) {
case OUR_TRACKING:
tracking_current = &tracking_our;
break;
case REFERENCE_TRACKING:
tracking_current = &tracking_reference;
break;
default:
tracking_current=NULL;
break;
}
void tracking_set_mode(enum Tracking_Implementation impl)
{
//Depending on the enum value let tracking_current point to a different setting/callback structure
switch (impl) {
case OUR_TRACKING:
tracking_current = &tracking_our;
break;
case REFERENCE_TRACKING:
tracking_current = &tracking_reference;
break;
default:
tracking_current = NULL;
break;
}
}
//Callback for when the screen is entered/loaded
static void enter(void* screen) {
tft_clear(WHITE);
//"Back" button
b_back.base.x1=5; //Start X of Button
b_back.base.y1=5; //Start Y of Button
b_back.base.x2=AUTO; //Auto Calculate X2 with String Width
b_back.base.y2=AUTO; //Auto Calculate Y2 with String Height
b_back.txtcolor=WHITE; //Set foreground color
b_back.bgcolor=HEX(0xAE1010); //Set background color (Don't take 255 or 0 on at least one channel, to make shadows possible)
b_back.font=0; //Select Font
b_back.text="Back"; //Set Text (For formatted strings take sprintf)
b_back.callback=b_back_cb; //Call b_back_cb as Callback
gui_button_add(&b_back); //Register Button (and run the callback from now on)
static void enter(void* screen)
{
tft_clear(WHITE);
//"Back" button
b_back.base.x1 = 5; //Start X of Button
b_back.base.y1 = 5; //Start Y of Button
b_back.base.x2 = AUTO; //Auto Calculate X2 with String Width
b_back.base.y2 = AUTO; //Auto Calculate Y2 with String Height
b_back.txtcolor = WHITE; //Set foreground color
b_back.bgcolor = HEX(0xAE1010); //Set background color (Don't take 255 or 0 on at least one channel, to make shadows possible)
b_back.font = 0; //Select Font
b_back.text = "Back"; //Set Text (For formatted strings take sprintf)
b_back.callback = b_back_cb; //Call b_back_cb as Callback
gui_button_add(&b_back); //Register Button (and run the callback from now on)
//"Select color" button
b_select.base.x1=150;
b_select.base.y1=5;
b_select.base.x2=AUTO;
b_select.base.y2=AUTO;
b_select.txtcolor=WHITE;
b_select.bgcolor=HEX(0xAE1010);
b_select.font=0;
b_select.text="Select Color";
b_select.callback=b_select_cb;
gui_button_add(&b_select);
//"Select color" button
b_select.base.x1 = 150;
b_select.base.y1 = 5;
b_select.base.x2 = AUTO;
b_select.base.y2 = AUTO;
b_select.txtcolor = WHITE;
b_select.bgcolor = HEX(0xAE1010);
b_select.font = 0;
b_select.text = "Select Color";
b_select.callback = b_select_cb;
gui_button_add(&b_select);
//"Frame visible" checkbox
c_frame_toggle.base.x1 = 50;
c_frame_toggle.base.x2 = 50+16;
c_frame_toggle.base.y1 = 5;
c_frame_toggle.base.y2 = 5+16;
c_frame_toggle.checked = frame_visible;
c_frame_toggle.fgcolor = CHECKBOX_WIN_FG_COLOR;
c_frame_toggle.callback = c_frame_toggle_cb;
gui_checkbox_add(&c_frame_toggle);
tft_print_line(73,8,BLACK,TRANSPARENT,0,"Show Video");
//"Frame visible" checkbox
c_frame_toggle.base.x1 = 50;
c_frame_toggle.base.x2 = 50 + 16;
c_frame_toggle.base.y1 = 5;
c_frame_toggle.base.y2 = 5 + 16;
c_frame_toggle.checked = frame_visible;
c_frame_toggle.fgcolor = CHECKBOX_WIN_FG_COLOR;
c_frame_toggle.callback = c_frame_toggle_cb;
gui_checkbox_add(&c_frame_toggle);
tft_print_line(73, 8, BLACK, TRANSPARENT, 0, "Show Video");
//Area to select a "color region"
a_area.hookedActions = PEN_DOWN | PEN_UP;
a_area.x1 = FRAME_START_X;
a_area.y1 = FRAME_START_Y;
a_area.x2 = FRAME_END_X;
a_area.y2 = FRAME_END_Y;
a_area.callback = touchCB;
//Area to select a "color region"
a_area.hookedActions = PEN_DOWN | PEN_UP;
a_area.x1 = FRAME_START_X;
a_area.y1 = FRAME_START_Y;
a_area.x2 = FRAME_END_X;
a_area.y2 = FRAME_END_Y;
a_area.callback = touchCB;
//Do not register it here, we do that later
if(tracking_current==NULL) {
state = error;
} else {
state = detecting; //Start with the detecting state
}
if (tracking_current == NULL) {
state = error;
} else {
state = detecting; //Start with the detecting state
}
}
//Callback for when the screen is left/unloaded
static void leave(void* screen) {
//Remove buttons and checkbox
gui_button_remove(&b_back);
gui_button_remove(&b_select);
gui_checkbox_remove(&c_frame_toggle);
static void leave(void* screen)
{
//Remove buttons and checkbox
gui_button_remove(&b_back);
gui_button_remove(&b_select);
gui_checkbox_remove(&c_frame_toggle);
if(state==selecting) { //the user left the screen in the "selecting" phase
touch_unregister_area(&a_area); //remove the touch area
}
if (state == selecting) { //the user left the screen in the "selecting" phase
touch_unregister_area(&a_area); //remove the touch area
}
if(state==tracking) { //the user left the screen in the "tracking" phase
tracking_current->stop(tracking_current); //stop tracking
pixy_led_set_RGB(0,0,0);
}
if (state == tracking) { //the user left the screen in the "tracking" phase
tracking_current->stop(tracking_current); //stop tracking
pixy_led_set_RGB(0, 0, 0);
}
}
//Callback for when the screen should be updated
//This is the main loop of the screen. This method will be called repeatedly
static void update(void* screen) {
switch(state) {
case detecting: //Detecting State: Where we try to connect to the pixy
if(pixy_init()==0) { //Pixy connection ok
state = init; //Go to next state
}
break;
static void update(void* screen)
{
switch (state) {
case detecting: //Detecting State: Where we try to connect to the pixy
if (pixy_init() == 0) { //Pixy connection ok
state = init; //Go to next state
}
case init: //Init State: Where we start the tracking
tracking_current->start(tracking_current);
state=tracking;
break;
break;
case tracking: //Tracking state: Where we render the frame and the tracked objects
pixy_service(); //Receive events (e.g. block-data) from pixy
case init: //Init State: Where we start the tracking
tracking_current->start(tracking_current);
state = tracking;
break;
if(pixy_blocks_are_new()) { //There are new blocks available
if(frame_visible) { //If the user want's us to draw the video data
pixy_render_full_frame(FRAME_START_X,FRAME_START_Y);
} else { //the user want's a colored background
tft_fill_rectangle(FRAME_START_X,FRAME_START_Y,FRAME_END_X,FRAME_END_Y,RGB(200,200,200));
}
case tracking: //Tracking state: Where we render the frame and the tracked objects
pixy_service(); //Receive events (e.g. block-data) from pixy
#define BLOCK_BUFFER_SIZE 5 //The maximum amount of blocks that we want to receive
struct Block blocks[BLOCK_BUFFER_SIZE]; //Storage to receive blocks from pixy
int blocks_received= pixy_get_blocks(BLOCK_BUFFER_SIZE,blocks); //Try to receive up to BLOCK_BUFFER_SIZE Blocks from pixy
if (pixy_blocks_are_new()) { //There are new blocks available
if (frame_visible) { //If the user want's us to draw the video data
pixy_render_full_frame(FRAME_START_X, FRAME_START_Y);
} else { //the user want's a colored background
tft_fill_rectangle(FRAME_START_X, FRAME_START_Y, FRAME_END_X, FRAME_END_Y, RGB(200, 200, 200));
}
if(blocks_received>=0) { //block receiving ok
tracking_current->update(tracking_current,blocks,blocks_received); //apply tracking
#define BLOCK_BUFFER_SIZE 5 //The maximum amount of blocks that we want to receive
struct Block blocks[BLOCK_BUFFER_SIZE]; //Storage to receive blocks from pixy
int blocks_received = pixy_get_blocks(BLOCK_BUFFER_SIZE, blocks); //Try to receive up to BLOCK_BUFFER_SIZE Blocks from pixy
//Draw blocks
for(int i=0; i<blocks_received; i++) { //for each received block
struct Block* block = &(blocks[i]);
//block.x and block.y are the center coordinates of the object relative to the camera origin.
uint16_t x = block->x-1+FRAME_START_X -block->width/2; //Calculate x-Coordinate on the display
uint16_t y = block->y-1+FRAME_START_Y -block->height/2; //Calculate y-Coordinate on the display
tft_draw_rectangle(x,y,x+block->width-1, y+block->height-1,WHITE); //Draw a white rectangle
}
}
}
break;
if (blocks_received >= 0) { //block receiving ok
tracking_current->update(tracking_current, blocks, blocks_received); //apply tracking
case preselecting: //Pre-Selecting State: Where we set up the color region selection
{
tracking_current->stop(tracking_current); //Stop tracking
//Draw blocks
for (int i = 0; i < blocks_received; i++) { //for each received block
struct Block* block = &(blocks[i]);
//block.x and block.y are the center coordinates of the object relative to the camera origin.
uint16_t x = block->x - 1 + FRAME_START_X - block->width / 2; //Calculate x-Coordinate on the display
uint16_t y = block->y - 1 + FRAME_START_Y - block->height / 2; //Calculate y-Coordinate on the display
tft_draw_rectangle(x, y, x + block->width - 1, y + block->height - 1, WHITE); //Draw a white rectangle
}
}
}
pixy_render_full_frame(FRAME_START_X,FRAME_START_Y); //Render one frame
break;
touch_register_area(&a_area); //Register touch area and receive events from now on
point1_valid=false; //we start with an invalid point1
case preselecting: { //Pre-Selecting State: Where we set up the color region selection
tracking_current->stop(tracking_current); //Stop tracking
b_select.text="Abort"; //Change the button text to "Abort"
gui_button_redraw(&b_select); //redraw button
pixy_render_full_frame(FRAME_START_X, FRAME_START_Y); //Render one frame
state = selecting; //The user can now select a region
}
break;
touch_register_area(&a_area); //Register touch area and receive events from now on
point1_valid = false; //we start with an invalid point1
case selected: //Selected State: Where we send the users selection to pixy
{
//Ensure that (x1,y1) represent the top-left point and (x2,y2) the bottom-right.
unsigned int tmp;
if(point1.x > point2.x){
tmp = point1.x;
point1.x = point2.x;
point2.x = tmp;
}
b_select.text = "Abort"; //Change the button text to "Abort"
gui_button_redraw(&b_select); //redraw button
if(point1.y > point2.y){
tmp = point1.y;
point1.y = point2.y;
point2.y = tmp;
}
//Send pixy the selected region
pixy_cc_set_region(1,point1.x,point1.y,point2.x-point1.x,point2.y-point1.y);
}
//no break here: We want the following code to be executed as well
state = selecting; //The user can now select a region
}
break;
case abortselecting: //Abort-Selecting State: Where we deinitialize the stuff we used for region selection
{
touch_unregister_area(&a_area); //Remove the touch area. We'll no longer receive touch events
case selected: { //Selected State: Where we send the users selection to pixy
//Ensure that (x1,y1) represent the top-left point and (x2,y2) the bottom-right.
unsigned int tmp;
b_select.text="Select Color"; //Change the button text back to "Select Color"
gui_button_redraw(&b_select); //redraw button
if (point1.x > point2.x) {
tmp = point1.x;
point1.x = point2.x;
point2.x = tmp;
}
tracking_current->start(tracking_current); //Start tracking again
state=tracking;
}
break;
if (point1.y > point2.y) {
tmp = point1.y;
point1.y = point2.y;
point2.y = tmp;
}
case selecting: //Selecting State: Where we wait on the user to select a color region
pixy_service(); //receive pixy events
//wait on user to select the image area
break;
//Send pixy the selected region
pixy_cc_set_region(1, point1.x, point1.y, point2.x - point1.x, point2.y - point1.y);
}
case error: //Error State: Where we show an error message and leave the user no other choice than to click the backbutton
//wait on user to click the back button
break;
}
//no break here: We want the following code to be executed as well
case abortselecting: { //Abort-Selecting State: Where we deinitialize the stuff we used for region selection
touch_unregister_area(&a_area); //Remove the touch area. We'll no longer receive touch events
b_select.text = "Select Color"; //Change the button text back to "Select Color"
gui_button_redraw(&b_select); //redraw button
tracking_current->start(tracking_current); //Start tracking again
state = tracking;
}
break;
case selecting: //Selecting State: Where we wait on the user to select a color region
pixy_service(); //receive pixy events
//wait on user to select the image area
break;
case error: //Error State: Where we show an error message and leave the user no other choice than to click the backbutton
//wait on user to click the back button
break;
}
}
//Declare screen callbacks
static SCREEN_STRUCT screen = {
enter,
leave,
update
enter,
leave,
update
};
SCREEN_STRUCT* get_screen_tracking() {
return &screen;
SCREEN_STRUCT* get_screen_tracking()
{
return &screen;
}