Go to Post I am a mechanical engineer. I have known since I was about 12 that I wanted to make complicated mechanisms. - Paul Copioli [more]
Home
Go Back   Chief Delphi > Technical > Programming
CD-Media   CD-Spy  
portal register members calendar search Today's Posts Mark Forums Read FAQ rules

 
 
 
Thread Tools Rate Thread Display Modes
Prev Previous Post   Next Post Next
  #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.
 


Thread Tools
Display Modes Rate This Thread
Rate This Thread:

Posting Rules
You may not post new threads
You may not post replies
You may not post attachments
You may not edit your posts

vB code is On
Smilies are On
[IMG] code is On
HTML code is Off
Forum Jump

Similar Threads
Thread Thread Starter Forum Replies Last Post
Scripting Setup and the Camera + Serial Port Drivers CJO Programming 22 11-01-2006 17:42
Autonomous With Camera schenkin Programming 20 16-02-2005 23:52
CMUCam2 Camera Code - Are important parts commented out? Mr. Lim Programming 4 14-01-2005 12:11
autonomous mode problem on field Chris_C Programming 17 26-03-2003 19:11


All times are GMT -5. The time now is 21:05.

The Chief Delphi Forums are sponsored by Innovation First International, Inc.


Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi