cmucam3-hardware (#1) - simple-track-color (#198) - Message List

simple-track-color

Hi...

I manage to this simple-track-color works.. not great but at least the centroid change when i move the object..

here what you need to add.. please let me know if you can make it work better. thanks 

/* dont forget to add lib on the top */

#include <cc3.h>

#include <cc3_ilp.h>

#include <cc3_color_track.h>

#include <cc3_color_info.h>

#include <cc3_frame_diff.h>


void simple_track_color(cc3_track_pkt_t *t_pkt);

int main(void) {
  cc3_track_pkt_t t_pkt;
  uint32_t x0, y0, x1, y1;
  cc3_frame_diff_pkt_t fd_pkt; 

  cc3_uart_init (0, CC3_UART_RATE_115200,
                 CC3_UART_MODE_8N1, CC3_UART_BINMODE_TEXT);

  cc3_camera_init ();
  

  cc3_camera_set_resolution(CC3_CAMERA_RESOLUTION_LOW);
  cc3_pixbuf_frame_set_subsample(CC3_SUBSAMPLE_NEAREST, 2, 1);
  


  // init pixbuf with full size width and height

  x0 = 0;

  x1 = cc3_g_pixbuf_frame.raw_width;

  y0 = 0;

  y1 = cc3_g_pixbuf_frame.raw_height;



  fd_pkt.coi = 1;

  fd_pkt.template_width = 8;

  fd_pkt.template_height = 8;

  t_pkt.track_invert = false;

  t_pkt.noise_filter = 0;


  cc3_pixbuf_frame_set_roi (x0, y0, x1, y1);


  // Load in color tracking parameters

  t_pkt.lower_bound.channel[0] = 164;

  t_pkt.upper_bound.channel[0] = 214;

  t_pkt.lower_bound.channel[1] = 0;

  t_pkt.upper_bound.channel[1] = 41;

  t_pkt.lower_bound.channel[2] = 0;

  t_pkt.upper_bound.channel[2] = 41;
 

  while(true) {

    simple_track_color(&t_pkt);

    printf("\n centroid=[%d,%d] bounding box=[%d,%d,%d,%d] num pix=[%d] density=[%d] \n",
                    t_pkt.centroid_x, t_pkt.centroid_y,
                    t_pkt.x0,t_pkt.y0,t_pkt.x1,t_pkt.y1,
                    t_pkt.num_pixels, t_pkt.int_density );

   }
  
}

void simple_track_color(cc3_track_pkt_t *t_pkt)
{
  cc3_image_t img;

  img.channels = 3;

  img.width = cc3_g_pixbuf_frame.width;

  img.height = 1;  // image will hold just 1 row for scanline processing

  img.pix = cc3_malloc_rows (1);

  if (img.pix == NULL) {

    return;

  }

    cc3_pixbuf_load ();
    if (cc3_track_color_scanline_start (t_pkt) != 0) {
      while (cc3_pixbuf_read_rows (img.pix, 1)) {
           // This does the HSV conversion 

           // cc3_rgb2hsv_row(img.pix,img.width);

           cc3_track_color_scanline (&img, t_pkt);
          }
        }
    cc3_track_color_scanline_finish (t_pkt);
        
  free (img.pix);
  return;
}
wikki
04/27/08 04:53:55
Tree View Flat View (newer first) Flat View (older first)
  • Message #510

    hello,

    I'm lookig for a simple color tracking program for a cmucam 3, do you know where I can find this kind of program ? does it exit or should I program it by myself?

    lvpk
    05/05/08 07:23:24
  • Message #511

    hello,

    I'm lookig for a simple color tracking program for a cmucam 3, do you know where I can find this kind of program ? does it exit or should I program it by myself?

    lvpk
    05/05/08 07:23:27
  • Message #526

    Nice work!

    Can you please give an example on using Pan and Tilt to make the robot look at the object all the time I will be glad =)

    Thanks!

    xsocom
    05/14/08 12:17:27
  • Message #527 
    #include <cc3.h>
#include <cc3_ilp.h>
#include <cc3_color_track.h>
#include <cc3_color_info.h>
#include <cc3_histogram.h>
#include <cc3_frame_diff.h>
#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include <stdlib.h>
#include <cc3_jpg.h>
#include <cc3_math.h>
#include <cc3_hsv.h>
 

#define SERVO_REVERSE_DIRECTION_PAN
#define SERVO_REVERSE_DIRECTION_TILT


#define SERIAL_BAUD_RATE  CC3_UART_RATE_115200


// Define a jitter guard such that more than SERVO_GUARD pixels are required
// for the servo to move.
#define SERVO_MIN 0
#define SERVO_MID 128
#define SERVO_MAX 255

#define DEFAULT_COLOR 0
#define HSV_COLOR     1


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;


static const unsigned int MAX_ARGS = 10;
static const unsigned int MAX_LINE = 128;


static const char VERSION_BANNER[] = "CMUcam2 v1.00 c6";

static uint8_t t_pkt_mask;

static void cmucam2_write_t_packet (cc3_track_pkt_t * pkt, cmucam2_servo_t * servo_settings);

static void cmucam2_track_color (cc3_track_pkt_t * t_pkt,
                        bool poll_mode,
                        int8_t line_mode, bool auto_led,
                        cmucam2_servo_t * servo_settings,
                        bool buf_mode, uint8_t sw_color_space, bool quiet);



int main(void) {
  uint8_t sw_color_space;
  bool poll_mode, auto_led, buf_mode;
  int8_t line_mode;
  cc3_track_pkt_t t_pkt;
  cc3_frame_diff_pkt_t fd_pkt;
  cmucam2_servo_t servo_settings;

  uint32_t x0, y0, x1, y1; 

  cc3_uart_init (0, CC3_UART_RATE_115200, CC3_UART_MODE_8N1, 
                 CC3_UART_BINMODE_TEXT);

  cc3_camera_init ();

  servo_settings.x_control = false;
  servo_settings.y_control = false;
  servo_settings.x_report = false;
  servo_settings.y_report = false;

  servo_settings.x = SERVO_MID;
  servo_settings.y = SERVO_MID;


  servo_settings.pan_range_far = 32;
  servo_settings.pan_range_near = 20;
  servo_settings.pan_step = 20;
  servo_settings.tilt_range_far = 32;
  servo_settings.tilt_range_near = 20;
  servo_settings.tilt_step = 20;

  cc3_camera_set_power_state (true);
  cc3_camera_set_resolution (CC3_CAMERA_RESOLUTION_LOW);
  printf ("%s\r", VERSION_BANNER);


  cc3_gpio_set_mode (0, CC3_GPIO_MODE_SERVO);
  cc3_gpio_set_mode (1, CC3_GPIO_MODE_SERVO);
  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_frame_set_subsample (CC3_SUBSAMPLE_NEAREST, 2, 1);


  //cc3_camera_set_colorspace(CC3_COLORSPACE_YCRCB);
  cc3_camera_set_resolution(CC3_CAMERA_RESOLUTION_LOW);
  //cc3_pixbuf_frame_set_subsample(CC3_SUBSAMPLE_NEAREST, 2, 2);
  
  sw_color_space=DEFAULT_COLOR;
  auto_led = true;
  poll_mode = false;
  line_mode = 0;
  buf_mode = false;

  t_pkt_mask = 0xFF;
  fd_pkt.coi = 1;
  fd_pkt.template_width = 8;
  fd_pkt.template_height = 8;
  t_pkt.track_invert = false;
  t_pkt.noise_filter = 0;

  servo_settings.x_control = true;
  servo_settings.y_control = true;
  servo_settings.x_report = true;
  servo_settings.y_report = true;

  // init pixbuf with full size width and height
  x0 = 0;
  x1 = cc3_g_pixbuf_frame.raw_width;
  y0 = 0;
  y1 = cc3_g_pixbuf_frame.raw_height;
  cc3_pixbuf_frame_set_roi (x0, y0, x1, y1);
  // Load in color tracking parameters
  t_pkt.lower_bound.channel[0] = 89;
  t_pkt.upper_bound.channel[0] = 139;
  t_pkt.lower_bound.channel[1] = 0;
  t_pkt.upper_bound.channel[1] = 41;
  t_pkt.lower_bound.channel[2] = 0;
  t_pkt.upper_bound.channel[2] = 41; 
  t_pkt.noise_filter = 0; 

  
  while(true) {
    cmucam2_track_color (&t_pkt, poll_mode, line_mode, auto_led,
                               &servo_settings, buf_mode,sw_color_space, 0);

    printf( "centroid = %d,%d bounding box = %d,%d,%d,%d num pix= %d 
           density = %d\n",
                    t_pkt.centroid_x, t_pkt.centroid_y,
                    t_pkt.x0,t_pkt.y0,t_pkt.x1,t_pkt.y1,
                    t_pkt.num_pixels, t_pkt.int_density );
   }
}


static void cmucam2_track_color (cc3_track_pkt_t * t_pkt,
                           bool poll_mode,
                           int8_t line_mode, bool auto_led,
                           cmucam2_servo_t * servo_settings,
                           bool buf_mode, uint8_t sw_color_space, bool quiet)
{
  cc3_image_t img;
  uint16_t x_mid, y_mid;
  int8_t t_step;
  bool prev_packet_empty = true;

  img.channels = 3;
  img.width = cc3_g_pixbuf_frame.width;
  img.height = 1;// image will hold just 1 row for scanline processing
  img.pix = cc3_malloc_rows (1);
  if (img.pix == NULL) {
    return;
  }

  /* GET FRAME CENTER */
  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 {
    if (!buf_mode)
      cc3_pixbuf_load ();
    else
      cc3_pixbuf_rewind ();
    if (cc3_track_color_scanline_start (t_pkt) != 0) {
      uint8_t lm_width, lm_height;

      uint8_t *lm;
      lm_width = 0;
      lm_height = 0;

      while (cc3_pixbuf_read_rows (img.pix, 1)) {
        if(sw_color_space==HSV_COLOR && img.channels==CC3_CHANNEL_ALL) 
                 cc3_rgb2hsv_row(img.pix,img.width);
                cc3_track_color_scanline (&img, t_pkt);
      }
      // keep this check here if you don't want the CMUcam2 GUI to hang after       
      // exiting a command in line mode

      while (!cc3_uart_has_data (0)) {
        if (getchar () == '\r') {
          free (img.pix);
          return;
        }
      }
      cc3_track_color_scanline_finish (t_pkt);

      if (auto_led) {
                printf ("AUTOLED SCANLINE \r");
        if (t_pkt->int_density > 2)
          cc3_led_set_state (0, true);
        else
          cc3_led_set_state (0, false);
      }
      if (t_pkt->int_density > 5 && servo_settings != NULL) {
                printf ("DENSITY & SERVO SET TC SCANLINE \r");

        if (servo_settings->x_control) {
          t_step = 0;
          if (t_pkt->centroid_x > x_mid + servo_settings->pan_range_far)
            t_step = servo_settings->pan_step;
          else if (t_pkt->centroid_x > x_mid + servo_settings->pan_range_near)
            t_step = (servo_settings->pan_step / 2);
          if (t_pkt->centroid_x < x_mid - servo_settings->pan_range_far)
            t_step = -servo_settings->pan_step;
          else if (t_pkt->centroid_x < x_mid - servo_settings->pan_range_near)
            t_step = -(servo_settings->pan_step / 2);

#ifdef SERVO_REVERSE_DIRECTION_PAN
          servo_settings->x -= t_step;
#else
          servo_settings->x += t_step;
#endif
          t_step = 0;
          if (servo_settings->x > SERVO_MAX)
            servo_settings->x = SERVO_MAX;
          if (servo_settings->x < SERVO_MIN)
            servo_settings->x = SERVO_MIN;
          cc3_gpio_set_servo_position (0, servo_settings->x);
        }
        if (servo_settings->y_control) {
          if (t_pkt->centroid_y > y_mid + servo_settings->tilt_range_far)
            t_step = servo_settings->tilt_step;
          else if (t_pkt->centroid_y >
                   y_mid + servo_settings->tilt_range_near)
            t_step = servo_settings->tilt_step / 2;
          if (t_pkt->centroid_y < y_mid - servo_settings->tilt_range_far)
            t_step = -(servo_settings->tilt_step);
          else if (t_pkt->centroid_y <
                   y_mid - servo_settings->tilt_range_near)
            t_step = -(servo_settings->tilt_step / 2);

#ifdef SERVO_REVERSE_DIRECTION_TILT
          servo_settings->y -= t_step;
#else
          servo_settings->y += t_step;
#endif
          if (servo_settings->y > SERVO_MAX)
            servo_settings->y = SERVO_MAX;
          if (servo_settings->y < SERVO_MIN)
            servo_settings->y = SERVO_MIN;
          cc3_gpio_set_servo_position (1, servo_settings->y);
        }
      }
      if (!quiet) {
        if (!(t_pkt->num_pixels == 0 && prev_packet_empty)) {
          cmucam2_write_t_packet (t_pkt, servo_settings);
        }
      }
      prev_packet_empty = t_pkt->num_pixels == 0;
    }
    else
      return;

    while (!cc3_uart_has_data (0)) {
      if (getchar () == '\r')
        break;
    }
  } while (!poll_mode);
  free (img.pix);
  return;
}
                
void cmucam2_write_t_packet (cc3_track_pkt_t * pkt, cmucam2_servo_t * servo_settings)
{
  printf ("CALL fn WRITE_T_PACKET \r");
  // cap at 255
  if (pkt->centroid_x > 255)
    pkt->centroid_x = 255;
  if (pkt->centroid_y > 255)
    pkt->centroid_y = 255;
  if (pkt->x0 > 255)
    pkt->x0 = 255;
  if (pkt->x1 > 255)
    pkt->x1 = 255;
  if (pkt->y1 > 255)
    pkt->y1 = 255;
  if (pkt->y0 > 255)
    pkt->y0 = 255;

  if (pkt->num_pixels > 255)
    pkt->num_pixels = 255;
  if (pkt->int_density > 255)
    pkt->int_density = 255;
  // values to print
  uint8_t p[8];
  if (pkt->num_pixels == 0) {
    p[0] = p[1] = p[2] = p[3] = p[4] = p[5] = p[6] = p[7] = 0;
  }
  else {
        /* BOUNDING BOX, CENTROID, NUM_PIXELS, PIX_DENSITY */
    p[0] = pkt->centroid_x;
    p[1] = pkt->centroid_y;
    p[2] = pkt->x0;
    p[3] = pkt->y0;
    p[4] = pkt->x1;
    p[5] = pkt->y1;
    p[6] = pkt->num_pixels;
    p[7] = pkt->int_density;
  }
  uint8_t mask = t_pkt_mask;
  printf ("T");
  // print out fields using mask
  for (int i = 0; i < 8; i++) {
    if (mask & 0x1) {
        printf (" %d", p[i]);
    }
    mask >>= 1;
  }

  // print servo settings?
  if (servo_settings != NULL) {
    uint8_t sx = servo_settings->x;
    uint8_t sy = servo_settings->y;

    if (servo_settings->x_report) {
        printf (" %d", sx);
    }
    if (servo_settings->y_report) {
        printf (" %d", sy);
    }
  }
}

    // Please compile and check the syntax . Let me know if there any copy & paste error from cmucam2 soucefiles

    // kind regards ,

    // Ammar

    wikki
    05/14/08 13:26:06
Tree View Flat View (newer first) Flat View (older first)