/************************************************************************************************************************************** * 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" #include "checkbox.h" #include "tft.h" #include "touch.h" #include "pixy.h" #include "system.h" #include "pixy_frame.h" static BUTTON_STRUCT b_back; //Button to navigate back static BUTTON_STRUCT b_select; //Button to start the color region selection 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 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 enum {detecting, init, tracking, preselecting, abortselecting, selecting, selected, error} state; //Current state of the screen state machine static POINT_STRUCT point1; //First point of the rectangle selected by the user (color region selection) static POINT_STRUCT point2; //End point of the rectangle selected by the user (color region selection) 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 } } //Video Region properties //The camera records with 320*200px, but we need to keep a 1px border because of color interpolation (bayer format) #define FRAME_START_X 1 //x-Coordinate of the top-left point of the frame rectangle on display #define FRAME_START_Y 41 //y-Coordinate of the top-left point of the frame rectangle on display #define FRAME_WIDTH 318 //Width of the video frame #define FRAME_HEIGHT 198 //Height of the video frame #define FRAME_END_X FRAME_START_X +FRAME_WIDTH-1 //x-Coordinate of the bottom-right point of the frame rectangle #define FRAME_END_Y FRAME_START_Y +FRAME_HEIGHT-1 //y-Coordinate of the bottom-right point of the frame rectangle //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; } } //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); //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_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); // 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); } //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); } //Method/Callback to calculate one step of our tracking void tracking_our_update(void* tracking_config, struct Block* blocks, int num_blocks) { 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 static TRACKING_CONFIG_STRUCT tracking_our = { 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); } //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); } //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. } //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 }; //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; } } //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) //"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"); //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 } } //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); 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); } } //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; case init: //Init State: Where we start the tracking tracking_current->start(tracking_current); state = tracking; break; case tracking: //Tracking state: Where we render the frame and the tracked objects pixy_service(); //Receive events (e.g. block-data) 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)); } #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 (blocks_received >= 0) { //block receiving ok tracking_current->update(tracking_current, blocks, blocks_received); //apply 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 } } } break; case preselecting: { //Pre-Selecting State: Where we set up the color region selection tracking_current->stop(tracking_current); //Stop tracking pixy_render_full_frame(FRAME_START_X, FRAME_START_Y); //Render one frame touch_register_area(&a_area); //Register touch area and receive events from now on point1_valid = false; //we start with an invalid point1 b_select.text = "Abort"; //Change the button text to "Abort" gui_button_redraw(&b_select); //redraw button state = selecting; //The user can now select a region } break; 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; } 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 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 }; SCREEN_STRUCT* get_screen_tracking() { return &screen; }