Autonomous Camera Integration

[Astronouth7303]

Here's my two implementations:

int cmd_point_at_color(void)
{
#if _USE_CMU_CAMERA
static int state = START;
static int color;
static long int tolerance;
static long int time_out;

int rc = DONE;
long int heading_error;

switch (state)
  {
  case START:
    {
    color = PARAM1;
    time_out = robot_time + PARAM2;
    tolerance = MRAD2SERVO(PARAM3);
    camera_find_color(color);
    printf("Looking for color %d\r", color);
	state = TURNING;
    rc = WORKING;
    break;
    }
  case IN_PROGRESS:
  case TURNING:
  case STOPPED:
  case LOST:
    {
    if (cam.x == 0)  //Not found!
    {
        state = LOST;
        break;
    }
    heading_error = (signed int)cam.pan_servo - 128; //BOT: center of X on cam
    // Whether we are turning or stopped we want to always check heading
    if (heading_error < -tolerance)
	  {
	  // Turn Left
      set_vel(LEFT, TURN_SPEED);
      set_vel(RIGHT, -TURN_SPEED);      
      state = TURNING;
      }
    else if (heading_error > tolerance)
	  {
	  // Turn Right
      set_vel(LEFT,  -TURN_SPEED);
      set_vel(RIGHT, TURN_SPEED);      
      state = TURNING;
      }
    else 
      {
      if (state == TURNING)
        {
        set_pid_stop(LEFT);
        set_pid_stop(RIGHT);
        state = STOPPED;
        }
      else
        {
        // We are stopped, so maintain postion
        set_pos(LEFT, 0);
        set_pos(RIGHT, 0);
        }
      }

	if (robot_time > time_out)
      {
      state = COMPLETE;
      }

    rc = WORKING;
    break;
    }
  case COMPLETE:
    {
	state = START;
	camera_stop();
    rc = DONE;
    break;
    }
  }
return rc;
#else
printf("POINT_AT_COLOR: Camera is not being used!\r");
return DONE;
#endif
}

int cmd_goto_color(void)
{
#if _USE_CMU_CAMERA
static int state = START;
static int color;
static long int tolerance;
static long int time_out;

int rc = DONE;
long int heading_error;

switch (state)
  {
  case START:
    {
    color = PARAM1;
    time_out = robot_time + PARAM2;
    tolerance = MRAD2SERVO(PARAM3);
    camera_find_color(color);
    printf("Going to color %d\r", color);
	state = IN_PROGRESS;
    rc = WORKING;
    break;
    }
  case IN_PROGRESS:
  case TURNING:
  case STOPPED:
  case LOST:
    {
    if (cam.x == 0)  //Not found!
    {
        state = LOST;
        break;
    }
    heading_error = (signed int)cam.pan_servo - 128; //BOT: center of X on cam
    if (heading_error < -tolerance)
	  {
	  // Turn Left
      set_vel(LEFT, TURN_SPEED * abs(heading_error)); //Psuedo PID (more of a PD)
      set_vel(RIGHT, DRIVE_SPEED);      
      state = TURNING;
      }
    else if (heading_error > tolerance)
	  {
	  // Turn Right
      set_vel(LEFT,  DRIVE_SPEED);
      set_vel(RIGHT, TURN_SPEED * abs(heading_error)); //Psuedo PID (more of a PD)
      state = TURNING;
      }
    else 
      {
      // We are right on
      set_vel(RIGHT, DRIVE_SPEED);
      set_vel(RIGHT, DRIVE_SPEED);
      }
	
	if (robot_time > time_out)
      {
      state = COMPLETE;
      }
    
    rc = WORKING;
    break;
    }
  case COMPLETE:
    {
/*    set_pos(LEFT, counts);
    set_pos(RIGHT, counts);*/
    camera_stop();
	state = START;
    rc = DONE;
    break;
    }
  }
return rc;
#else
printf("GOTO_COLOR: Camera is not being used!\r");
return DONE;
#endif
}

int cmd_camera_stop(void)
{
#if _USE_CMU_CAMERA
    printf("Stopping camera\r");
    camera_stop();
    return DONE;
#else
printf("CAMERA_STOP: Camera is not being used!\r");
return DONE;
#endif
}

int cmd_track_color(void)
{
#if _USE_CMU_CAMERA
    int color = 0;
    color = PARAM1;
    printf("Tracking color %d\r", color);
    camera_find_color(color);
    return DONE;
#else
printf("TRACK_COLOR: Camera is not being used!\r");
return DONE;
#endif
}

int cmd_turn_to_color(void)
{
#if _USE_CMU_CAMERA
static int state = START;
static long int tolerance;
static long int time_out;

int rc = DONE;
long int heading_error;

switch (state)
  {
  case START:
    {
    time_out = robot_time + PARAM1;
    tolerance = MRAD2SERVO(PARAM2);
    printf("Turning towards tracked color.\r");
	state = TURNING;
    rc = WORKING;
    break;
    }
  case IN_PROGRESS:
  case TURNING:
  case STOPPED:
  case LOST:
    {
    if (cam.x == 0)  //Not found!
    {
        state = LOST;
        break;
    }
    heading_error = (signed int)cam.pan_servo - 128; //BOT: center of X on cam
    // Whether we are turning or stopped we want to always check heading
    if (heading_error < -tolerance)
	  {
	  // Turn Left
      set_vel(LEFT, TURN_SPEED);
      set_vel(RIGHT, -TURN_SPEED);      
      state = TURNING;
      }
    else if (heading_error > tolerance)
	  {
	  // Turn Right
      set_vel(LEFT,  -TURN_SPEED);
      set_vel(RIGHT, TURN_SPEED);      
      state = TURNING;
      }
    else 
      {
      if (state == TURNING)
        {
        set_pid_stop(LEFT);
        set_pid_stop(RIGHT);
        state = STOPPED;
        }
      else
        {
        // We are stopped, so maintain postion
        set_pos(LEFT, 0);
        set_pos(RIGHT, 0);
        }
      }

	if (robot_time > time_out)
      {
      state = COMPLETE;
      }

    rc = WORKING;
    break;
    }
  case COMPLETE:
    {
	state = START;
    rc = DONE;
    break;
    }
  }
return rc;
#else
printf("TURN_TO_COLOR: Camera is not being used!\r");
return DONE;
#endif
}

int cmd_drive_to_color(void)
{
#if _USE_CMU_CAMERA
static int state = START;
static long int tolerance;
static long int time_out;

int rc = DONE;
long int heading_error;

switch (state)
  {
  case START:
    {
    time_out = robot_time + PARAM1;
    tolerance = MRAD2SERVO(PARAM2);
    printf("Driving towards tracked color\r");
	state = IN_PROGRESS;
    rc = WORKING;
    break;
    }
  case IN_PROGRESS:
  case TURNING:
  case STOPPED:
  case LOST:
    {
    if (cam.x == 0)  //Not found!
    {
        state = LOST;
        break;
    }
    heading_error = (signed int)cam.pan_servo - 128; //BOT: center of X on cam
    if (heading_error < -tolerance)
	  {
	  // Turn Left
      set_vel(LEFT, TURN_SPEED * abs(heading_error)); //Psuedo PID (more of a PD)
      set_vel(RIGHT, DRIVE_SPEED);      
      state = TURNING;
      }
    else if (heading_error > tolerance)
	  {
	  // Turn Right
      set_vel(LEFT,  DRIVE_SPEED);
      set_vel(RIGHT, TURN_SPEED * abs(heading_error)); //Psuedo PID (more of a PD)
      state = TURNING;
      }
    else 
      {
      // We are right on
      set_vel(RIGHT, DRIVE_SPEED);
      set_vel(RIGHT, DRIVE_SPEED);
      }
	
	if (robot_time > time_out)
      {
      state = COMPLETE;
      }
    
    rc = WORKING;
    break;
    }
  case COMPLETE:
    {
	state = START;
    rc = DONE;
    break;
    }
  }
return rc;
#else
printf("DRIVE_TO_COLOR: Camera is not being used!\r");
return DONE;
#endif
}

There is a lot of stuff in there that is not in the default code. You need to add it.

[GregC]

Thank you!
We're getting all sorts of errors with what we are trying to do, we'll take a look at yours to try to figure out our own problems x_x
Thanks again
-greg