Vision-Guided Fetch Pom Example

Most code descriptions in this tutorial have shown how to write simple functions to do a single task, but real competition requirements are made up of multiple task objectives.  Here is a multitask example which illustrates how to combine functions to create a behavior which may seem complex, but really is just a sequence of behaviors.  The overall objective is to demonstrate the robot’s ability to seek out a colored pom, stop and grab it, then turn to its starting position, release the pom and reorient itself to do it again, all after being initiated by the user.  This implies the need for an arm and claw on the demo bot, as well as an initial calibration of the camera vision system to identify the pom by color and to set the camera pointing to be aligned with the claw opening (Eye-claw coordination).  [This code will require modification for use with the Link controller.  Note function name changes under The Link Camera.]

TopBack-View-camera-claw-CBC2-094crAbove is a typical Demo Bot With Camera, Arm, and Claw attached

Therefore we have the following requirements:

  1. Setup the camera color channel to match the pom color under field lighting (separate from C-code requirements)
  2. Build the arm & claw to grab and lift the pom, test it for reliability and identify the servo values for arm ‘up-down’ and claw ‘open-close’.
  3. Comment the code describing these bot requirements
  4. Start with an initialization loop that displays the x & y coordinates of the pom color blob and the value of the y-coord [y_loc] where the bot must stop moving toward the pom. [the y-coord increases as the bot gets closer to the pom.]  It must also read touch buttons on the display to allow increasing and decreasing this y_loc value by the user.  Finally the initialization must end when the user touches another button. (after they have placed the pom target at some distance so it can be ‘fetched’)
  5. Seeking pom loop This is the proportional control loop using the Camera to guide the robot to the pm.  Before starting at least one motor position reference should be zeroed, to measure forward motion. [clear_motor_position_counter(port#)]  Assuming a left and right wheel drive, each drive has a forward speed and turn speed component.  The turn speed is based upon the wheel size and axle width, and the motor function speed range [up to 1500 for Link mav(), 100 for motor()] and uses the x-cord of the pom color blob, assuming the camera FOV is centered straight with the bot, and a value of 80 is zero error. [note the different function names for the CBC and the Link]  Constant test of the y-coord of the blob is needed to stop forward motion by ending the loop when y_loc is reached.  The loop can be limited by a max position count, or just by the touch of a button, for experiments.
  6. Stop, drop arm, & close claw, lift arm, save distance  These actions are self-explanatory, except that the servo motions are called in a ‘for’ loop with a delay to set arm and claw  speed.  [Parameters for ‘up’ – ‘down’ and ‘open’ – ‘closed’ are found using the servo control display.]
  7. Turn around-  turn 180 degrees,  to return the pom.[ for Link the speed is numerically faster, to accommodate the mrp() function speed scale.
  8. Drive back- use mrp() with saved forward distance [Vision –guidance to another drop could be substituted here.]
  9. Release pom- use claw in for loop
  10. Turn back around- same as #7
  11. Enclose #4 to #10 in a repeat loop for ease of experiment repeat.

Two Vision-Guided Fetch Bots in Action

Demo Program

//Actual Vision code for CBC2 Vision-guided Grab & Return, 3.6b
 /*Demonstrates the robot’s ability to seek out a colored pom, stop and grab it, then turn to its starting position, release the pom and reorient itself to do it again, all after being initiated by the user. This implies the need for an arm and claw on the demo bot, as well as an initial calibration of the camera vision system to identify the pom by color and to set the camera pointing to be aligned with the claw opening (Eye-claw coordination) */
#define SPEED 600 // nominal speed - small tires 1/5/11
 /*********** Robot port configuration ***********/
 #define R_MOTOR 0 /* motor port 0 */
 #define L_MOTOR 2 /* motor port 2 */
 #define DELTA 0 /* left motor speed adjust if needed */
 #define TURN 1810L /* for black motors, direct to 1 3/4" wheels, on 6.0" axle; more for smaller wheels, wider track, or motors geared down to the wheel */
 #define FWD 4200L /* max forward distance, in ticks */
int main() {
 int i, speed_r, speed_l, y_loc=110; // camera limit 120 pixels down
 long dist;
 enable_servos();
 set_servo_position(1,1000); //claw – open
 set_servo_position(0,1400); //arm - up
 printf("Fwd, vision-guided\n grab,lift- return v3.6b cbc\n");
 sleep(3.);
 while(1){ //repeat fetch loop to save adjustment of stop
 // [calibration of y_loc] for next run
 /*calibrate stop based on track_y() by adjusting target position & camera tilt */
    while(!black_button()){
       cbc_printf(1,1,"To set target at desired range");
       cbc_printf(2,2,"use Right-Left PAD");
       cbc_printf(2,3,"black button to end");
       i=y_loc;
       if(left_button()&& i>=5)i-=5;
       if(right_button() && i<=120)i+=5;
       track_update(); y_loc =i;
       cbc_printf(2,4,"actual x = %d", track_x(0,0));
       cbc_printf(2,5,"Y loc = %d , actual = %d",y_loc,track_y(0,0));
       sleep(.3); cbc_display_clear();
       } // end calibrate display
 // set reference on L_MOTOR
 clear_motor_position_counter(L_MOTOR);
 //**** start drive toward target
 mav(R_MOTOR,SPEED); mrp(L_MOTOR,SPEED+DELTA,FWD);
 // the key factor is the directional feedback gain
  while(get_motor_position_counter(L_MOTOR)< FWD) {// begin guide forward 
     track_update(); speed_r=SPEED-11*(track_x(0,0)-80)/6; // CBC camera max x=160 
     if (speed_r>1000)speed_r=1000; // keep speed within limits
     speed_l=SPEED+11*(track_x(0,0)-80)/6;
     if(speed_l>1000)speed_l=1000; // keep speed within limits
     mav(R_MOTOR,speed_r); mav(L_MOTOR,speed_l);
     if (track_y(0,0)>y_loc){break;} //stop when close
     msleep(50L);
     }  //end guide forward
  off(R_MOTOR); off(L_MOTOR);
  //**** drop arm, then grab and pickup (w/o camera check)
  // for servo sg5010, CBC2 (850 w. knob) 1/15/11
  for (i=1400; i>=950; i-=50)
  {set_servo_position(0,i); sleep(.07); }
  //*** close claw
  for (i=1000; i>=350 ;i-=50) // for servo sg5010, CBC2
  {set_servo_position(1,i); sleep(.07); }
  //*** lift arm
  for(i=950; i<=1400; i+=50)
  {set_servo_position(0,i); sleep(.07); }
  dist = get_motor_position_counter(L_MOTOR); // save forward dist
  //*** turn around 180 degrees
  mrp(R_MOTOR,SPEED/2,TURN); mrp(L_MOTOR,SPEED/2,-TURN);
  bmd(L_MOTOR); bmd(R_MOTOR);
  //*** drive back
  mrp(R_MOTOR,SPEED, dist-100L); // return distance - minus arm length
  mrp(L_MOTOR,SPEED+DELTA, dist-100L); bmd(L_MOTOR); off(R_MOTOR);
  //*** release pom
  for (i=500; i<=1000; i+=50)
  {set_servo_position(1,i); sleep(.07);}
  //*** turn back 180 degrees
  mrp(R_MOTOR,SPEED/2,TURN); mrp(L_MOTOR,SPEED/2,-TURN);
  bmd(L_MOTOR); bmd(R_MOTOR);
  } // end repeat fetch loop
 } // end main
Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s