Changeset 426

Show
Ignore:
Timestamp:
02/17/07 22:14:35 (2 years ago)
Author:
goodea
Message:

indent

Files:

Legend:

Unmodified
Added
Removed
Modified
Copied
Moved

  • trunk/projects/cmucam2/cmucam2.c

    r425 r426  
    141141 
    142142 
    143 static void cmucam2_get_histogram(cc3_histogram_pkt_t *h_pkt, bool poll_mode, bool quiet); 
    144 static void cmucam2_get_mean (cc3_color_info_pkt_t * t_pkt, 
    145                             bool poll_mode, 
    146                              bool line_mode, bool quite); 
     143static void cmucam2_get_histogram (cc3_histogram_pkt_t * h_pkt, 
     144                                   bool poll_mode, bool quiet); 
     145static void cmucam2_get_mean (cc3_color_info_pkt_t * t_pkt, bool poll_mode, 
     146                              bool line_mode, bool quite); 
    147147static void cmucam2_write_s_packet (cc3_color_info_pkt_t * pkt); 
    148148static void cmucam2_track_color (cc3_track_pkt_t * t_pkt, 
    149                                 bool poll_mode, 
    150                                 bool line_mode, bool auto_led, bool quite); 
     149                                bool poll_mode, 
     150                                bool line_mode, bool auto_led, bool quite); 
    151151static int32_t cmucam2_get_command (int32_t * cmd, int32_t * arg_list); 
    152152static void print_ACK (void); 
    153153static void print_NCK (void); 
    154154static void cmucam2_write_t_packet (cc3_track_pkt_t * pkt); 
    155 static void cmucam2_write_h_packet (cc3_histogram_pkt_t *pkt); 
     155static void cmucam2_write_h_packet (cc3_histogram_pkt_t * pkt); 
    156156static void cmucam2_send_image_direct (bool auto_led); 
    157157 
    …… 
    160160  int32_t command; 
    161161  int32_t val, n; 
    162   uint32_t arg_list[MAX_ARGS],start_time; 
    163   bool error, poll_mode, line_mode,auto_led,demo_mode,auto_servo_mode; 
     162  uint32_t arg_list[MAX_ARGS], start_time; 
     163  bool error, poll_mode, line_mode, auto_led, demo_mode, auto_servo_mode; 
    164164  cc3_track_pkt_t t_pkt; 
    165165  cc3_color_info_pkt_t s_pkt; 
    …… 
    171171  cc3_system_setup (); 
    172172 
    173   cc3_filesystem_init(); 
     173  cc3_filesystem_init (); 
    174174 
    175175  cc3_uart_init (0, 
    …… 
    179179 
    180180  if (!cc3_camera_init ()) { 
    181     cc3_led_set_on(0); 
    182     exit(1); 
     181    cc3_led_set_on (0); 
     182    exit (1); 
    183183  } 
    184184 
    185   demo_mode=false; 
    186   auto_servo_mode=false; 
     185  demo_mode = false; 
     186  auto_servo_mode = false; 
    187187  start_time = cc3_timer_get_current_ms (); 
    188   do  
    189   { 
    190     if(cc3_button_get_state()==1) 
    191     { 
    192         // Demo Mode flag 
    193         demo_mode=true; 
    194         auto_servo_mode=true; 
    195         // Debounce Switch 
    196         cc3_led_set_off(0); 
    197         cc3_timer_wait_ms(500); 
    198         break; 
    199     } 
    200  
    201   }while(cc3_timer_get_current_ms()<(start_time+2000)); 
    202    
    203    
     188  do { 
     189    if (cc3_button_get_state () == 1) { 
     190      // Demo Mode flag 
     191      demo_mode = true; 
     192      auto_servo_mode = true; 
     193      // Debounce Switch 
     194      cc3_led_set_off (0); 
     195      cc3_timer_wait_ms (500); 
     196      break; 
     197    } 
     198 
     199  } while (cc3_timer_get_current_ms () < (start_time + 2000)); 
     200 
     201 
    204202cmucam2_start: 
    205   auto_led= true;  
     203  auto_led = true; 
    206204  poll_mode = false; 
    207205  line_mode = false; 
    208   h_pkt.bins=28; 
     206  h_pkt.bins = 28; 
    209207  t_pkt.track_invert = false; 
    210208  t_pkt.noise_filter = 0; 
    …… 
    215213  t_pkt.lower_bound.channel[2] = 16; 
    216214  t_pkt.upper_bound.channel[2] = 240; 
    217    
    218   
    219   cc3_camera_set_resolution(CC3_CAMERA_RESOLUTION_LOW); 
    220  
    221   cc3_pixbuf_load(); 
     215 
     216 
     217  cc3_camera_set_resolution (CC3_CAMERA_RESOLUTION_LOW); 
     218 
     219  cc3_pixbuf_load (); 
    222220 
    223221  printf ("%s\r", VERSION_BANNER); 
    224222 
    225   cc3_gpio_set_mode(0, CC3_GPIO_MODE_SERVO); 
    226   cc3_gpio_set_mode(1, CC3_GPIO_MODE_SERVO); 
    227   cc3_gpio_set_mode(2, CC3_GPIO_MODE_SERVO); 
    228   cc3_gpio_set_mode(3, CC3_GPIO_MODE_SERVO); 
     223  cc3_gpio_set_mode (0, CC3_GPIO_MODE_SERVO); 
     224  cc3_gpio_set_mode (1, CC3_GPIO_MODE_SERVO); 
     225  cc3_gpio_set_mode (2, CC3_GPIO_MODE_SERVO); 
     226  cc3_gpio_set_mode (3, CC3_GPIO_MODE_SERVO); 
    229227 
    230228  cc3_pixbuf_set_subsample (CC3_SUBSAMPLE_NEAREST, 2, 1); 
    231229 
    232   if(demo_mode) { 
    233        // Wait for second button press as target lock 
    234         while(1) 
    235         { 
    236                cc3_led_set_on(0); 
    237                cc3_timer_wait_ms(100); 
    238                cc3_led_set_off(0); 
    239                cc3_timer_wait_ms(100); 
    240                if(cc3_button_get_state()==1) break; 
    241        } 
     230  if (demo_mode) { 
     231    // Wait for second button press as target lock 
     232    while (1) { 
     233      cc3_led_set_on (0); 
     234      cc3_timer_wait_ms (100); 
     235      cc3_led_set_off (0); 
     236      cc3_timer_wait_ms (100); 
     237      if (cc3_button_get_state () == 1) 
     238        break; 
     239    } 
    242240 
    243241  } 
    …… 
    247245    printf (":"); 
    248246    error = false; 
    249     if(demo_mode==true) 
    250     { 
    251             n=0; 
    252             command=TRACK_WINDOW; 
     247    if (demo_mode == true) { 
     248      n = 0; 
     249      command = TRACK_WINDOW; 
    253250    } 
    254251    else 
    255            n = cmucam2_get_command (&command, arg_list); 
     252      n = cmucam2_get_command (&command, arg_list); 
    256253    if (n != -1) { 
    257254      switch (command) { 
    …… 
    279276        break; 
    280277 
    281      case LED_0: 
    282        if (n != 1 || arg_list[0]>2 ) { 
    283           error = true; 
    284           break; 
    285         } 
    286         else 
    287           print_ACK (); 
    288           auto_led=false;  
     278      case LED_0: 
     279        if (n != 1 || arg_list[0] > 2) { 
     280          error = true; 
     281          break; 
     282        } 
     283        else 
     284          print_ACK (); 
     285        auto_led = false; 
    289286        if (arg_list[0] == 0) 
    290           cc3_led_set_off(0);  
     287          cc3_led_set_off (0); 
    291288        if (arg_list[0] == 1) 
    292           cc3_led_set_on(0);  
     289          cc3_led_set_on (0); 
    293290        if (arg_list[0] == 2) 
    294           auto_led=true;  
    295        break; 
     291          auto_led = true; 
     292        break; 
    296293 
    297294      case POLL_MODE: 
    …… 
    309306 
    310307 
    311        case HI_RES: 
     308      case HI_RES: 
    312309        if (n != 1) { 
    313310          error = true; 
    …… 
    317314          print_ACK (); 
    318315        if (arg_list[0] == 1) 
    319          cc3_camera_set_resolution(CC3_CAMERA_RESOLUTION_HIGH); 
    320         else 
    321          cc3_camera_set_resolution(CC3_CAMERA_RESOLUTION_LOW); 
    322  
    323        // re-init fifo 
    324        cc3_pixbuf_load(); 
    325         break; 
    326   
    327      case TRACK_INVERT: 
    328         if (n != 1 || arg_list[0]>1 ) { 
    329           error = true; 
    330           break; 
    331         } 
    332         else 
    333           print_ACK (); 
    334         if (arg_list[0] == 0)  
    335                t_pkt.track_invert=0; 
    336        else 
    337                t_pkt.track_invert=1; 
    338         break; 
    339   
    340      case NOISE_FILTER: 
    341         if (n != 1 ) { 
    342           error = true; 
    343           break; 
    344         } 
    345         else 
    346           print_ACK (); 
    347          t_pkt.noise_filter=arg_list[0]; 
     316          cc3_camera_set_resolution (CC3_CAMERA_RESOLUTION_HIGH); 
     317        else 
     318          cc3_camera_set_resolution (CC3_CAMERA_RESOLUTION_LOW); 
     319 
     320        // re-init fifo 
     321        cc3_pixbuf_load (); 
     322        break; 
     323 
     324      case TRACK_INVERT: 
     325        if (n != 1 || arg_list[0] > 1) { 
     326          error = true; 
     327          break; 
     328        } 
     329        else 
     330          print_ACK (); 
     331        if (arg_list[0] == 0) 
     332          t_pkt.track_invert = 0; 
     333        else 
     334          t_pkt.track_invert = 1; 
     335        break; 
     336 
     337      case NOISE_FILTER: 
     338        if (n != 1) { 
     339          error = true; 
     340          break; 
     341        } 
     342        else 
     343          print_ACK (); 
     344        t_pkt.noise_filter = arg_list[0]; 
    348345        break; 
    349346 
    …… 
    373370          print_ACK (); 
    374371        //init_jpeg(); 
    375        // cc3_set_resolution(CC3_RES_HIGH); 
    376        //cc3_pixbuf_set_subsample (CC3_SUBSAMPLE_NEAREST, 1, 1); 
    377  
    378         cc3_jpeg_send_simple(); 
    379        printf( "JPG_END\r" ); 
     372        // cc3_set_resolution(CC3_RES_HIGH); 
     373        //cc3_pixbuf_set_subsample (CC3_SUBSAMPLE_NEAREST, 1, 1); 
     374 
     375        cc3_jpeg_send_simple (); 
     376        printf ("JPG_END\r"); 
    380377        break; 
    381378 
    382379 
    383380      case SEND_FRAME: 
    384        old_coi = cc3_g_pixbuf_frame.coi; 
     381        old_coi = cc3_g_pixbuf_frame.coi; 
    385382        if (n == 1) { 
    386          if (arg_list[0] > 4) { 
     383          if (arg_list[0] > 4) { 
    387384            error = true; 
    388385            break; 
    389386          } 
    390          cc3_pixbuf_set_coi(arg_list[0]); 
    391        } 
     387          cc3_pixbuf_set_coi (arg_list[0]); 
     388        } 
    392389        else if (n > 1) { 
    393390          error = true; 
    …… 
    397394          print_ACK (); 
    398395        cmucam2_send_image_direct (auto_led); 
    399        cc3_pixbuf_set_coi(old_coi); 
     396        cc3_pixbuf_set_coi (old_coi); 
    400397        break; 
    401398 
    …… 
    421418          print_ACK (); 
    422419        cc3_pixbuf_set_roi (arg_list[0] * 2, 
    423                             arg_list[1], 
    424                             arg_list[2] * 2, 
    425                             arg_list[3]); 
    426         break; 
    427  
    428      case GET_TRACK: 
     420                            arg_list[1], arg_list[2] * 2, arg_list[3]); 
     421        break; 
     422 
     423      case GET_TRACK: 
    429424        if (n != 0) { 
    430425          error = true; 
    …… 
    433428        else 
    434429          print_ACK (); 
    435           printf( "%d %d %d %d %d %d\r",t_pkt.lower_bound.channel[0],t_pkt.lower_bound.channel[1], t_pkt.lower_bound.channel[2], 
    436                                         t_pkt.upper_bound.channel[0], t_pkt.upper_bound.channel[1], t_pkt.upper_bound.channel[2] ); 
    437         break; 
     430        printf ("%d %d %d %d %d %d\r", t_pkt.lower_bound.channel[0], 
     431                t_pkt.lower_bound.channel[1], t_pkt.lower_bound.channel[2], 
     432                t_pkt.upper_bound.channel[0], t_pkt.upper_bound.channel[1], 
     433                t_pkt.upper_bound.channel[2]); 
     434        break; 
    438435 
    439436      case GET_WINDOW: 
    …… 
    444441        else 
    445442          print_ACK (); 
    446           printf( "%d %d %d %d\r",cc3_g_pixbuf_frame.x0/2, cc3_g_pixbuf_frame.y0, cc3_g_pixbuf_frame.x1/2, cc3_g_pixbuf_frame.y1 ); 
    447         break; 
     443        printf ("%d %d %d %d\r", cc3_g_pixbuf_frame.x0 / 2, 
     444                cc3_g_pixbuf_frame.y0, cc3_g_pixbuf_frame.x1 / 2, 
     445                cc3_g_pixbuf_frame.y1); 
     446        break; 
    448447 
    449448      case DOWN_SAMPLE: 
    …… 
    454453        else 
    455454          print_ACK (); 
    456         cc3_pixbuf_set_subsample (CC3_SUBSAMPLE_NEAREST, arg_list[0] * 2, arg_list[1]); 
     455        cc3_pixbuf_set_subsample (CC3_SUBSAMPLE_NEAREST, arg_list[0] * 2, 
     456                                  arg_list[1]); 
    457457        break; 
    458458 
    …… 
    473473          t_pkt.upper_bound.channel[2] = arg_list[5]; 
    474474        } 
    475         cmucam2_track_color (&t_pkt, poll_mode, line_mode,auto_led, 0); 
    476         break; 
    477  
    478      case TRACK_WINDOW: 
    479         if (n != 0 && n!=1 ) { 
    480           error = true; 
    481           break; 
    482         } 
    483         else 
    484         { 
    485           uint32_t threshold,x0,y0,x1,y1; 
    486           int32_t tmp; 
    487           threshold=30; 
    488           if(n==1) threshold=arg_list[0]; 
    489           print_ACK (); 
    490           // set window to 1/2 size 
    491           x0=cc3_g_pixbuf_frame.x0 + cc3_g_pixbuf_frame.width/4; 
    492           x1=cc3_g_pixbuf_frame.x1 - cc3_g_pixbuf_frame.width/4; 
    493           y0=cc3_g_pixbuf_frame.y0 + cc3_g_pixbuf_frame.width/4; 
    494           y1=cc3_g_pixbuf_frame.y1 - cc3_g_pixbuf_frame.width/4; 
    495           cc3_pixbuf_set_roi ( x0, y0 ,x1 ,y1 );  
    496           // call get mean 
    497           cmucam2_get_mean (&s_pkt, 1, line_mode,1); 
    498           // set window back to full size 
    499           x0=0; 
    500           x1=cc3_g_pixbuf_frame.raw_width;   
    501           y0=0; 
    502           y1=cc3_g_pixbuf_frame.raw_height; 
    503           cc3_pixbuf_set_roi ( x0, y0 ,x1 ,y1 );  
    504           // fill in parameters and call track color 
    505           tmp= s_pkt.mean.channel[0]-threshold; if(tmp<16) tmp=16; if(tmp>240) tmp=240; t_pkt.lower_bound.channel[0] = tmp;  
    506           tmp= s_pkt.mean.channel[0]+threshold; if(tmp<16) tmp=16; if(tmp>240) tmp=240; t_pkt.upper_bound.channel[0] = tmp;  
    507           tmp= s_pkt.mean.channel[1]-threshold; if(tmp<16) tmp=16; if(tmp>240) tmp=240;t_pkt.lower_bound.channel[1] = tmp;  
    508           tmp= s_pkt.mean.channel[1]+threshold; if(tmp<16) tmp=16; if(tmp>240) tmp=240; t_pkt.upper_bound.channel[1] = tmp; 
    509           tmp= s_pkt.mean.channel[2]-threshold; if(tmp<16) tmp=16; if(tmp>240) tmp=240;t_pkt.lower_bound.channel[2] = tmp; 
    510           tmp= s_pkt.mean.channel[2]+threshold; if(tmp<16) tmp=16; if(tmp>240) tmp=240; t_pkt.upper_bound.channel[2] = tmp; 
    511         cmucam2_track_color (&t_pkt, poll_mode, line_mode,auto_led,0); 
    512         } 
    513         demo_mode=false; 
    514         break; 
    515  
    516          
     475        cmucam2_track_color (&t_pkt, poll_mode, line_mode, auto_led, 0); 
     476        break; 
     477 
     478      case TRACK_WINDOW: 
     479        if (n != 0 && n != 1) { 
     480          error = true; 
     481          break; 
     482        } 
     483        else { 
     484          uint32_t threshold, x0, y0, x1, y1; 
     485          int32_t tmp; 
     486          threshold = 30; 
     487          if (n == 1) 
     488            threshold = arg_list[0]; 
     489          print_ACK (); 
     490          // set window to 1/2 size 
     491          x0 = cc3_g_pixbuf_frame.x0 + cc3_g_pixbuf_frame.width / 4; 
     492          x1 = cc3_g_pixbuf_frame.x1 - cc3_g_pixbuf_frame.width / 4; 
     493          y0 = cc3_g_pixbuf_frame.y0 + cc3_g_pixbuf_frame.width / 4; 
     494          y1 = cc3_g_pixbuf_frame.y1 - cc3_g_pixbuf_frame.width / 4; 
     495          cc3_pixbuf_set_roi (x0, y0, x1, y1); 
     496          // call get mean 
     497          cmucam2_get_mean (&s_pkt, 1, line_mode, 1); 
     498          // set window back to full size 
     499          x0 = 0; 
     500          x1 = cc3_g_pixbuf_frame.raw_width; 
     501          y0 = 0; 
     502          y1 = cc3_g_pixbuf_frame.raw_height; 
     503          cc3_pixbuf_set_roi (x0, y0, x1, y1); 
     504          // fill in parameters and call track color 
     505          tmp = s_pkt.mean.channel[0] - threshold; 
     506          if (tmp < 16) 
     507            tmp = 16; 
     508          if (tmp > 240) 
     509            tmp = 240; 
     510          t_pkt.lower_bound.channel[0] = tmp; 
     511          tmp = s_pkt.mean.channel[0] + threshold; 
     512          if (tmp < 16) 
     513            tmp = 16; 
     514          if (tmp > 240) 
     515            tmp = 240; 
     516          t_pkt.upper_bound.channel[0] = tmp; 
     517          tmp = s_pkt.mean.channel[1] - threshold; 
     518          if (tmp < 16) 
     519            tmp = 16; 
     520          if (tmp > 240) 
     521            tmp = 240; 
     522          t_pkt.lower_bound.channel[1] = tmp; 
     523          tmp = s_pkt.mean.channel[1] + threshold; 
     524          if (tmp < 16) 
     525            tmp = 16; 
     526          if (tmp > 240) 
     527            tmp = 240; 
     528          t_pkt.upper_bound.channel[1] = tmp; 
     529          tmp = s_pkt.mean.channel[2] - threshold; 
     530          if (tmp < 16) 
     531            tmp = 16; 
     532          if (tmp > 240) 
     533            tmp = 240; 
     534          t_pkt.lower_bound.channel[2] = tmp; 
     535          tmp = s_pkt.mean.channel[2] + threshold; 
     536          if (tmp < 16) 
     537            tmp = 16; 
     538          if (tmp > 240) 
     539            tmp = 240; 
     540          t_pkt.upper_bound.channel[2] = tmp; 
     541          cmucam2_track_color (&t_pkt, poll_mode, line_mode, auto_led, 0); 
     542        } 
     543        demo_mode = false; 
     544        break; 
     545 
     546 
    517547      case GET_POLLY: 
    518         if (n != 6 ) { 
    519           error = true; 
    520           break; 
    521         } 
    522         else 
    523           print_ACK (); 
    524         { 
    525                 uint8_t *x_axis; 
    526                 polly_config_t p_config; 
    527                 cc3_linear_reg_data_t reg_line; 
    528                 x_axis = malloc(cc3_g_pixbuf_frame.width); 
    529           
    530                 p_config.color_thresh=arg_list[0]; //20; 
    531                 p_config.min_blob_size=arg_list[1]; //20; 
    532                 p_config.connectivity=arg_list[2]; //0; 
    533                 p_config.horizontal_edges=arg_list[3]; //0; 
    534                 p_config.vertical_edges=arg_list[4]; //1; 
    535                 p_config.blur=arg_list[5]; //1; 
    536                 p_config.histogram=malloc(cc3_g_pixbuf_frame.width);  
    537                 for(uint32_t i=0; i<cc3_g_pixbuf_frame.width; i++ ) x_axis[i]=i; 
    538                 do { 
    539                         polly(p_config); 
    540                         cc3_linear_reg(x_axis, p_config.histogram, cc3_g_pixbuf_frame.width,&reg_line); 
    541  
    542                         // return linear regression offset value    
    543                         printf( "P %f ",reg_line.b );   
    544                         // return linear regression slope 
    545                         printf( "%f ",reg_line.m );     
    546                         // return r squared value        
    547                         printf( "%f ",reg_line.r_sqr );      
    548                         // return distance to line at middle of image 
    549                         double distance=reg_line.m*(cc3_g_pixbuf_frame.width/2)+reg_line.b; 
    550                         printf( " %f\r",distance );  
    551      
    552                         if (!cc3_uart_has_data (0)) 
    553                                 break; 
    554                 } while (!poll_mode); 
    555                 free(p_config.histogram);  
    556         } 
     548        if (n != 6) { 
     549          error = true; 
     550          break; 
     551        } 
     552        else 
     553          print_ACK (); 
     554        { 
     555          uint8_t *x_axis; 
     556          polly_config_t p_config; 
     557          cc3_linear_reg_data_t reg_line; 
     558          x_axis = malloc (cc3_g_pixbuf_frame.width); 
     559 
     560          p_config.color_thresh = arg_list[0];  //20; 
     561          p_config.min_blob_size = arg_list[1]; //20; 
     562          p_config.connectivity = arg_list[2];  //0; 
     563          p_config.horizontal_edges = arg_list[3];      //0; 
     564          p_config.vertical_edges = arg_list[4];        //1; 
     565          p_config.blur = arg_list[5];  //1; 
     566          p_config.histogram = malloc (cc3_g_pixbuf_frame.width); 
     567          for (uint32_t i = 0; i < cc3_g_pixbuf_frame.width; i++) 
     568            x_axis[i] = i; 
     569          do { 
     570            polly (p_config); 
     571            cc3_linear_reg (x_axis, p_config.histogram, 
     572                            cc3_g_pixbuf_frame.width, &reg_line); 
     573 
     574            // return linear regression offset value 
     575            printf ("P %f ", reg_line.b); 
     576            // return linear regression slope 
     577            printf ("%f ", reg_line.m); 
     578            // return r squared value 
     579            printf ("%f ", reg_line.r_sqr); 
     580            // return distance to line at middle of image 
     581            double distance = 
     582              reg_line.m * (cc3_g_pixbuf_frame.width / 2) + reg_line.b; 
     583            printf (" %f\r", distance); 
     584 
     585            if (!cc3_uart_has_data (0)) 
     586              break; 
     587          } while (!poll_mode); 
     588          free (p_config.histogram); 
     589        } 
    557590        break; 
    558591 
    …… 
    565598        else 
    566599          print_ACK (); 
    567         cmucam2_get_mean (&s_pkt, poll_mode, line_mode,0); 
    568         break; 
    569  
    570          
     600        cmucam2_get_mean (&s_pkt, poll_mode, line_mode, 0); 
     601        break; 
     602 
     603 
    571604      case GET_HISTOGRAM: 
    572         if (n != 1 || arg_list[0]>2) { 
    573           error = true; 
    574           break; 
    575         } 
    576         else 
    577           print_ACK (); 
    578          h_pkt.channel=arg_list[0]; 
    579           cmucam2_get_histogram(&h_pkt, poll_mode, 0); 
     605        if (n != 1 || arg_list[0] > 2) { 
     606          error = true; 
     607          break; 
     608        } 
     609        else 
     610          print_ACK (); 
     611        h_pkt.channel = arg_list[0]; 
     612        cmucam2_get_histogram (&h_pkt, poll_mode, 0); 
    580613        break; 
    581614 
    …… 
    588621        else 
    589622          print_ACK (); 
    590        cc3_gpio_set_mode(arg_list[0], CC3_GPIO_MODE_SERVO); 
     623        cc3_gpio_set_mode (arg_list[0], CC3_GPIO_MODE_SERVO); 
    591624        cc3_gpio_set_servo_position (arg_list[0], arg_list[1]); 
    592625        break; 
    593626 
    594627      case SERVO_OUTPUT: 
    595        if (n != 2) { 
    596          error = true; 
    597          break; 
    598        } 
    599        print_ACK (); 
    600        cc3_gpio_set_mode(arg_list[0], CC3_GPIO_MODE_OUTPUT); 
    601        cc3_gpio_set_value(arg_list[0], arg_list[1]); 
    602  
    603        break; 
     628        if (n != 2) { 
     629          error = true; 
     630          break; 
     631        } 
     632        print_ACK (); 
     633        cc3_gpio_set_mode (arg_list[0], CC3_GPIO_MODE_OUTPUT); 
     634        cc3_gpio_set_value (arg_list[0], arg_list[1]); 
     635 
     636        break; 
    604637 
    605638      default: 
    …… 
    626659  uint32_t x, y; 
    627660  uint32_t size_x, size_y; 
    628   uint8_t *row = cc3_malloc_rows(1); 
     661  uint8_t *row = cc3_malloc_rows (1); 
    629662  uint8_t num_channels = cc3_g_pixbuf_frame.coi == CC3_CHANNEL_ALL ? 3 : 1; 
    630663 
    …… 
    640673  for (y = 0; y < size_y; y++) { 
    641674    putchar (2); 
    642     if(auto_led) 
    643            { 
    644                 if(y%4==0) cc3_led_set_on(0); 
    645                 else cc3_led_set_off(0); 
    646            }  
    647     cc3_pixbuf_read_rows(row, 1); 
     675    if (auto_led) { 
     676      if (y % 4 == 0) 
     677        cc3_led_set_on (0); 
     678      else 
     679        cc3_led_set_off (0); 
     680    } 
     681    cc3_pixbuf_read_rows (row, 1); 
    648682    for (x = 0; x < size_x * num_channels; x++) { 
    649683      uint8_t p = row[x]; 
    …… 
    652686  } 
    653687  putchar (3); 
    654    
    655   cc3_led_set_off(0); 
    656   free(row); 
    657 } 
    658  
    659  
    660  
    661 void cmucam2_get_histogram(cc3_histogram_pkt_t *h_pkt, bool poll_mode, bool quite) 
     688 
     689  cc3_led_set_off (0); 
     690  free (row); 
     691} 
     692 
     693 
     694 
     695void cmucam2_get_histogram (cc3_histogram_pkt_t * h_pkt, bool poll_mode, 
     696                            bool quite) 
    662697{ 
    663698  cc3_image_t img; 
    …… 
    666701  img.height = 1;               // image will hold just 1 row for scanline processing 
    667702  img.pix = malloc (3 * img.width); 
    668   h_pkt->hist=malloc(h_pkt->bins*sizeof(uint32_t)); 
     703  h_pkt->hist = malloc (h_pkt->bins * sizeof (uint32_t)); 
    669704  do { 
    670705    cc3_pixbuf_load (); 
    …… 
    674709      } 
    675710      cc3_histogram_scanline_finish (h_pkt); 
    676       while (!cc3_uart_has_data (0)) { if(fgetc(stdin)=='\r' ) free(img.pix); free(h_pkt->hist); return; } 
    677       if(!quite) cmucam2_write_h_packet (h_pkt); 
    678     } 
    679     if (!cc3_uart_has_data (0)) 
    680     { 
    681       if(fgetc(stdin)=='\r' ) 
    682         break; 
     711      while (!cc3_uart_has_data (0)) { 
     712        if (fgetc (stdin) == '\r') 
     713          free (img.pix); 
     714        free (h_pkt->hist); 
     715        return; 
     716      } 
     717      if (!quite) 
     718        cmucam2_write_h_packet (h_pkt); 
     719    } 
     720    if (!cc3_uart_has_data (0)) { 
     721      if (fgetc (stdin) == '\r') 
     722        break; 
    683723    } 
    684724  } while (!poll_mode); 
    …… 
    692732 
    693733void cmucam2_get_mean (cc3_color_info_pkt_t * s_pkt, 
    694                        bool poll_mode, 
    695                        bool line_mode, bool quite) 
     734                       bool poll_mode, bool line_mode, bool quite) 
    696735{ 
    697736  cc3_image_t img; 
    …… 
    707746      } 
    708747      cc3_color_info_scanline_finish (s_pkt); 
    709       while (!cc3_uart_has_data (0)) { if(fgetc(stdin)=='\r' ) free(img.pix); return; } 
    710       if(!quite) cmucam2_write_s_packet (s_pkt); 
    711     } 
    712     if (!cc3_uart_has_data (0)) 
    713     { 
    714       if(fgetc(stdin)=='\r' ) 
    715         break; 
     748      while (!cc3_uart_has_data (0)) { 
     749        if (fgetc (stdin) == '\r') 
     750          free (img.pix); 
     751        return; 
     752      } 
     753      if (!quite) 
     754        cmucam2_write_s_packet (s_pkt); 
     755    } 
     756    if (!cc3_uart_has_data (0)) { 
     757      if (fgetc (stdin) == '\r') 
     758        break; 
    716759    } 
    717760  } while (!poll_mode); 
    …… 
    721764 
    722765void cmucam2_track_color (cc3_track_pkt_t * t_pkt, 
    723                          bool poll_mode, 
    724                           bool line_mode, bool auto_led,bool quite) 
     766                          bool poll_mode, 
     767                          bool line_mode, bool auto_led, bool quite) 
    725768{ 
    726769  cc3_image_t img; 
    …… 
    730773  img.width = cc3_g_pixbuf_frame.width; 
    731774  img.height = 1;               // image will hold just 1 row for scanline processing 
    732   img.pix = cc3_malloc_rows(1); 
    733   if(img.pix==NULL ) { 
    734        return; 
     775  img.pix = cc3_malloc_rows (1); 
     776  if (img.pix == NULL) { 
     777    return; 
    735778  } 
    736779  do { 
    …… 
    746789        if (img.width % 8 != 0) 
    747790          lm_width++; 
    748         if(!quite) putchar (0xAA); 
     791        if (!quite) 
     792          putchar (0xAA); 
    749793        if (cc3_g_pixbuf_frame.height > 255) 
    750794          lm_height = 255; 
    751795        else 
    752796          lm_height = cc3_g_pixbuf_frame.height; 
    753         if(!quite) putchar (img.width); 
    754         if(!quite) putchar (lm_height); 
     797        if (!quite) 
     798          putchar (img.width); 
     799        if (!quite) 
     800          putchar (lm_height); 
    755801      } 
    756802      while (cc3_pixbuf_read_rows (img.pix, 1)) { 
    757803        cc3_track_color_scanline (&img, t_pkt); 
    758         if (line_mode) { 
    759            // keep this check here if you don't want the CMUcam2 GUI to hang after exiting a command in line mode 
    760            while (!cc3_uart_has_data (0)) { if(fgetc(stdin)=='\r' ) free(img.pix); return; } 
    761            for (int j = 0; j < lm_width; j++) { 
    762             if (lm[j] == 0xAA) 
    763             { 
    764               if(!quite) putchar (0xAB); 
    765             } else 
    766             { 
    767               if(!quite) putchar (lm[j]); 
    768             } 
     804        if (line_mode) { 
     805          // keep this check here if you don't want the CMUcam2 GUI to hang after exiting a command in line mode 
     806          while (!cc3_uart_has_data (0)) { 
     807            if (fgetc (stdin) == '\r') 
     808              free (img.pix); 
     809            return; 
     810          } 
     811          for (int j = 0; j < lm_width; j++) { 
     812            if (lm[j] == 0xAA) { 
     813              if (!quite) 
     814                putchar (0xAB); 
     815            } 
     816            else { 
     817              if (!quite) 
     818                putchar (lm[j]); 
     819            } 
    769820          } 
    770821        } 
    771822      } 
    772823      // keep this check here if you don't want the CMUcam2 GUI to hang after exiting a command in line mode 
    773       while (!cc3_uart_has_data (0)) { if(fgetc(stdin)=='\r' ) free(img.pix); return; } 
     824      while (!cc3_uart_has_data (0)) { 
     825        if (fgetc (stdin) == '\r') 
     826          free (img.pix); 
     827        return; 
     828      } 
    774829      cc3_track_color_scanline_finish (t_pkt); 
    775830      if (line_mode) { 
    776         if(!quite) putchar (0xAA); 
    777         if(!quite) putchar (0xAA); 
     831        if (!quite) 
     832          putchar (0xAA); 
     833        if (!quite) 
     834          putchar (0xAA); 
    778835      } 
    779       if(auto_led) { 
    780         if(t_pkt->int_density>2 ) cc3_led_set_on(0); 
    781         else cc3_led_set_off(0); 
     836      if (auto_led) { 
     837        if (t_pkt->int_density > 2) 
     838          cc3_led_set_on (0); 
     839        else 
     840          cc3_led_set_off (0); 
    782841      } 
    783       if(!quite) cmucam2_write_t_packet (t_pkt); 
    784     } else return 0; 
    785     while (!cc3_uart_has_data (0)) 
    786     { 
    787       if(fgetc(stdin)=='\r' ) 
    788         break; 
     842      if (!quite) 
     843        cmucam2_write_t_packet (t_pkt); 
     844    } 
     845    else 
     846      return 0; 
     847    while (!cc3_uart_has_data (0)) { 
     848      if (fgetc (stdin) == '\r') 
     849        break; 
    789850    } 
    790851  } while (!poll_mode); 
    …… 
    822883} 
    823884 
    824 void cmucam2_write_h_packet (cc3_histogram_pkt_t *pkt) 
    825 { 
    826 uint32_t i; 
    827 uint32_t total_pix; 
    828  
    829   total_pix=cc3_g_pixbuf_frame.width*cc3_g_pixbuf_frame.height; 
    830   printf ("H" ); 
    831   for(i=0; i<pkt->bins; i++ ) 
    832   { 
    833         pkt->hist[i]=(pkt->hist[i]*256)/total_pix; 
    834        if(pkt->hist[i]>255) pkt->hist[i]=255; 
    835        printf( " %d",pkt->hist[i] ); 
     885void cmucam2_write_h_packet (cc3_histogram_pkt_t * pkt) 
     886{ 
     887  uint32_t i; 
     888  uint32_t total_pix; 
     889 
     890  total_pix = cc3_g_pixbuf_frame.width * cc3_g_pixbuf_frame.height; 
     891  printf ("H"); 
     892  for (i = 0; i < pkt->bins; i++) { 
     893    pkt->hist[i] = (pkt->hist[i] * 256) / total_pix; 
     894    if (pkt->hist[i] > 255) 
     895      pkt->hist[i] = 255; 
     896    printf (" %d", pkt->hist[i]); 
    836897  } 
    837  printf( "\r" );  
     898  printf ("\r"); 
    838899} 
    839900 
    …… 
    871932  *cmd = 0; 
    872933  c = 0; 
    873   while (c != '\r' ) { 
     934  while (c != '\r') { 
    874935    c = fgetc (stdin); 
    875936    if (length < (MAX_LINE - 1)) {