Chief Delphi

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)
-   Programming (http://www.chiefdelphi.com/forums/forumdisplay.php?f=51)
-   -   Autonomous Camera Integration (http://www.chiefdelphi.com/forums/showthread.php?t=34043)

GregC 05-02-2005 16:28

Autonomous Camera Integration
 
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 05-02-2005 18:56

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. ;)

GregC 06-02-2005 14:34

Re: Autonomous Camera Integration
 
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


All times are GMT -5. The time now is 13:09.

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