View Single Post
  #2   Spotlight this post!  
Unread 05-02-2005, 18:56
Astronouth7303's Avatar
Astronouth7303 Astronouth7303 is offline
Why did I come back?
AKA: Jamie Bliss
FRC #4967 (That ONE Team)
Team Role: Mentor
 
Join Date: Jan 2004
Rookie Year: 2004
Location: Grand Rapids, MI
Posts: 2,071
Astronouth7303 has much to be proud ofAstronouth7303 has much to be proud ofAstronouth7303 has much to be proud ofAstronouth7303 has much to be proud ofAstronouth7303 has much to be proud ofAstronouth7303 has much to be proud ofAstronouth7303 has much to be proud ofAstronouth7303 has much to be proud ofAstronouth7303 has much to be proud ofAstronouth7303 has much to be proud of
Re: Autonomous Camera Integration

Here's my two implementations:
Code:
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
}
Code:
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.