cmucam2-emulation: cmucam2.c

File cmucam2.c, 44.2 kB (added by agr, 8 months 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