Changeset 433

Timestamp:

02/18/07 16:53:30 (2 years ago)

Author:

anthony_rowe

Message:

auto servo mode

Files:

• trunk/projects/cmucam2/cmucam2.c (modified) (21 diffs)

trunk/projects/cmucam2/cmucam2.c

@@ -12 +12 @@
-#include <cc3_jpg.h>
-#include <cc3_math.h>
-
+
+
+
-
-//#define SERIAL_BAUD_RATE  CC3_UART_RATE_230400
@@ -25 +27 @@
-//#define SERIAL_BAUD_RATE  CC3_UART_RATE_300
-
+#define SERVO_MIN 0
+#define SERVO_MID 128
+#define SERVO_MAX 255
+// Define a jitter guard such that more than SERVO_GUARD pixels are required
+// for the servo to move.
-
-static const int MAX_ARGS = 10;
@@ -47 +54 @@
-  VIRTUAL_WINDOW,
-  DOWN_SAMPLE,
-  GET_POLLY,
-  GET_HISTOGRAM,
-  TRACK_WINDOW,
@@ -55 +61 @@
-  NOISE_FILTER,
-  TRACK_INVERT,
+  SERVO_MASK,
+  SERVO_PARAMETERS,
-  SERVO_OUTPUT,
-  GET_SERVO,
-  CMUCAM2_CMD_END               // Must be last entry so array sizes are correct
-} cmucam2_command_t;
+
+typedef struct {
+uint8_t pan_step,tilt_step;
+uint8_t pan_range_near,tilt_range_near;
+uint8_t pan_range_far,tilt_range_far;
+int16_t x;
+int16_t y;
+bool y_control;
+bool x_control;
+bool y_report;
+bool x_report;
+} cmucam2_servo_t;
+
-
-char *cmucam2_cmds[CMUCAM2_CMD_END];
@@ -89 +110 @@
-  //  SM servo mask
-  cmucam2_cmds[SERVO_OUTPUT] = "SO";
-
+
+
+
+
+
+
+
-  /* Image Windowing Commands */
-  cmucam2_cmds[SEND_FRAME] = "SF";
@@ -138 +165 @@
-  /* CMUcam3 New Commands */
-  cmucam2_cmds[SEND_JPEG] = "SJ";
-
+//
-}
-
@@ -148 +175 @@
-static void cmucam2_write_s_packet (cc3_color_info_pkt_t * pkt);
-static void cmucam2_track_color (cc3_track_pkt_t * t_pkt,
-                                
-                                 bool line_mode, bool auto_led
+                               
+                                bool line_mode, bool auto_led, cmucam2_servo_t *servo_settings
-static int32_t cmucam2_get_command (int32_t * cmd, int32_t * arg_list);
-static void print_ACK (void);
-static void print_NCK (void);
-
+, cmucam2_servo_t *servo_settings
-static void cmucam2_write_h_packet (cc3_histogram_pkt_t * pkt);
-static void cmucam2_send_image_direct (bool auto_led);
+
+
-
-int main (void)
@@ -162 +191 @@
-  int32_t val, n;
-  uint32_t arg_list[MAX_ARGS], start_time;
-, auto_servo_mode
+
-  cc3_track_pkt_t t_pkt;
-  cc3_color_info_pkt_t s_pkt;
-  cc3_histogram_pkt_t h_pkt;
+  cmucam2_servo_t servo_settings;
-
-  set_cmucam2_commands ();
@@ -181 +211 @@
-  }
-
+  servo_settings.x_control=false;
+  servo_settings.y_control=false;
+  servo_settings.x_report=false;
+  servo_settings.y_report=false;
-  demo_mode = false;
- auto_servo_mode = false;
+
-  start_time = cc3_timer_get_current_ms ();
+  
-  do {
-    if (cc3_button_get_state () == 1) {
-      // Demo Mode flag
-      demo_mode = true;
-
+
+
+
+
-      // Debounce Switch
-      cc3_led_set_off (0);
@@ -211 +249 @@
-  t_pkt.lower_bound.channel[2] = 16;
-  t_pkt.upper_bound.channel[2] = 240;
+ 
+  
+  servo_settings.x=SERVO_MID;
+  servo_settings.y=SERVO_MID;
+  servo_settings.pan_range_far=16;
+  servo_settings.pan_range_near=8;
+  servo_settings.pan_step=5;
+  servo_settings.tilt_range_far=30;
+  servo_settings.tilt_range_near=15;
+  servo_settings.tilt_step=5;
-
-
@@ -223 +271 @@
-  cc3_gpio_set_mode (2, CC3_GPIO_MODE_SERVO);
-  cc3_gpio_set_mode (3, CC3_GPIO_MODE_SERVO);
+  
+  cc3_gpio_set_servo_position (0, SERVO_MID);
+  cc3_gpio_set_servo_position (1, SERVO_MID);
+  cc3_gpio_set_servo_position (2, SERVO_MID);
+  cc3_gpio_set_servo_position (3, SERVO_MID);
-
-  cc3_pixbuf_set_subsample (CC3_SUBSAMPLE_NEAREST, 2, 1);
@@ -303 +356 @@
-        break;
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-        if (n != 1) {
-          error = true;
@@ -471 +551 @@
-          t_pkt.upper_bound.channel[2] = arg_list[5];
-        }
- auto_led
+auto_led,&servo_settings
-        break;
-
@@ -537 +617 @@
-            tmp = 240;
-          t_pkt.upper_bound.channel[2] = tmp;
-
+&servo_settings,
-        }
-        demo_mode = false;
-        break;
-
-
-      case GET_POLLY:
-        if (n != 6) {
-          error = true;
-          break;
-        }
-
-        print_ACK ();
-        {
-          uint8_t *x_axis;
-          polly_config_t p_config;
-          cc3_linear_reg_data_t reg_line;
-          x_axis = malloc (cc3_g_pixbuf_frame.width);
-
-          p_config.color_thresh = arg_list[0];  //20;
-          p_config.min_blob_size = arg_list[1]; //20;
-          p_config.connectivity = arg_list[2];  //0;
-          p_config.horizontal_edges = arg_list[3];      //0;
-          p_config.vertical_edges = arg_list[4];        //1;
-          p_config.blur = arg_list[5];  //1;
-          p_config.histogram = malloc (cc3_g_pixbuf_frame.width);
-          for (uint32_t i = 0; i < cc3_g_pixbuf_frame.width; i++)
-            x_axis[i] = i;
-          do {
-            polly (p_config);
-            cc3_linear_reg (x_axis, p_config.histogram,
-                            cc3_g_pixbuf_frame.width, &reg_line);
-
-            // return linear regression offset value
-            printf ("P %f ", reg_line.b);
-            // return linear regression slope
-            printf ("%f ", reg_line.m);
-            // return r squared value
-            printf ("%f ", reg_line.r_sqr);
-            // return distance to line at middle of image
-            double distance =
-              reg_line.m * (cc3_g_pixbuf_frame.width / 2) + reg_line.b;
-            printf (" %f\r", distance);
-
-            if (!cc3_uart_has_data (0))
-              break;
-          } while (!poll_mode);
-          free (p_config.histogram);
-        }
-        break;
-
@@ -613 +647 @@
-
-      case SET_SERVO:
-
+ || arg_list[0]>4
-          error = true;
-          break;
@@ -621 +655 @@
-        cc3_gpio_set_mode (arg_list[0], CC3_GPIO_MODE_SERVO);
-        cc3_gpio_set_servo_position (arg_list[0], arg_list[1]);
+        if(arg_list[0]==0) servo_settings.x=arg_list[1];
+        if(arg_list[0]==1) servo_settings.y=arg_list[1];
-        break;
-
@@ -772 +808 @@
-
-void cmucam2_track_color (cc3_track_pkt_t * t_pkt,
-                        
-
+                       
+cmucam2_servo_t *servo_settings,
-{
-  cc3_image_t img;
-
+,x_mid,y_mid
-
-  img.channels = 3;
@@ -785 +821 @@
-    return;
-  }
+
+  x_mid=cc3_g_pixbuf_frame.x0 + (cc3_g_pixbuf_frame.width/2);
+  y_mid=cc3_g_pixbuf_frame.y0 + (cc3_g_pixbuf_frame.height/2);
+
+  
-  do {
-    cc3_pixbuf_load ();
@@ -848 +889 @@
-          cc3_led_set_off (0);
-      }
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-    }
-  } while (!poll_mode);
@@ -863 +932 @@
-
-
-
+, cmucam2_servo_t *servo_settings
-{
-  if (pkt->centroid_x > 255)
@@ -883 +952 @@
-
-  if (pkt->num_pixels == 0)
-\r
+
-  else
-\r
+
-            pkt->x0, pkt->y0, pkt->x1, pkt->y1, pkt->num_pixels,
-            pkt->int_density);
+  if(servo_settings->x_report) printf( " %d",servo_settings->x );
+  if(servo_settings->y_report) printf( " %d",servo_settings->y );
+  printf( "\r" );
-
-}