Autonomous Camera Integration

[GregC]

Hey all,
My team is having quite a problem attemping to integrate the current camera code (2-26) into our default code for autonomous.
We're hitting all sorts of problems.
Can anyone PLEASE share their implementation so we have something to go off of?
We're totally stuck here!
Thanks!!
-Greg, 384

[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