Added Tracking Screen and implemented "Reference Tracking" and "Color Region Selection"

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

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 */

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) {

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

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();
+
+/*@}*/
+/*@}*/