diff --git a/common/app/pixy_helper.c b/common/app/pixy_helper.c index 71efb4d..d137d71 100644 --- a/common/app/pixy_helper.c +++ b/common/app/pixy_helper.c @@ -219,5 +219,23 @@ static int saveBA81(FILE_HANDLE* handle, uint16_t width, uint16_t height, uint32 frame++; } return 0; - } + + +int pixy_cc_set_region(uint8_t signum, uint16_t xoffset, uint16_t yoffset, uint16_t width, uint16_t height) { + int32_t response; + + int return_value = pixy_command("cc_setSigRegion", // String id for remote procedure + INT32(0), // type = normal color code + INT8(signum), + INT16(xoffset), // xoffset + INT16(yoffset), // yoffset + INT16(width), // width + INT16(height), // height + END_OUT_ARGS, // separator + &response, // pointer to mem address for return value + END_IN_ARGS); + return return_value; +} + + diff --git a/common/app/pixy_helper.h b/common/app/pixy_helper.h index 282133b..7f7cd40 100644 --- a/common/app/pixy_helper.h +++ b/common/app/pixy_helper.h @@ -45,5 +45,15 @@ int pixy_save_full_frame(FILE_HANDLE* handle); */ int pixy_save_cropped_frame(FILE_HANDLE* handle, uint16_t xoffset, uint16_t yoffset, uint16_t width, uint16_t height); +/** + * Sets the color signature to the color in the selected region of the frame + * @param signum the color signature number (1..7) + * @param xoffset The x-Coordinate of the topleft point of the region + * @param yoffset The y-Coordinate of the topleft point of the region + * @param width The width of the region + * @param height The height of the region + * @return 0 on success, otherwise the errorcode from pixy + */ +int pixy_cc_set_region(uint8_t signum, uint16_t xoffset, uint16_t yoffset, uint16_t width, uint16_t height); #endif /* PIXY_HELPER_H */ diff --git a/common/app/screen_main.c b/common/app/screen_main.c index bdf089d..1ff52d6 100644 --- a/common/app/screen_main.c +++ b/common/app/screen_main.c @@ -3,6 +3,7 @@ #include "screen_pixytest.h" #include "screen_filetest.h" #include "screen_photomode.h" +#include "screen_tracking.h" #include "button.h" #include "tft.h" #include "filesystem.h" @@ -17,11 +18,13 @@ BUTTON_STRUCT b_photo_mode; static void b_our_tracking_cb(void* button) { - + tracking_set_mode(OUR_TRACKING); + gui_screen_navigate(get_screen_tracking()); } static void b_ref_tracking_cb(void* button) { - + tracking_set_mode(REFERENCE_TRACKING); + gui_screen_navigate(get_screen_tracking()); } static void b_photo_mode_cb(void* button) { diff --git a/common/app/screen_tracking.c b/common/app/screen_tracking.c new file mode 100644 index 0000000..e99c866 --- /dev/null +++ b/common/app/screen_tracking.c @@ -0,0 +1,257 @@ +#include "screen_tracking.h" +#include "button.h" +#include "checkbox.h" +#include "tft.h" +#include "touch.h" +#include "pixy.h" +#include "system.h" +#include "pixy_helper.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} 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; + } +} + +void tracking_set_mode(enum Tracking_Implementation impl) { + //not used yet +} + +//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 + + 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 + //Stop reference tracking + int32_t response; + int return_value; + return_value = pixy_command("stop", END_OUT_ARGS, &response, END_IN_ARGS); + } +} + +//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 + { + //Run reference tracking + int32_t response; + int return_value; + return_value = pixy_command("runprog", INT8(2), END_OUT_ARGS, &response, END_IN_ARGS); + 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)); + } + + struct Block block1; //Storage to receive one block from pixy + if(pixy_get_blocks(1,&block1)==1) { //Receiving one block succeeded + //block1.x and block1.y are the center coordinates of the object relative to the camera origin. + uint16_t x = block1.x-1+FRAME_START_X -block1.width/2; //Calculate x-Coordinate on the display + uint16_t y = block1.y-1+FRAME_START_Y -block1.height/2; //Calculate y-Coordinate on the display + tft_draw_rectangle(x,y,x+block1.width-1, y+block1.height-1,WHITE); //Draw a white rectangle + } + } + break; + + case preselecting: //Pre-Selecting State: Where we set up the color region selection + { + //Stop reference tracking + int32_t response; + int return_value; + return_value = pixy_command("stop", END_OUT_ARGS, &response, END_IN_ARGS); + + 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 + + //Run reference tracking again + int32_t response; + int return_value; + return_value = pixy_command("runprog", INT8(2), END_OUT_ARGS, &response, END_IN_ARGS); + 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; + } +} + +//Declare screen callbacks +static SCREEN_STRUCT screen = { + enter, + leave, + update +}; + + +SCREEN_STRUCT* get_screen_tracking() { + return &screen; +} diff --git a/common/app/screen_tracking.h b/common/app/screen_tracking.h new file mode 100644 index 0000000..5a2dbde --- /dev/null +++ b/common/app/screen_tracking.h @@ -0,0 +1,37 @@ +#include "screen.h" + +/** + * @addtogroup screens + */ +/*@{*/ + +/** + * @defgroup tracking Tracking (Screen) + * The Tracking-Screen shows the object-tracking and allows some configuration + */ +/*@{*/ + + +/** + * Enum which contains the available tracking implementations + */ +enum Tracking_Implementation { + OUR_TRACKING, //!< Our own tracking PID implementation + REFERENCE_TRACKING//!< Pixy's internal tracking implementation +}; + +/** + * Sets the current Mode/Tracking Implementation. Call this before using the screen obtained by get_screen_tracking() + * @param impl The new mode + */ +void tracking_set_mode(enum Tracking_Implementation impl); + +/** + * Returns a pointer to the tracking screen + * \sa gui_screen_navigate + * @return + */ +SCREEN_STRUCT* get_screen_tracking(); + +/*@}*/ +/*@}*/