Context Navigation

Back to cmucam2-emulation

cmucam2-emulation: cmucam2.c

File cmucam2.c, 44.2 KB (added by agr, 2 years ago)
CMUcam2 Emulation Source

Line
1	#include <cc3.h>
2	#include <cc3_ilp.h>
3	#include <cc3_color_track.h>
4	#include <cc3_color_info.h>
5	#include <cc3_histogram.h>
6	#include <cc3_frame_diff.h>
7	#include <math.h>
8	#include <stdbool.h>
9	#include <stdio.h>
10	#include <string.h>
11	#include <ctype.h>
12	#include <stdlib.h>
13	#include <cc3_jpg.h>
14	#include <cc3_math.h>
15	#include <cc3_hsv.h>
16
17	// Uncomment line below to reverse servo direction for auto-servo and demo mode
18	#define SERVO_REVERSE_DIRECTION_PAN
19	#define SERVO_REVERSE_DIRECTION_TILT
20
21	//#define SERIAL_BAUD_RATE CC3_UART_RATE_230400
22	#define SERIAL_BAUD_RATE CC3_UART_RATE_115200
23	//#define SERIAL_BAUD_RATE CC3_UART_RATE_57600
24	//#define SERIAL_BAUD_RATE CC3_UART_RATE_38400
25	//#define SERIAL_BAUD_RATE CC3_UART_RATE_19200
26	//#define SERIAL_BAUD_RATE CC3_UART_RATE_9600
27	//#define SERIAL_BAUD_RATE CC3_UART_RATE_4800
28	//#define SERIAL_BAUD_RATE CC3_UART_RATE_2400
29	//#define SERIAL_BAUD_RATE CC3_UART_RATE_1200
30	//#define SERIAL_BAUD_RATE CC3_UART_RATE_300
31
32	#define SERVO_MIN 0
33	#define SERVO_MID 128
34	#define SERVO_MAX 255
35	// Define a jitter guard such that more than SERVO_GUARD pixels are required
36	// for the servo to move.
37
38	#define DEFAULT_COLOR 0
39	#define HSV_COLOR 1
40
41	static const unsigned int MAX_ARGS = 10;
42	static const unsigned int MAX_LINE = 128;
43
44	static const char VERSION_BANNER[] = "CMUcam2 v1.00 c6";
45
46	typedef struct {
47	uint8_t pan_step, tilt_step;
48	uint8_t pan_range_near, tilt_range_near;
49	uint8_t pan_range_far, tilt_range_far;
50	int16_t x;
51	int16_t y;
52	bool y_control;
53	bool x_control;
54	bool y_report;
55	bool x_report;
56	} cmucam2_servo_t;
57
58
59	typedef enum {
60	RESET,
61	TRACK_COLOR,
62	SEND_FRAME,
63	HI_RES,
64	FRAME_DIFF,
65	GET_VERSION,
66	GET_MEAN,
67	SET_SERVO,
68	CAMERA_REG,
69	CAMERA_POWER,
70	POLL_MODE,
71	LINE_MODE,
72	SEND_JPEG,
73	VIRTUAL_WINDOW,
74	DOWN_SAMPLE,
75	GET_HISTOGRAM,
76	TRACK_WINDOW,
77	GET_TRACK,
78	GET_WINDOW,
79	LED_0,
80	NOISE_FILTER,
81	TRACK_INVERT,
82	SERVO_MASK,
83	SERVO_PARAMETERS,
84	SERVO_OUTPUT,
85	GET_SERVO,
86	SET_INPUT,
87	GET_INPUT,
88	SET_TRACK,
89	BUF_MODE,
90	READ_FRAME,
91	OUTPUT_MASK,
92	PACKET_FILTER,
93	CONF_HISTOGRAM,
94	GET_BUTTON,
95	FRAME_DIFF_CHANNEL,
96	LOAD_FRAME,
97	RAW_MODE,
98	COLOR_SPACE,
99	HIRES_DIFF,
100	FRAME_STREAM,
101
102	RETURN, // Must be second to last
103	CMUCAM2_CMDS_COUNT // Must be last entry so array sizes are correct
104	} cmucam2_command_t;
105
106	static const char cmucam2_cmds[CMUCAM2_CMDS_COUNT][3] = {
107	[RETURN] = "",
108
109	/* Buffer Commands */
110	[BUF_MODE] = "BM",
111	[READ_FRAME] = "RF",
112
113	/* Camera Module Commands */
114	[CAMERA_REG] = "CR",
115	[CAMERA_POWER] = "CP",
116	// CT camera type
117
118	/* Data Rate Commands */
119	// DM delay mode
120	[FRAME_STREAM] = "FS",
121	[POLL_MODE] = "PM",
122	// PS packet skip
123	[RAW_MODE] = "RM",
124	[PACKET_FILTER] = "PF",
125	[OUTPUT_MASK] = "OM",
126
127	/* Servo Commands */
128	[SET_SERVO] = "SV",
129	[GET_SERVO] = "GS",
130	[SERVO_OUTPUT] = "SO",
131	[SERVO_MASK] = "SM",
132	[SERVO_PARAMETERS] = "SP",
133
134	/* Image Windowing Commands */
135	[SEND_FRAME] = "SF",
136	[DOWN_SAMPLE] = "DS",
137	[VIRTUAL_WINDOW] = "VW",
138	// FS frame stream
139	[HI_RES] = "HR",
140	[GET_WINDOW] = "GW",
141	// PD pixel difference
142
143	/* Auxiliary I/O Commands */
144	[GET_INPUT] = "GI",
145	[SET_INPUT] = "SI", // new for cmucam3
146	[GET_BUTTON] = "GB",
147	[LED_0] = "L0",
148	// L1 LED control
149
150	/* Color Tracking Commands */
151	[TRACK_COLOR] = "TC",
152	[TRACK_INVERT] = "TI",
153	[TRACK_WINDOW] = "TW",
154	[NOISE_FILTER] = "NF",
155	[LINE_MODE] = "LM",
156	[GET_TRACK] = "GT",
157	[SET_TRACK] = "ST",
158
159	/* Histogram Commands */
160	[GET_HISTOGRAM] = "GH",
161	[CONF_HISTOGRAM] = "HC",
162	// HT histogram track
163
164	/* Frame Differencing Commands */
165	[FRAME_DIFF] = "FD",
166	[LOAD_FRAME] = "LF",
167	[FRAME_DIFF_CHANNEL] = "DC",
168	[HIRES_DIFF] = "HD",
169	// MD mask difference
170	// UD upload difference
171
172	/* Color Statistics Commands */
173	[GET_MEAN] = "GM",
174
175	/* System Level Commands */
176	// SD sleep deeply
177	// SL sleep
178	[RESET] = "RS",
179	[GET_VERSION] = "GV",
180
181	[COLOR_SPACE] = "CS",
182
183	/* CMUcam3 New Commands */
184	[SEND_JPEG] = "SJ",
185	};
186
187
188	static void cmucam2_load_frame (cc3_frame_diff_pkt_t * pkt, bool buf_mode, uint8_t sw_color_space);
189	static void cmucam2_get_histogram (cc3_histogram_pkt_t * h_pkt,
190	bool poll_mode, bool buf_mode, uint8_t sw_color_space, bool quiet);
191	static void cmucam2_get_mean (cc3_color_info_pkt_t * t_pkt, bool poll_mode,
192	bool line_mode, bool buf_mode, uint8_t sw_color_space, bool quiet);
193	static void cmucam2_write_s_packet (cc3_color_info_pkt_t * pkt);
194	static void cmucam2_track_color (cc3_track_pkt_t * t_pkt,
195	bool poll_mode,
196	int8_t line_mode, bool auto_led,
197	cmucam2_servo_t * servo_settings,
198	bool buf_mode, uint8_t sw_color_space, bool quiet);
199	static void cmucam2_frame_diff (cc3_frame_diff_pkt_t * pkt,
200	bool poll_mode, bool line_mode, bool buf_mode,
201	bool auto_led, uint8_t sw_color_space, bool quiet);
202	static int32_t cmucam2_get_command (cmucam2_command_t * cmd,
203	uint32_t arg_list[]);
204	static int32_t cmucam2_get_command_raw (cmucam2_command_t * cmd,
205	uint32_t arg_list[]);
206	static void print_ACK (void);
207	static void print_NCK (void);
208	static void print_prompt (void);
209	static void print_cr (void);
210	static void cmucam2_write_t_packet (cc3_track_pkt_t * pkt,
211	cmucam2_servo_t * servo_settings);
212	static void cmucam2_write_h_packet (cc3_histogram_pkt_t * pkt);
213	static void cmucam2_send_image_direct (bool auto_led,uint8_t sw_color_space);
214
215	static void raw_print (uint8_t val);
216
217	static bool packet_filter_flag;
218	static uint8_t t_pkt_mask;
219	static uint8_t s_pkt_mask;
220
221	static bool raw_mode_output;
222	static bool raw_mode_no_confirmations;
223	static bool raw_mode_input;
224
225	int main (void)
226	{
227	cmucam2_command_t command;
228	int32_t val, n;
229	uint32_t arg_list[MAX_ARGS];
230	uint32_t start_time;
231	uint8_t sw_color_space;
232	bool error, poll_mode, auto_led, demo_mode, buf_mode,frame_stream_mode;
233	int8_t line_mode;
234	cc3_track_pkt_t t_pkt;
235	cc3_color_info_pkt_t s_pkt;
236	cc3_histogram_pkt_t h_pkt;
237	cc3_frame_diff_pkt_t fd_pkt;
238	cmucam2_servo_t servo_settings;
239
240	//cc3_filesystem_init ();
241
242	cc3_uart_init (0,
243	SERIAL_BAUD_RATE,
244	CC3_UART_MODE_8N1, CC3_UART_BINMODE_BINARY);
245	val = setvbuf (stdout, NULL, _IONBF, 0);
246
247	if (!cc3_camera_init ()) {
248	cc3_led_set_state (0, true);
249	exit (1);
250	}
251
252	servo_settings.x_control = false;
253	servo_settings.y_control = false;
254	servo_settings.x_report = false;
255	servo_settings.y_report = false;
256	demo_mode = false;
257
258	// Keep this memory in the bank for frame differencing
259	fd_pkt.previous_template = malloc (16 * 16 * sizeof (uint32_t));
260	if (fd_pkt.previous_template == NULL)
261	printf ("Malloc FD startup error!\r");
262	start_time = cc3_timer_get_current_ms ();
263
264	do {
265	if (cc3_button_get_state () == 1) {
266	// Demo Mode flag
267	demo_mode = true;
268	servo_settings.x_control = true;
269	servo_settings.y_control = true;
270	servo_settings.x_report = true;
271	servo_settings.y_report = true;
272	// Debounce Switch
273	cc3_led_set_state (0, false);
274	cc3_timer_wait_ms (500);
275	break;
276	}
277
278	} while (cc3_timer_get_current_ms () < (start_time + 2000));
279
280
281	cmucam2_start:
282	sw_color_space=DEFAULT_COLOR;
283	auto_led = true;
284	poll_mode = false;
285	frame_stream_mode = false;
286	line_mode = 0;
287	buf_mode = false;
288	packet_filter_flag = false;
289	t_pkt_mask = 0xFF;
290	s_pkt_mask = 0xFF;
291	h_pkt.bins = 28;
292	fd_pkt.coi = 1;
293	fd_pkt.template_width = 8;
294	fd_pkt.template_height = 8;
295	t_pkt.track_invert = false;
296	t_pkt.noise_filter = 0;
297
298	// set to 0 since cmucam2 appears to initialize to this
299	t_pkt.lower_bound.channel[0] = 0;
300	t_pkt.upper_bound.channel[0] = 0;
301	t_pkt.lower_bound.channel[1] = 0;
302	t_pkt.upper_bound.channel[1] = 0;
303	t_pkt.lower_bound.channel[2] = 0;
304	t_pkt.upper_bound.channel[2] = 0;
305
306	raw_mode_output = false;
307	raw_mode_no_confirmations = false;
308	raw_mode_input = false;
309
310	servo_settings.x = SERVO_MID;
311	servo_settings.y = SERVO_MID;
312	servo_settings.pan_range_far = 32;
313	servo_settings.pan_range_near = 20;
314	servo_settings.pan_step = 20;
315	servo_settings.tilt_range_far = 32;
316	servo_settings.tilt_range_near = 20;
317	servo_settings.tilt_step = 20;
318
319
320	cc3_camera_set_power_state (true);
321	cc3_camera_set_resolution (CC3_CAMERA_RESOLUTION_LOW);
322
323	printf ("%s\r", VERSION_BANNER);
324
325	cc3_gpio_set_mode (0, CC3_GPIO_MODE_SERVO);
326	cc3_gpio_set_mode (1, CC3_GPIO_MODE_SERVO);
327	cc3_gpio_set_mode (2, CC3_GPIO_MODE_SERVO);
328	cc3_gpio_set_mode (3, CC3_GPIO_MODE_SERVO);
329
330	cc3_gpio_set_servo_position (0, SERVO_MID);
331	cc3_gpio_set_servo_position (1, SERVO_MID);
332	cc3_gpio_set_servo_position (2, SERVO_MID);
333	cc3_gpio_set_servo_position (3, SERVO_MID);
334
335	cc3_pixbuf_frame_set_subsample (CC3_SUBSAMPLE_NEAREST, 2, 1);
336
337	if (demo_mode) {
338	cc3_led_set_state (0, true);
339	cc3_timer_wait_ms (5000);
340	cc3_camera_set_auto_exposure (false);
341	cc3_camera_set_auto_white_balance (false);
342	// Wait for second button press as target lock
343	while (1) {
344	cc3_led_set_state (0, true);
345	cc3_timer_wait_ms (100);
346	cc3_led_set_state (0, false);
347	cc3_timer_wait_ms (100);
348	if (cc3_button_get_state () == 1)
349	break;
350	}
351
352	}
353	while (true) {
354	cc3_channel_t old_coi;
355
356	print_prompt ();
357	error = false;
358	if (demo_mode == true) {
359	n = 0;
360	command = TRACK_WINDOW;
361	}
362	else if (raw_mode_input) {
363	n = cmucam2_get_command_raw (&command, arg_list);
364	}
365	else {
366	n = cmucam2_get_command (&command, arg_list);
367	}
368	if (n != -1) {
369	switch (command) {
370
371	case RESET:
372	if (n != 0) {
373	error = true;
374	break;
375	}
376
377	print_ACK ();
378	print_cr ();
379	goto cmucam2_start;
380	break;
381
382	case READ_FRAME:
383	if (n != 0) {
384	error = true;
385	break;
386	}
387	print_ACK ();
388	cc3_pixbuf_load ();
389	break;
390
391	case OUTPUT_MASK:
392	if (n != 2 \|\| arg_list[0] > 1) {
393	error = true;
394	break;
395	}
396	if (arg_list[0] == 0)
397	t_pkt_mask = arg_list[1];
398	if (arg_list[0] == 1)
399	s_pkt_mask = arg_list[1];
400	print_ACK ();
401	break;
402
403
404	case GET_VERSION:
405	if (n != 0) {
406	error = true;
407	break;
408	}
409
410	print_ACK ();
411	// no different in raw mode
412	printf ("%s", VERSION_BANNER);
413	break;
414
415	case LED_0:
416	if (n != 1 \|\| arg_list[0] > 2) {
417	error = true;
418	break;
419	}
420
421	print_ACK ();
422	auto_led = false;
423	if (arg_list[0] == 0)
424	cc3_led_set_state (0, false);
425	if (arg_list[0] == 1)
426	cc3_led_set_state (0, true);
427	if (arg_list[0] == 2)
428	auto_led = true;
429	break;
430
431	case BUF_MODE:
432	if (n != 1 \|\| arg_list[0] > 1) {
433	error = true;
434	break;
435	}
436
437	print_ACK ();
438	if (arg_list[0] == 1)
439	buf_mode = true;
440	else
441	buf_mode = false;
442	break;
443
444	case PACKET_FILTER:
445	if (n != 1 \|\| arg_list[0] > 1) {
446	error = true;
447	break;
448	}
449
450	print_ACK ();
451	if (arg_list[0] == 1)
452	packet_filter_flag = true;
453	else
454	packet_filter_flag = false;
455	break;
456
457	case POLL_MODE:
458	if (n != 1 \|\| arg_list[0] > 1) {
459	error = true;
460	break;
461	}
462
463	print_ACK ();
464	if (arg_list[0] == 1)
465	poll_mode = true;
466	else
467	poll_mode = false;
468	break;
469
470
471	case FRAME_STREAM:
472	if (n != 1 \|\| arg_list[0] > 1) {
473	error = true;
474	break;
475	}
476
477	print_ACK ();
478	if (arg_list[0] == 1)
479	frame_stream_mode = true;
480	else
481	frame_stream_mode = false;
482	break;
483
484	case SERVO_PARAMETERS:
485	if (n != 6) {
486	error = true;
487	break;
488	}
489
490	print_ACK ();
491	servo_settings.pan_range_far = arg_list[0];
492	servo_settings.pan_range_near = arg_list[1];
493	servo_settings.pan_step = arg_list[2];
494	servo_settings.tilt_range_far = arg_list[3];
495	servo_settings.tilt_range_near = arg_list[4];
496	servo_settings.tilt_step = arg_list[5];
497	break;
498
499	case SERVO_MASK:
500	if (n != 1) {
501	error = true;
502	break;
503	}
504
505	print_ACK ();
506	servo_settings.x_control = !!(arg_list[0] & 0x1);
507	servo_settings.y_control = !!(arg_list[0] & 0x2);
508	servo_settings.x_report = !!(arg_list[0] & 0x4);
509	servo_settings.y_report = !!(arg_list[0] & 0x8);
510	break;
511
512	case HI_RES:
513	if (n != 1) {
514	error = true;
515	break;
516	}
517
518	print_ACK ();
519	if (arg_list[0] == 1)
520	cc3_camera_set_resolution (CC3_CAMERA_RESOLUTION_HIGH);
521	else
522	cc3_camera_set_resolution (CC3_CAMERA_RESOLUTION_LOW);
523	cc3_pixbuf_frame_set_subsample (CC3_SUBSAMPLE_NEAREST, 2, 1);
524	break;
525
526
527
528	case LOAD_FRAME:
529	if (n != 0) {
530	error = true;
531	break;
532	}
533	print_ACK ();
534	fd_pkt.total_x = cc3_g_pixbuf_frame.width;
535	fd_pkt.total_y = cc3_g_pixbuf_frame.height;
536	fd_pkt.load_frame = 1; // load a new frame
537	cmucam2_load_frame (&fd_pkt, buf_mode, sw_color_space);
538	break;
539
540	case HIRES_DIFF:
541	if (n != 1 \|\| arg_list[0] > 1) {
542	error = true;
543	break;
544	}
545	print_ACK ();
546	if (arg_list[0] == 0) {
547	fd_pkt.template_width = 8;
548	fd_pkt.template_height = 8;
549	}
550	else {
551	fd_pkt.template_width = 16;
552	fd_pkt.template_height = 16;
553	}
554	break;
555
556	case FRAME_DIFF:
557	if (n != 1) {
558	error = true;
559	break;
560	}
561	print_ACK ();
562	fd_pkt.threshold = arg_list[0];
563	fd_pkt.load_frame = 0;
564	fd_pkt.total_x = cc3_g_pixbuf_frame.width;
565	fd_pkt.total_y = cc3_g_pixbuf_frame.height;
566	cmucam2_frame_diff (&fd_pkt, poll_mode, line_mode, buf_mode, auto_led, sw_color_space,
567	0);
568	break;
569
570	case FRAME_DIFF_CHANNEL:
571	if (n != 1 \|\| arg_list[0] > 2) {
572	error = true;
573	break;
574	}
575	print_ACK ();
576	fd_pkt.coi = arg_list[0];
577	break;
578
579
580	case CONF_HISTOGRAM:
581	if (n != 1 \|\| arg_list[0] < 1) {
582	error = true;
583	break;
584	}
585	h_pkt.bins = arg_list[0];
586	print_ACK ();
587
588	break;
589
590
591	case TRACK_INVERT:
592	if (n != 1 \|\| arg_list[0] > 1) {
593	error = true;
594	break;
595	}
596
597	print_ACK ();
598	if (arg_list[0] == 0)
599	t_pkt.track_invert = 0;
600	else
601	t_pkt.track_invert = 1;
602	break;
603
604	case COLOR_SPACE:
605	if (n != 1) {
606	error = true;
607	break;
608	}
609
610	print_ACK ();
611	sw_color_space= arg_list[0];
612	break;
613
614	case NOISE_FILTER:
615	if (n != 1) {
616	error = true;
617	break;
618	}
619
620	print_ACK ();
621	t_pkt.noise_filter = arg_list[0];
622	break;
623
624	case LINE_MODE:
625	if (n != 2) {
626	error = true;
627	break;
628	}
629
630	print_ACK ();
631	// FIXME: Make bitmasks later
632	if (arg_list[0] == 0) {
633	if (arg_list[1] == 1)
634	line_mode = 1;
635	else if (arg_list[1] == 2)
636	line_mode=2;
637	else
638	line_mode = 0;
639	}
640	break;
641
642
643	case SEND_JPEG:
644	if (n != 0 && n != 1) {
645	error = true;
646	break;
647	}
648
649	print_ACK ();
650	// ignore raw mode
651	cc3_jpeg_send_simple ();
652	printf ("JPG_END\r");
653	break;
654
655
656	case SEND_FRAME:
657	old_coi = cc3_g_pixbuf_frame.coi;
658	if (n == 1) {
659	if (arg_list[0] > 4) {
660	error = true;
661	break;
662	}
663	cc3_pixbuf_frame_set_coi (arg_list[0]);
664	}
665	else if (n > 1) {
666	error = true;
667	break;
668	}
669
670	print_ACK ();
671	do {
672	cmucam2_send_image_direct (auto_led,sw_color_space);
673	// Check to see if data is on the UART to break from frame_stream_mode
674	if (!cc3_uart_has_data (0)) {
675	if (getchar () == '\r')
676	break;
677	}
678	} while (frame_stream_mode);
679
680	cc3_pixbuf_frame_set_coi (old_coi);
681	break;
682
683	case RAW_MODE:
684	if (n != 1) {
685	error = true;
686	break;
687	}
688
689	raw_mode_output = arg_list[0] & 1;
690	raw_mode_no_confirmations = arg_list[0] & 2;
691	raw_mode_input = arg_list[0] & 4;
692	print_ACK (); // last because ACK may be supressed
693	break;
694
695	case CAMERA_REG:
696	if (n % 2 != 0 \|\| n < 2) {
697	error = true;
698	break;
699	}
700
701	print_ACK ();
702	for (int i = 0; i < n; i += 2)
703	cc3_camera_set_raw_register (arg_list[i], arg_list[i + 1]);
704	break;
705
706	case CAMERA_POWER:
707	if (n != 1) {
708	error = true;
709	break;
710	}
711
712	print_ACK ();
713	{
714	// save
715	uint16_t x_0 = cc3_g_pixbuf_frame.x0;
716	uint16_t y_0 = cc3_g_pixbuf_frame.y0;
717	uint16_t x_1 = cc3_g_pixbuf_frame.x1;
718	uint16_t y_1 = cc3_g_pixbuf_frame.y1;
719	uint8_t x_step = cc3_g_pixbuf_frame.x_step;
720	uint8_t y_step = cc3_g_pixbuf_frame.y_step;
721	cc3_subsample_mode_t ss_mode = cc3_g_pixbuf_frame.subsample_mode;
722
723	cc3_camera_set_power_state (arg_list[0]);
724
725	// restore
726	cc3_pixbuf_frame_set_roi (x_0, y_0, x_1, y_1);
727	cc3_pixbuf_frame_set_subsample (ss_mode, x_step, y_step);
728	}
729	break;
730
731	case VIRTUAL_WINDOW:
732	if (n != 4) {
733	error = true;
734	break;
735	}
736
737	print_ACK ();
738	cc3_pixbuf_frame_set_roi (arg_list[0] * 2,
739	arg_list[1], arg_list[2] * 2, arg_list[3]);
740	break;
741
742	case GET_TRACK:
743	if (n != 0) {
744	error = true;
745	break;
746	}
747
748	print_ACK ();
749	if (raw_mode_output) {
750	putchar (255);
751	raw_print (t_pkt.lower_bound.channel[0]);
752	raw_print (t_pkt.lower_bound.channel[1]);
753	raw_print (t_pkt.lower_bound.channel[2]);
754	raw_print (t_pkt.upper_bound.channel[0]);
755	raw_print (t_pkt.upper_bound.channel[1]);
756	raw_print (t_pkt.upper_bound.channel[2]);
757	}
758	else {
759	printf ("%d %d %d %d %d %d\r", t_pkt.lower_bound.channel[0],
760	t_pkt.lower_bound.channel[1], t_pkt.lower_bound.channel[2],
761	t_pkt.upper_bound.channel[0], t_pkt.upper_bound.channel[1],
762	t_pkt.upper_bound.channel[2]);
763	}
764	break;
765
766	case GET_WINDOW:
767	if (n != 0) {
768	error = true;
769	break;
770	}
771
772	print_ACK ();
773	if (raw_mode_output) {
774	putchar (255);
775	raw_print (cc3_g_pixbuf_frame.x0 / 2);
776	raw_print (cc3_g_pixbuf_frame.y0);
777	raw_print (cc3_g_pixbuf_frame.x1 / 2);
778	raw_print (cc3_g_pixbuf_frame.y1);
779	}
780	else {
781	printf ("%d %d %d %d\r", cc3_g_pixbuf_frame.x0 / 2,
782	cc3_g_pixbuf_frame.y0, cc3_g_pixbuf_frame.x1 / 2,
783	cc3_g_pixbuf_frame.y1);
784	}
785	break;
786
787	case DOWN_SAMPLE:
788	if (n != 2) {
789	error = true;
790	break;
791	}
792
793	print_ACK ();
794	cc3_pixbuf_frame_set_subsample (CC3_SUBSAMPLE_NEAREST,
795	arg_list[0] * 2, arg_list[1]);
796	break;
797
798	case SET_TRACK:
799	if (n != 0 && n != 6) {
800	error = true;
801	break;
802	}
803	print_ACK ();
804	if (n == 6) {
805	t_pkt.lower_bound.channel[0] = arg_list[0];
806	t_pkt.upper_bound.channel[0] = arg_list[1];
807	t_pkt.lower_bound.channel[1] = arg_list[2];
808	t_pkt.upper_bound.channel[1] = arg_list[3];
809	t_pkt.lower_bound.channel[2] = arg_list[4];
810	t_pkt.upper_bound.channel[2] = arg_list[5];
811	}
812	break;
813
814	case TRACK_COLOR:
815	if (n != 0 && n != 6) {
816	error = true;
817	break;
818	}
819
820	print_ACK ();
821	if (n == 6) {
822	t_pkt.lower_bound.channel[0] = arg_list[0];
823	t_pkt.upper_bound.channel[0] = arg_list[1];
824	t_pkt.lower_bound.channel[1] = arg_list[2];
825	t_pkt.upper_bound.channel[1] = arg_list[3];
826	t_pkt.lower_bound.channel[2] = arg_list[4];
827	t_pkt.upper_bound.channel[2] = arg_list[5];
828	}
829	cmucam2_track_color (&t_pkt, poll_mode, line_mode, auto_led,
830	&servo_settings, buf_mode, sw_color_space, 0);
831	break;
832
833	case TRACK_WINDOW:
834	if (n != 0 && n != 1) {
835	error = true;
836	break;
837	}
838	else {
839	uint32_t threshold, x0, y0, x1, y1;
840	int32_t tmp;
841	threshold = 30;
842	if (n == 1)
843	threshold = arg_list[0];
844	print_ACK ();
845	// set window to 1/2 size
846	x0 = cc3_g_pixbuf_frame.x0 + cc3_g_pixbuf_frame.width / 4;
847	x1 = cc3_g_pixbuf_frame.x1 - cc3_g_pixbuf_frame.width / 4;
848	y0 = cc3_g_pixbuf_frame.y0 + cc3_g_pixbuf_frame.width / 4;
849	y1 = cc3_g_pixbuf_frame.y1 - cc3_g_pixbuf_frame.width / 4;
850	cc3_pixbuf_frame_set_roi (x0, y0, x1, y1);
851	// call get mean
852	cmucam2_get_mean (&s_pkt, 1, line_mode, buf_mode,sw_color_space, 1);
853	// set window back to full size
854	x0 = 0;
855	x1 = cc3_g_pixbuf_frame.raw_width;
856	y0 = 0;
857	y1 = cc3_g_pixbuf_frame.raw_height;
858	cc3_pixbuf_frame_set_roi (x0, y0, x1, y1);
859	// fill in parameters and call track color
860	tmp = s_pkt.mean.channel[0] - threshold;
861	if (tmp < 16)
862	tmp = 16;
863	if (tmp > 240)
864	tmp = 240;
865	t_pkt.lower_bound.channel[0] = tmp;
866	tmp = s_pkt.mean.channel[0] + threshold;
867	if (tmp < 16)
868	tmp = 16;
869	if (tmp > 240)
870	tmp = 240;
871	t_pkt.upper_bound.channel[0] = tmp;
872	tmp = s_pkt.mean.channel[1] - threshold;
873	if (tmp < 16)
874	tmp = 16;
875	if (tmp > 240)
876	tmp = 240;
877	t_pkt.lower_bound.channel[1] = tmp;
878	tmp = s_pkt.mean.channel[1] + threshold;
879	if (tmp < 16)
880	tmp = 16;
881	if (tmp > 240)
882	tmp = 240;
883	t_pkt.upper_bound.channel[1] = tmp;
884	tmp = s_pkt.mean.channel[2] - threshold;
885	if (tmp < 16)
886	tmp = 16;
887	if (tmp > 240)
888	tmp = 240;
889	t_pkt.lower_bound.channel[2] = tmp;
890	tmp = s_pkt.mean.channel[2] + threshold;
891	if (tmp < 16)
892	tmp = 16;
893	if (tmp > 240)
894	tmp = 240;
895	t_pkt.upper_bound.channel[2] = tmp;
896	cmucam2_track_color (&t_pkt, poll_mode, line_mode, auto_led,
897	&servo_settings, buf_mode,sw_color_space, 0);
898	}
899	demo_mode = false;
900	break;
901
902
903	case GET_MEAN:
904	if (n != 0) {
905	error = true;
906	break;
907	}
908
909	print_ACK ();
910	cmucam2_get_mean (&s_pkt, poll_mode, line_mode, buf_mode,sw_color_space, 0);
911	break;
912
913
914	case GET_HISTOGRAM:
915	if (n != 1 \|\| arg_list[0] > 2) {
916	error = true;
917	break;
918	}
919
920	print_ACK ();
921	h_pkt.channel = arg_list[0];
922	cmucam2_get_histogram (&h_pkt, poll_mode, buf_mode,sw_color_space, 0);
923	break;
924
925
926	case SET_SERVO:
927	if (n != 2 \|\| arg_list[0] > 4) {
928	error = true;
929	break;
930	}
931
932	print_ACK ();
933	cc3_gpio_set_mode (arg_list[0], CC3_GPIO_MODE_SERVO);
934	cc3_gpio_set_servo_position (arg_list[0], arg_list[1]);
935	if (arg_list[0] == 0)
936	servo_settings.x = arg_list[1];
937	if (arg_list[0] == 1)
938	servo_settings.y = arg_list[1];
939	break;
940
941	case GET_SERVO:
942	if (n != 1) {
943	error = true;
944	break;
945	}
946
947	print_ACK ();
948
949	{
950	uint8_t servo = cc3_gpio_get_servo_position (arg_list[0]);
951	if (raw_mode_output) {
952	putchar (255);
953	raw_print (servo);
954	}
955	else {
956	printf ("%d\r", servo);
957	}
958	break;
959	}
960
961	case GET_INPUT:
962	if (n != 0) {
963	error = true;
964	break;
965	}
966
967	print_ACK ();
968	{
969	uint8_t input =
970	(cc3_gpio_get_value (arg_list[0])) \|
971	(cc3_gpio_get_value (arg_list[1]) << 1) \|
972	(cc3_gpio_get_value (arg_list[2]) << 2) \|
973	(cc3_gpio_get_value (arg_list[3]) << 3);
974	if (raw_mode_output) {
975	putchar (255);
976	raw_print (input);
977	}
978	else {
979	printf ("%d\r", input);
980	}
981	}
982	break;
983
984
985	case SET_INPUT:
986	if (n != 1) {
987	error = true;
988	break;
989	}
990	print_ACK ();
991	cc3_gpio_set_mode (arg_list[0], CC3_GPIO_MODE_INPUT);
992
993	break;
994
995	case GET_BUTTON:
996	if (n != 0) {
997	error = true;
998	break;
999	}
1000
1001	print_ACK ();
1002
1003	{
1004	int button = cc3_button_get_and_reset_trigger ()? 1 : 0;
1005	if (raw_mode_output) {
1006	putchar (255);
1007	raw_print (button);
1008	}
1009	else {
1010	printf ("%d\r", button);
1011	}
1012	}
1013	break;
1014
1015	case SERVO_OUTPUT:
1016	if (n != 2) {
1017	error = true;
1018	break;
1019	}
1020	print_ACK ();
1021	cc3_gpio_set_mode (arg_list[0], CC3_GPIO_MODE_OUTPUT);
1022	cc3_gpio_set_value (arg_list[0], arg_list[1]);
1023
1024	break;
1025
1026	default:
1027	print_ACK ();
1028	break;
1029	}
1030	}
1031	else
1032	error = true;
1033
1034	if (error)
1035	print_NCK ();
1036	}
1037
1038
1039	return 0;
1040	}
1041
1042
1043	void cmucam2_send_image_direct (bool auto_led, uint8_t sw_color_space)
1044	{
1045	cc3_pixbuf_load ();
1046
1047	uint32_t x, y;
1048	uint32_t size_x, size_y;
1049	uint8_t *row = cc3_malloc_rows (1);
1050	uint8_t num_channels = cc3_g_pixbuf_frame.coi == CC3_CHANNEL_ALL ? 3 : 1;
1051
1052
1053	size_x = cc3_g_pixbuf_frame.width;
1054	size_y = cc3_g_pixbuf_frame.height;
1055
1056	putchar (1);
1057	putchar (size_x);
1058	if (size_y > 255)
1059	size_y = 255;
1060	putchar (size_y);
1061	for (y = 0; y < size_y; y++) {
1062	putchar (2);
1063	if (auto_led) {
1064	if (y % 4 == 0)
1065	cc3_led_set_state (0, true);
1066	else
1067	cc3_led_set_state (0, false);
1068	}
1069	cc3_pixbuf_read_rows (row, 1);
1070	if(sw_color_space==HSV_COLOR && num_channels==CC3_CHANNEL_ALL ) cc3_rgb2hsv_row(row,size_x);
1071	for (x = 0; x < size_x * num_channels; x++) {
1072	uint8_t p = row[x];
1073
1074	// avoid confusion from FIFO corruptions
1075	if (p < 16) {
1076	p = 16;
1077	}
1078	else if (p > 240) {
1079	p = 240;
1080	}
1081	putchar (p);
1082	}
1083	}
1084	putchar (3);
1085
1086	cc3_led_set_state (0, false);
1087	free (row);
1088	}
1089
1090
1091
1092	void cmucam2_get_histogram (cc3_histogram_pkt_t * h_pkt, bool poll_mode,
1093	bool buf_mode, uint8_t sw_color_space, bool quiet)
1094	{
1095	cc3_image_t img;
1096	img.channels = 3;
1097	img.width = cc3_g_pixbuf_frame.width;
1098	img.height = 1; // image will hold just 1 row for scanline processing
1099	img.pix = malloc (3 * img.width);
1100	h_pkt->hist = malloc (h_pkt->bins * sizeof (uint32_t));
1101	if (img.pix == NULL \|\| h_pkt->hist == NULL) {
1102	printf ("INTERNAL ERROR\r");
1103	return;
1104	}
1105	do {
1106	if (!buf_mode)
1107	cc3_pixbuf_load ();
1108	else
1109	cc3_pixbuf_rewind ();
1110	if (cc3_histogram_scanline_start (h_pkt) != 0) {
1111	while (cc3_pixbuf_read_rows (img.pix, 1)) {
1112	if(sw_color_space==HSV_COLOR && img.channels==CC3_CHANNEL_ALL) cc3_rgb2hsv_row(img.pix,img.width);
1113	cc3_histogram_scanline (&img, h_pkt);
1114	}
1115	cc3_histogram_scanline_finish (h_pkt);
1116	while (!cc3_uart_has_data (0)) {
1117	if (getchar () == '\r') {
1118	free (img.pix);
1119	free (h_pkt->hist);
1120	return;
1121	}
1122	}
1123	if (!quiet)
1124	cmucam2_write_h_packet (h_pkt);
1125	}
1126	if (!cc3_uart_has_data (0)) {
1127	if (getchar () == '\r')
1128	break;
1129	}
1130	} while (!poll_mode);
1131
1132	free (img.pix);
1133	free (h_pkt->hist);
1134
1135
1136	}
1137
1138	void cmucam2_load_frame (cc3_frame_diff_pkt_t * pkt, bool buf_mode, uint8_t sw_color_space)
1139	{
1140	cc3_image_t img;
1141	uint8_t old_coi;
1142
1143	old_coi = cc3_g_pixbuf_frame.coi;
1144	cc3_pixbuf_frame_set_coi (pkt->coi);
1145
1146	img.channels = 1;
1147	img.width = cc3_g_pixbuf_frame.width;
1148	img.height = 1; // image will hold just 1 row for scanline processing
1149	img.pix = malloc (img.width);
1150
1151	if (!buf_mode)
1152	cc3_pixbuf_load ();
1153	else
1154	cc3_pixbuf_rewind ();
1155
1156	if (cc3_frame_diff_scanline_start (pkt) != 0) {
1157	while (cc3_pixbuf_read_rows (img.pix, 1)) {
1158	if(sw_color_space==HSV_COLOR && img.channels==CC3_CHANNEL_ALL) cc3_rgb2hsv_row(img.pix,img.width);
1159	cc3_frame_diff_scanline (&img, pkt);
1160	}
1161	cc3_frame_diff_scanline_finish (pkt);
1162	}
1163	else
1164	printf ("frame diff start error\r");
1165
1166	cc3_pixbuf_frame_set_coi (old_coi);
1167	free (img.pix);
1168
1169
1170	}
1171
1172	void cmucam2_frame_diff (cc3_frame_diff_pkt_t * pkt,
1173	bool poll_mode, bool line_mode, bool buf_mode,
1174	bool auto_led, uint8_t sw_color_space, bool quiet)
1175	{
1176	cc3_track_pkt_t t_pkt;
1177	cc3_image_t img;
1178	uint8_t old_coi;
1179
1180	bool prev_packet_empty = true;
1181
1182	old_coi = cc3_g_pixbuf_frame.coi;
1183	cc3_pixbuf_frame_set_coi (pkt->coi);
1184	img.channels = 1;
1185	img.width = cc3_g_pixbuf_frame.width;
1186	img.height = 1; // image will hold just 1 row for scanline processing
1187	img.pix = malloc (img.width);
1188	pkt->current_template =
1189	malloc (pkt->template_width * pkt->template_height * sizeof (uint32_t));
1190	if (pkt->current_template == NULL)
1191	printf ("Malloc failed in frame diff\r");
1192	do {
1193	if (!buf_mode)
1194	cc3_pixbuf_load ();
1195	else
1196	cc3_pixbuf_rewind ();
1197
1198	if (cc3_frame_diff_scanline_start (pkt) != 0) {
1199
1200	while (cc3_pixbuf_read_rows (img.pix, 1)) {
1201	if(sw_color_space==HSV_COLOR && img.channels==CC3_CHANNEL_ALL) cc3_rgb2hsv_row(img.pix,img.width);
1202	cc3_frame_diff_scanline (&img, pkt);
1203	}
1204	cc3_frame_diff_scanline_finish (pkt);
1205
1206	while (!cc3_uart_has_data (0)) {
1207	if (getchar () == '\r') {
1208	cc3_pixbuf_frame_set_coi (old_coi);
1209	free (pkt->current_template);
1210	free (img.pix);
1211	return;
1212	}
1213	}
1214	if (!quiet) {
1215	t_pkt.x0 = pkt->x0 + 1;
1216	t_pkt.y0 = pkt->y0 + 1;
1217	t_pkt.x1 = pkt->x1 + 1;
1218	t_pkt.y1 = pkt->y1 + 1;
1219	t_pkt.num_pixels = pkt->num_pixels;
1220	t_pkt.centroid_x = pkt->centroid_x + 1;
1221	t_pkt.centroid_y = pkt->centroid_y + 1;
1222	t_pkt.int_density = pkt->int_density;
1223	if (auto_led) {
1224	if (t_pkt.num_pixels > 2)
1225	cc3_led_set_state (0, true);
1226	else
1227	cc3_led_set_state (0, false);
1228	}
1229
1230	if (!(packet_filter_flag &&
1231	t_pkt.num_pixels == 0 && prev_packet_empty)) {
1232	cmucam2_write_t_packet (&t_pkt, NULL);
1233	}
1234	prev_packet_empty = t_pkt.num_pixels == 0;
1235	}
1236	}
1237
1238
1239	if (!cc3_uart_has_data (0)) {
1240	if (getchar () == '\r')
1241	break;
1242	}
1243	} while (!poll_mode);
1244
1245	cc3_pixbuf_frame_set_coi (old_coi);
1246	free (pkt->current_template);
1247	free (img.pix);
1248	}
1249
1250	void cmucam2_get_mean (cc3_color_info_pkt_t * s_pkt,
1251	bool poll_mode, bool line_mode, bool buf_mode, uint8_t sw_color_space,
1252	bool quiet)
1253	{
1254	cc3_image_t img;
1255	img.channels = 3;
1256	img.width = cc3_g_pixbuf_frame.width;
1257	img.height = 1; // image will hold just 1 row for scanline processing
1258	img.pix = malloc (3 * img.width);
1259	do {
1260	if (!buf_mode)
1261	cc3_pixbuf_load ();
1262	else
1263	cc3_pixbuf_rewind ();
1264	if (cc3_color_info_scanline_start (s_pkt) != 0) {
1265	while (cc3_pixbuf_read_rows (img.pix, 1)) {
1266	if(sw_color_space==HSV_COLOR && img.channels==CC3_CHANNEL_ALL) cc3_rgb2hsv_row(img.pix,img.width);
1267	cc3_color_info_scanline (&img, s_pkt);
1268	}
1269	cc3_color_info_scanline_finish (s_pkt);
1270	while (!cc3_uart_has_data (0)) {
1271	if (getchar () == '\r') {
1272	free (img.pix);
1273	return;
1274	}
1275	}
1276	if (!quiet)
1277	cmucam2_write_s_packet (s_pkt);
1278	}
1279	if (!cc3_uart_has_data (0)) {
1280	if (getchar () == '\r')
1281	break;
1282	}
1283	} while (!poll_mode);
1284
1285	free (img.pix);
1286	}
1287
1288	void cmucam2_track_color (cc3_track_pkt_t * t_pkt,
1289	bool poll_mode,
1290	int8_t line_mode, bool auto_led,
1291	cmucam2_servo_t * servo_settings, bool buf_mode, uint8_t sw_color_space,
1292	bool quiet)
1293	{
1294	cc3_image_t img;
1295	uint16_t x_mid, y_mid;
1296	int8_t t_step;
1297
1298	bool prev_packet_empty = true;
1299
1300	img.channels = 3;
1301	img.width = cc3_g_pixbuf_frame.width;
1302	img.height = 1; // image will hold just 1 row for scanline processing
1303	img.pix = cc3_malloc_rows (1);
1304	if (img.pix == NULL) {
1305	return;
1306	}
1307
1308	x_mid = cc3_g_pixbuf_frame.x0 + (cc3_g_pixbuf_frame.width / 2);
1309	y_mid = cc3_g_pixbuf_frame.y0 + (cc3_g_pixbuf_frame.height / 2);
1310
1311
1312	do {
1313	if (!buf_mode)
1314	cc3_pixbuf_load ();
1315	else
1316	cc3_pixbuf_rewind ();
1317	if (cc3_track_color_scanline_start (t_pkt) != 0) {
1318	uint8_t lm_width, lm_height;
1319	uint8_t *lm;
1320	lm_width = 0;
1321	lm_height = 0;
1322	if (line_mode==1) {
1323	// FIXME: This doesn't make sense
1324	lm = &t_pkt->binary_scanline;
1325	lm_width = img.width / 8;
1326	if (img.width % 8 != 0)
1327	lm_width++;
1328	if (!quiet)
1329	putchar (0xAA);
1330	if (cc3_g_pixbuf_frame.height > 255)
1331	lm_height = 255;
1332	else
1333	lm_height = cc3_g_pixbuf_frame.height;
1334	if (!quiet)
1335	putchar (img.width);
1336	if (!quiet)
1337	putchar (lm_height);
1338	}
1339
1340	if (line_mode==2) {
1341	// FIXME: This still doesn't make sense
1342	lm = &t_pkt->binary_scanline;
1343	lm_width = img.width / 8;
1344	if (img.width % 8 != 0)
1345	lm_width++;
1346	if (!quiet)
1347	putchar (0xFE);
1348	if (cc3_g_pixbuf_frame.height > 255)
1349	lm_height = 255;
1350	else
1351	lm_height = cc3_g_pixbuf_frame.height;
1352	//if (!quiet)
1353	//putchar (img.width);
1354	if (!quiet)
1355	putchar (lm_height);
1356	}
1357
1358
1359	while (cc3_pixbuf_read_rows (img.pix, 1)) {
1360	if(sw_color_space==HSV_COLOR && img.channels==CC3_CHANNEL_ALL) cc3_rgb2hsv_row(img.pix,img.width);
1361	cc3_track_color_scanline (&img, t_pkt);
1362
1363	if (line_mode==1) {
1364	// keep this check here if you don't want the CMUcam2 GUI to hang after exiting a command in line mode
1365	while (!cc3_uart_has_data (0)) {
1366	if (getchar () == '\r') {
1367	free (img.pix);
1368	return;
1369	}
1370	}
1371	for (int j = 0; j < lm_width; j++) {
1372	if (lm[j] == 0xAA) {
1373	if (!quiet)
1374	putchar (0xAB);
1375	}
1376	else {
1377	if (!quiet)
1378	putchar (lm[j]);
1379	}
1380	}
1381	}
1382
1383
1384	if (line_mode==2) {
1385	uint8_t min,max,p_count,conf;
1386	uint32_t mean;
1387	// keep this check here if you don't want the CMUcam2 GUI to hang after exiting a command in line mode
1388	while (!cc3_uart_has_data (0)) {
1389	if (getchar () == '\r') {
1390	free (img.pix);
1391	return;
1392	}
1393	}
1394	mean=0;
1395	min=255;
1396	max=0;
1397	p_count=0;
1398	for (int j = 0; j < img.width; j++) {
1399	uint8_t block, offset;
1400	block = j / 8;
1401	offset = j % 8;
1402	offset = 7 - offset;
1403	if((lm[block] & (1<<offset))!=0)
1404	{
1405	// bit detected
1406	if(j<min) min=j;
1407	if(j>max) max=j;
1408	p_count++;
1409	mean+=j;
1410	}
1411	}
1412	mean=mean/p_count;
1413	conf=((max-min)*100)/p_count;
1414	if (!quiet)
1415	printf( "%c%c%c%c%c",(uint8_t)mean,min,max,p_count,conf);
1416	}
1417
1418
1419
1420	}
1421	// keep this check here if you don't want the CMUcam2 GUI to hang after exiting a command in line mode
1422	while (!cc3_uart_has_data (0)) {
1423	if (getchar () == '\r') {
1424	free (img.pix);
1425	return;
1426	}
1427	}
1428	cc3_track_color_scanline_finish (t_pkt);
1429	if (line_mode==1) {
1430	if (!quiet)
1431	putchar (0xAA);
1432	if (!quiet)
1433	putchar (0xAA);
1434	}
1435
1436	if (line_mode==2) {
1437	if (!quiet)
1438	putchar (0xFD);
1439	}
1440
1441	if (auto_led) {
1442	if (t_pkt->int_density > 2)
1443	cc3_led_set_state (0, true);
1444	else
1445	cc3_led_set_state (0, false);
1446	}
1447
1448	if (t_pkt->int_density > 5 && servo_settings != NULL) {
1449	if (servo_settings->x_control) {
1450	t_step = 0;
1451	if (t_pkt->centroid_x > x_mid + servo_settings->pan_range_far)
1452	t_step = servo_settings->pan_step;
1453	else if (t_pkt->centroid_x > x_mid + servo_settings->pan_range_near)
1454	t_step = (servo_settings->pan_step / 2);
1455
1456	if (t_pkt->centroid_x < x_mid - servo_settings->pan_range_far)
1457	t_step = -servo_settings->pan_step;
1458	else if (t_pkt->centroid_x < x_mid - servo_settings->pan_range_near)
1459	t_step = -(servo_settings->pan_step / 2);
1460
1461
1462	#ifdef SERVO_REVERSE_DIRECTION_PAN
1463	servo_settings->x -= t_step;
1464	#else
1465	servo_settings->x += t_step;
1466	#endif
1467	t_step = 0;
1468	if (servo_settings->x > SERVO_MAX)
1469	servo_settings->x = SERVO_MAX;
1470	if (servo_settings->x < SERVO_MIN)
1471	servo_settings->x = SERVO_MIN;
1472	cc3_gpio_set_servo_position (0, servo_settings->x);
1473	}
1474	if (servo_settings->y_control) {
1475	if (t_pkt->centroid_y > y_mid + servo_settings->tilt_range_far)
1476	t_step = servo_settings->tilt_step;
1477	else if (t_pkt->centroid_y >
1478	y_mid + servo_settings->tilt_range_near)
1479	t_step = servo_settings->tilt_step / 2;
1480
1481	if (t_pkt->centroid_y < y_mid - servo_settings->tilt_range_far)
1482	t_step = -(servo_settings->tilt_step);
1483	else if (t_pkt->centroid_y <
1484	y_mid - servo_settings->tilt_range_near)
1485	t_step = -(servo_settings->tilt_step / 2);
1486
1487	#ifdef SERVO_REVERSE_DIRECTION_TILT
1488	servo_settings->y -= t_step;
1489	#else
1490	servo_settings->y += t_step;
1491	#endif
1492
1493	if (servo_settings->y > SERVO_MAX)
1494	servo_settings->y = SERVO_MAX;
1495	if (servo_settings->y < SERVO_MIN)
1496	servo_settings->y = SERVO_MIN;
1497	cc3_gpio_set_servo_position (1, servo_settings->y);
1498	}
1499	}
1500
1501	if (!quiet) {
1502	if (!(packet_filter_flag &&
1503	t_pkt->num_pixels == 0 && prev_packet_empty)) {
1504	cmucam2_write_t_packet (t_pkt, servo_settings);
1505	}
1506	}
1507	prev_packet_empty = t_pkt->num_pixels == 0;
1508	}
1509	else
1510	return;
1511
1512	while (!cc3_uart_has_data (0)) {
1513	if (getchar () == '\r')
1514	break;
1515	}
1516	} while (!poll_mode);
1517	free (img.pix);
1518	return;
1519	}
1520
1521	void cmucam2_write_t_packet (cc3_track_pkt_t * pkt,
1522	cmucam2_servo_t * servo_settings)
1523	{
1524
1525	// cap at 255
1526	if (pkt->centroid_x > 255)
1527	pkt->centroid_x = 255;
1528	if (pkt->centroid_y > 255)
1529	pkt->centroid_y = 255;
1530	if (pkt->x0 > 255)
1531	pkt->x0 = 255;
1532	if (pkt->x1 > 255)
1533	pkt->x1 = 255;
1534	if (pkt->y1 > 255)
1535	pkt->y1 = 255;
1536	if (pkt->y0 > 255)
1537	pkt->y0 = 255;
1538	if (pkt->num_pixels > 255)
1539	pkt->num_pixels = 255;
1540	if (pkt->int_density > 255)
1541	pkt->int_density = 255;
1542
1543	// values to print
1544	uint8_t p[8];
1545
1546	if (pkt->num_pixels == 0) {
1547	p[0] = p[1] = p[2] = p[3] = p[4] = p[5] = p[6] = p[7] = 0;
1548	}
1549	else {
1550	p[0] = pkt->centroid_x;
1551	p[1] = pkt->centroid_y;
1552	p[2] = pkt->x0;
1553	p[3] = pkt->y0;
1554	p[4] = pkt->x1;
1555	p[5] = pkt->y1;
1556	p[6] = pkt->num_pixels;
1557	p[7] = pkt->int_density;
1558	}
1559
1560	uint8_t mask = t_pkt_mask;
1561	if (raw_mode_output) {
1562	putchar (255);
1563	}
1564	printf ("T");
1565
1566	// print out fields using mask
1567	for (int i = 0; i < 8; i++) {
1568	if (mask & 0x1) {
1569	if (raw_mode_output) {
1570	raw_print (p[i]);
1571	}
1572	else {
1573	printf (" %d", p[i]);
1574	}
1575	}
1576	mask >>= 1;
1577	}
1578
1579	// print servo settings?
1580	if (servo_settings != NULL) {
1581	uint8_t sx = servo_settings->x;
1582	uint8_t sy = servo_settings->y;
1583
1584	if (servo_settings->x_report) {
1585	if (raw_mode_output) {
1586	raw_print (sx);
1587	}
1588	else {
1589	printf (" %d", sx);
1590	}
1591	}
1592	if (servo_settings->y_report) {
1593	if (raw_mode_output) {
1594	raw_print (sy);
1595	}
1596	else {
1597	printf (" %d", sy);
1598	}
1599	}
1600	}
1601
1602	print_cr ();
1603	}
1604
1605	void cmucam2_write_h_packet (cc3_histogram_pkt_t * pkt)
1606	{
1607	uint32_t i;
1608	uint32_t total_pix;
1609
1610	total_pix = cc3_g_pixbuf_frame.width * cc3_g_pixbuf_frame.height;
1611
1612	if (raw_mode_output) {
1613	putchar (255);
1614	}
1615	printf ("H");
1616
1617	for (i = 0; i < pkt->bins; i++) {
1618	pkt->hist[i] = (pkt->hist[i] * 256) / total_pix;
1619	if (pkt->hist[i] > 255)
1620	pkt->hist[i] = 255;
1621
1622	if (raw_mode_output) {
1623	raw_print (pkt->hist[i]);
1624	}
1625	else {
1626	printf (" %d", pkt->hist[i]);
1627	}
1628	}
1629
1630	print_cr ();
1631	}
1632
1633	void cmucam2_write_s_packet (cc3_color_info_pkt_t * pkt)
1634	{
1635	uint8_t pkt2[6];
1636	uint8_t mask = s_pkt_mask;
1637
1638	if (raw_mode_output) {
1639	putchar (255);
1640	}
1641
1642	pkt2[0] = pkt->mean.channel[0];
1643	pkt2[1] = pkt->mean.channel[1];
1644	pkt2[2] = pkt->mean.channel[2];
1645	pkt2[3] = pkt->deviation.channel[0];
1646	pkt2[4] = pkt->deviation.channel[1];
1647	pkt2[5] = pkt->deviation.channel[2];
1648
1649	printf ("S");
1650
1651	for (int i = 0; i < 6; i++) {
1652	if (mask & 0x1) {
1653	if (raw_mode_output) {
1654	putchar (pkt2[i]);
1655	}
1656	else {
1657	printf (" %d", pkt2[i]);
1658	}
1659	}
1660	mask >>= 1;
1661	}
1662
1663	print_cr ();
1664	}
1665
1666	void print_ACK ()
1667	{
1668	if (!raw_mode_no_confirmations)
1669	printf ("ACK\r");
1670	}
1671
1672	void print_NCK ()
1673	{
1674	if (!raw_mode_no_confirmations)
1675	printf ("NCK\r");
1676	}
1677
1678	void print_prompt ()
1679	{
1680	printf (":");
1681	}
1682
1683	void print_cr ()
1684	{
1685	if (!raw_mode_output) {
1686	printf ("\r");
1687	}
1688	}
1689
1690	int32_t cmucam2_get_command (cmucam2_command_t * cmd, uint32_t * arg_list)
1691	{
1692	char line_buf[MAX_LINE];
1693	int c;
1694	char *token;
1695	bool fail = false;
1696	uint32_t length, argc;
1697	uint32_t i;
1698
1699	length = 0;
1700	c = 0;
1701	while (c != '\r') {
1702	c = getchar ();
1703
1704	if (length < (MAX_LINE - 1) && c != EOF) {
1705	line_buf[length] = c;
1706	length++;
1707	}
1708	else {
1709	// too long or EOF
1710	return -1;
1711	}
1712	}
1713	// null terminate
1714	line_buf[length] = '\0';
1715
1716	// check for empty command
1717	if (line_buf[0] == '\r' \|\| line_buf[0] == '\n') {
1718	*cmd = RETURN;
1719	return 0;
1720	}
1721
1722	// start looking for command
1723	token = strtok (line_buf, " \r\n");
1724
1725	if (token == NULL) {
1726	// no command ?
1727	return -1;
1728	}
1729
1730	// get name of the command
1731	for (i = 0; i < strlen (token); i++) {
1732	token[i] = toupper (token[i]);
1733	}
1734
1735	// do lookup of command
1736	fail = true;
1737	for (i = 0; i < CMUCAM2_CMDS_COUNT; i++) {
1738	if (strcmp (token, cmucam2_cmds[i]) == 0) {
1739	fail = false;
1740	*cmd = i;
1741	break;
1742	}
1743	}
1744	if (fail) {
1745	return -1;
1746	}
1747
1748	// now get the arguments
1749	argc = 0;
1750	while (true) {
1751	// extract string from string sequence
1752	token = strtok (NULL, " \r\n");
1753	// check if there is nothing else to extract
1754	if (token == NULL) {
1755	// printf("Tokenizing complete\n");
1756	return argc;
1757	}
1758
1759	// make sure the argument is fully numeric
1760	for (i = 0; i < strlen (token); i++) {
1761	if (!isdigit (token[i]))
1762	return -1;
1763	}
1764
1765	// we have a valid token, add it
1766	arg_list[argc] = atoi (token);
1767	argc++;
1768	}
1769
1770	return -1;
1771	}
1772
1773	int32_t cmucam2_get_command_raw (cmucam2_command_t * cmd, uint32_t * arg_list)
1774	{
1775	bool fail;
1776	int c;
1777	unsigned int i;
1778	uint32_t argc;
1779
1780	char cmd_str[3];
1781	cmd_str[2] = '\0';
1782
1783	// read characters
1784	for (i = 0; i < 2; i++) {
1785	c = getchar ();
1786	if (c == EOF) {
1787	return -1;
1788	}
1789
1790	cmd_str[i] = c;
1791	}
1792
1793	// do lookup of command
1794	fail = true;
1795	for (i = 0; i < CMUCAM2_CMDS_COUNT; i++) {
1796	if (strcmp (cmd_str, cmucam2_cmds[i]) == 0) {
1797	fail = false;
1798	*cmd = i;
1799	break;
1800	}
1801	}
1802	if (fail) {
1803	return -1;
1804	}
1805
1806	// read argc
1807	c = getchar ();
1808	if (c == EOF) {
1809	return -1;
1810	}
1811	argc = c;
1812	if (argc > MAX_ARGS) {
1813	return -1;
1814	}
1815
1816	// read args
1817	for (i = 0; i < argc; i++) {
1818	c = getchar ();
1819	if (c == EOF) {
1820	return -1;
1821	}
1822
1823	arg_list[i] = toupper (c);
1824	}
1825
1826	// done
1827	return argc;
1828	}
1829
1830
1831	void raw_print (uint8_t val)
1832	{
1833	if (val == 255) {
1834	putchar (254); // avoid confusion
1835	}
1836	else {
1837	putchar (val);
1838	}
1839	}

Download in other formats:

Original Format