All new accounts created on Gitlab now require administrator approval. If you invite any collaborators, please let Flux staff know so they can approve the accounts.

grobot.cc 10.5 KB
Newer Older
Timothy Stack's avatar
Timothy Stack committed
1 2 3 4 5 6
/*
 * EMULAB-COPYRIGHT
 * Copyright (c) 2005 University of Utah and the Flux Group.
 * All rights reserved.
 */

7 8 9 10 11
/* Garcia robot class methods
 *
 * Dan Flickinger
 *
 * 2004/11/16
12
 * 2004/12/15
13 14 15
 */

#include "grobot.h"
Timothy Stack's avatar
 
Timothy Stack committed
16
#include "gcallbacks.h"
17 18 19 20


grobot::grobot() {
  // default constructor
Daniel Flickinger's avatar
Daniel Flickinger committed
21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45
  aIO_GetLibRef(&ioRef, &err);
  
  
  Vl = 0.0f;
  Vr = 0.0f;
  
  dleft = 0.0f;
  dright = 0.0f;
  
  dt_init = 0.0f;
  dx_est = 0.0f;
  dy_est = 0.0f;
  
  
  gotolock = 0;
  gotocomplete = 0;
  
  gotomexec = 0;
  gotomcomplete = 0;
  
  gotop1 = 0;
  gotom1 = 0;
  gotop2 = 0;
  

46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64
}



grobot::~grobot() {
  // grobot the DESTRUCTOR
  aIO_ReleaseLibRef(ioRef, &err);
 }

 

void grobot::estop() {
  // emergency stop
 
  // flush everything
  garcia.flushQueuedBehaviors();
  
//   acpValue abortI((int)(aGARCIA_ERRFLAG_ABORT));
//   garcia.setNamedValue("status", &abortI);
Daniel Flickinger's avatar
Daniel Flickinger committed
65 66 67 68
  
  
  // clear out goto command
  gotolock = 0;
69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136

}
  
void grobot::setWheels(float Vl, float Vr) {
  // set wheel speeds
  
//  float V_avg;
  
 
  
  acpValue lVelocity((float)(Vl));
  acpValue rVelocity((float)(Vr));
//  acpValue enable((int)(1));
 
  garcia.setNamedValue("damped-speed-left", &lVelocity);
  garcia.setNamedValue("damped-speed-right", &rVelocity);
   
  // Get the null primitive running if needed
  createNULLbehavior();
  
  // The front/rear IR rangers should be enabled automatically
  
  // calculate the threshold for detection based on the average speed
  //   V_avg = (Vl + Vr) / 2.0f;
  //   acpValue IR_threshold((float)(V_avg / 10.0f));
}



void grobot::setvPath(float Wv, float Wr) {
  // set wheelspeeds according to a path

  float b = 0.0f;
  float Vleft = 0.0f;
  float Vright = 0.0f;
  
  // b: velocity difference, v: velocity, a: track width, r: turning radius
  // vr: right wheel velocity, vl: left wheel velocity
  // b = (v*a)/(2*r)
  // vr = v+b , vl = v-b

  // based on:
  // r = (a/2) * (vr + vl)/(vr - vl)
 
  if (Wr == 0.0f) {
    // zero turning radius
    Vleft = -Wv;
    Vright = Wv;
  } else if (Wr >= 100.0f) {
    // 100 meters or greater, drive straight
    Vleft = Wv;
    Vright = Wv;
  } else {
    // turning 
    b = (Wv * TRACK_WIDTH) / (2 * Wr);
    Vleft = Wv + b;
    Vright = Wv - b;
  }

  setWheels(Vleft, Vright); 
  
}



void grobot::pbMove(float mdisplacement) {
  // execute a move primitive
  
Daniel Flickinger's avatar
 
Daniel Flickinger committed
137 138 139 140 141
  // TRUNCATE THE CRAP
  mdisplacement = 100.0*mdisplacement;
  mdisplacement = floor(mdisplacement);
  mdisplacement = mdisplacement/100.0;
  
Timothy Stack's avatar
Timothy Stack committed
142
  if (fabs(mdisplacement) > 0.05f) {
143
    // send the move to the robot
Timothy Stack's avatar
Timothy Stack committed
144
    // if you want to move less than 5 cm, FUCK OFF.
145 146
    std::cout << "Move length: " << mdisplacement << std::endl;
    
147 148 149
    acpValue moveLength((float)(mdisplacement));
    pBehavior = garcia.createNamedBehavior("move", "move1");
    pBehavior->setNamedValue("distance", &moveLength);
150
  
151 152 153 154 155 156 157 158 159 160 161 162 163 164
    createPRIMbehavior(CBT_MOVE);
  } else {
    // a zero length move will fuckup the robot!
    std::cout << "ZERO LENGTH MOVE" << std::endl;
    // fake a successful move, and increment the callback counts
    // if this is part of a goto sequence of commands
    if (1 == gotolock) {
      ++gotomexec;
      ++gotomcomplete;
      
      dx_est = 0.0f; // move nowhere!
      dy_est = 0.0f; // move nowhere!
    }
  }
165 166 167 168 169 170 171 172
  
}

    
    
void grobot::pbPivot(float pangle) {
  // execute a pivot primitive
  
Daniel Flickinger's avatar
 
Daniel Flickinger committed
173 174
  int numturns;
  
Daniel Flickinger's avatar
 
Daniel Flickinger committed
175 176 177 178 179
  // TRUNCATE THE CRAP
  pangle = 100.0*pangle;
  pangle = floor(pangle);
  pangle = pangle/100.0;
  
Daniel Flickinger's avatar
 
Daniel Flickinger committed
180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202
  // make the pivot smarter
  if (fabs(pangle) == 2*M_PI) {
    // this is zero! Who sends this shit??
    //pangle = 0.0f;
    // PROBLEM: if the garcia is told to pivot 0.0,
    // !!!! BAD THINGS WILL HAPPEN !!!!
  }
  
  if (fabs(pangle) > 2*M_PI) {
    // no dancing! Reduce this.
    numturns = (int)(pangle / (2*M_PI));
    pangle = pangle - (2*M_PI * numturns);
  }
  
  if (fabs(pangle) > M_PI) {
    // turn the short way
    if (pangle >= 0) {
      pangle = -(2*M_PI - pangle);
    } else {
      pangle = pangle + 2*M_PI;
    }
  }
  
Timothy Stack's avatar
Timothy Stack committed
203
  if (fabs(pangle) > 0.05f) {
204
    // send the pivot to the robot
Daniel Flickinger's avatar
 
Daniel Flickinger committed
205
    // If it wants to turn less than 0.05 radians, it gets the hose again
206 207
    std::cout << "Pivot angle: " << pangle << std::endl;
    
208 209 210
    acpValue pivotAngle((float)(pangle));
    pBehavior = garcia.createNamedBehavior("pivot", "pivot1");
    pBehavior->setNamedValue("angle", &pivotAngle);
Daniel Flickinger's avatar
 
Daniel Flickinger committed
211
  
212 213 214 215 216 217 218 219 220 221 222 223 224
    createPRIMbehavior(CBT_PIVOT);
  } else {
    // a zero angle pivot will fuckup the robot!
    std::cout << "ZERO ANGLE PIVOT" << std::endl;
    
    // fake a successful pivot, and increment the callback counts
    // if this is part of a goto sequence of commands
    if (1 == gotolock) {
      ++gotomexec;
      ++gotomcomplete;
    }
    
  }
225 226 227 228 229 230 231 232
  
}



void grobot::dgoto(float Dx, float Dy, float Rf) {
  // goto a defined point and orientation
  
Daniel Flickinger's avatar
Daniel Flickinger committed
233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250
  // make sure that a goto is not currently executing
  if (0 == gotolock) {
   
    // set lock to anticipate first move
    gotolock = 1;
    
    
    // initialize the status variables
    gotocomplete = 0;
    
    gotomexec = 0;
    gotomcomplete = 0;
    
    gotop1 = 0;
    gotom1 = 0;
    gotop2 = 0;
     
    // calculate rotation components
Timothy Stack's avatar
Timothy Stack committed
251 252
    dt_init = atan2(Dy, Dx); // dt_init is private to grobot
    float Rfr = Rf - dt_init;
Daniel Flickinger's avatar
Daniel Flickinger committed
253 254
    float moveL = sqrt((pow(Dx,2)) + (pow(Dy,2)));

Timothy Stack's avatar
Timothy Stack committed
255
    if (0.0f == dt_init && 0.0f == moveL && 0.0f == Rfr) {
256
      set_gotocomplete();
Daniel Flickinger's avatar
Daniel Flickinger committed
257
    } else {
258 259
      // execute primitives

Timothy Stack's avatar
Timothy Stack committed
260
      pbPivot(dt_init);
Daniel Flickinger's avatar
Daniel Flickinger committed
261 262 263
      pbMove(moveL);
      pbPivot(Rfr);
    }
264
    
Daniel Flickinger's avatar
Daniel Flickinger committed
265 266 267 268 269
    
  } else {
    // if a goto is already executing, drop the command
    std::cout << "GOTO COMMAND ALREADY EXECUTING" << std::endl;
  }
270 271 272 273 274 275 276 277 278 279 280 281
  
}



void grobot::resetPosition() {
  // zeros the current odometer readings

  acpValue zvalue(0.0f);  
  garcia.setNamedValue("distance-left", &zvalue);
  garcia.setNamedValue("distance-right", &zvalue);
  
Daniel Flickinger's avatar
Daniel Flickinger committed
282 283 284
  dleft = 0.0f;
  dright = 0.0f;
  
Timothy Stack's avatar
Timothy Stack committed
285 286 287
  dx_est = 0.0f;
  dy_est = 0.0f;
  
288 289 290 291 292 293 294 295
}



void grobot::updatePosition() {
  // updates the current position
  
  // get wheel odometry:
Daniel Flickinger's avatar
Daniel Flickinger committed
296 297
  dleft = garcia.getNamedValue("distance-left")->getFloatVal();
  dright = garcia.getNamedValue("distance-right")->getFloatVal();
298 299 300 301 302 303 304 305 306

}



float grobot::getArclen() {
  // return the current arc length, assuming a nice circular arc
  // (MAKE SURE TO RESET THE POSITION AT THE BEGINNING OF EACH SEGMENT!)
  
Daniel Flickinger's avatar
Daniel Flickinger committed
307
  updatePosition();  
308 309 310 311 312
  return (dleft + dright) / 2.0f;
}



Timothy Stack's avatar
Timothy Stack committed
313
void grobot::getDisplacement(float &dxtemp, float &dytemp) {
Daniel Flickinger's avatar
Daniel Flickinger committed
314 315 316 317 318 319
  // return the current displacement (for a goto move)
 
  dxtemp = dx_est;
  dytemp = dy_est;
}

320 321 322 323 324 325 326 327 328


int grobot::getGstatus() {
  // return the status of the garcia robot --
  // for the current or previous primitive.
  return garcia.getNamedValue("status")->getIntVal();
}


Daniel Flickinger's avatar
Daniel Flickinger committed
329 330 331 332 333 334 335 336 337

int grobot::getGOTOstatus() {
  // return the status of the currently executing goto command
  
  int retval_gotocomplete = gotocomplete;
  
  if (0 != gotocomplete) {
    // reset the status
    gotolock = 0;
Timothy Stack's avatar
 
Timothy Stack committed
338
    gotocomplete = 0;
Daniel Flickinger's avatar
Daniel Flickinger committed
339 340 341 342 343
  }
  
  return retval_gotocomplete;
  
}
344
   
Daniel Flickinger's avatar
Daniel Flickinger committed
345
void grobot::sleepy() {
346 347 348
  // sleep for 50ms, allow robot to handle callbacks for another 50ms
  
  aIO_MSSleep(ioRef, 50, NULL);
Daniel Flickinger's avatar
Daniel Flickinger committed
349 350
  garcia.handleCallbacks(50);

351 352 353 354 355 356 357
}




void grobot::setCBexec(int id) {
  // set execution callback up
Daniel Flickinger's avatar
Daniel Flickinger committed
358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385

  std::cout << "Behavior #" << id
            << " started." << std::endl;
  
  if (0 != gotolock) {
    // a goto behavior is currently executing
    ++gotomexec;
    
    if (1 == gotomexec) {
      // set status for first pivot
      gotop1 = -1;
    } else if (2 == gotomexec) {
      // set status for move
      gotom1 = -1;
      
      // clear position estimate at the start of main move
      resetPosition();
      
    } else if (3 == gotomexec) {
      // set status for second pivot
      gotop2 = -1;
    } else {
      // oh shit: weirdness!
      std::cout << "ERROR: Execution callback count "
                << "for goto command exceeded." << std::endl;
    }
  }
  
386 387 388 389
}



Timothy Stack's avatar
 
Timothy Stack committed
390
void grobot::setCBstatus(int id, int stat, cb_type_t cbt) {
391 392 393 394 395
  // set completion callback status
  
  cout << "Behavior #" << id
       << " returned with status: " << stat
       << endl;
Daniel Flickinger's avatar
Daniel Flickinger committed
396 397 398 399
     
  if (0 != gotolock) {
    // a goto behavior is currently executing
    ++gotomcomplete;
Timothy Stack's avatar
 
Timothy Stack committed
400

Timothy Stack's avatar
Timothy Stack committed
401
    if (cbt == CBT_MOVE) {
Timothy Stack's avatar
 
Timothy Stack committed
402
      // get the Arclength, then estimate and store the displacement
Timothy Stack's avatar
Timothy Stack committed
403
      // (assume that the first pivot was accurate)
Timothy Stack's avatar
 
Timothy Stack committed
404 405 406 407
      float al_temp = getArclen();
      dx_est = al_temp * cos(dt_init);
      dy_est = al_temp * sin(dt_init);
    }
Daniel Flickinger's avatar
Daniel Flickinger committed
408
    
Timothy Stack's avatar
Timothy Stack committed
409
    if (1 == gotomcomplete) {
Daniel Flickinger's avatar
Daniel Flickinger committed
410 411
      // first pivot has finished
      gotop1 = stat;
Timothy Stack's avatar
Timothy Stack committed
412 413 414
    } else if (2 == gotomcomplete) {
      // main move has executed
      gotom1 = stat;
Daniel Flickinger's avatar
Daniel Flickinger committed
415
      
Timothy Stack's avatar
Timothy Stack committed
416
    } else if (3 == gotomcomplete) {
Daniel Flickinger's avatar
Daniel Flickinger committed
417 418 419 420 421 422 423 424 425
      // second pivot has executed
      gotop2 = stat;
      
      // the goto command has completed
      /////////////////////////////////
      
      // set the gotocomplete flag
      set_gotocomplete();
      
Timothy Stack's avatar
Timothy Stack committed
426
    } else {
Daniel Flickinger's avatar
Daniel Flickinger committed
427 428 429 430 431
      // weirdness
      std::cout << "ERROR: Completion callback count "
                << "for goto command exceeded." << std::endl;
    }
  }
432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452
  
}







//private functions:
void grobot::createNULLbehavior() {
  // create a NULL behavior if the robot is idle

  if (garcia.getNamedValue("idle")->getBoolVal()) {
    // execute NULL primitive
    
    cout << "Creating NULL primitive" << endl;
    
    acpValue acceleration(2.0f);
    pBehavior = garcia.createNamedBehavior("null", "driver");
    
Timothy Stack's avatar
 
Timothy Stack committed
453
    completeCB = new CallbackComplete(pBehavior, this, CBT_NONE);
454 455 456 457 458 459 460 461 462 463 464
    completeCBacpV.set(completeCB);
    pBehavior->setNamedValue("completion-callback", &completeCBacpV);
    
    
    pBehavior->setNamedValue("acceleration", &acceleration);
    garcia.queueBehavior(pBehavior);
  }
}



Timothy Stack's avatar
 
Timothy Stack committed
465
void grobot::createPRIMbehavior(cb_type_t cbt) {
466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481
  // create and execute a PRIMitive behavior
  
  // wait until garcia is ready
  int count = 0;
  while (!garcia.getNamedValue("active")->getBoolVal()) {
    std::cout << "." << std::flush;
    aIO_MSSleep(ioRef, 100, NULL);
    
    count++;
    if (count == 20) {
      std::cout << std::endl << "Robot is not responding." << std::endl;
      break;
    }
  }
  
  executeCB = new CallbackExecute(pBehavior, this);
Timothy Stack's avatar
 
Timothy Stack committed
482
  completeCB = new CallbackComplete(pBehavior, this, cbt);
483 484 485 486 487 488 489 490 491 492 493 494 495
  

  executeCBacpV.set(executeCB);
  completeCBacpV.set(completeCB);
  
  pBehavior->setNamedValue("execute-callback", &executeCBacpV);
  pBehavior->setNamedValue("completion-callback", &completeCBacpV);
  
  garcia.queueBehavior(pBehavior);
}



Daniel Flickinger's avatar
Daniel Flickinger committed
496 497 498 499 500 501 502 503 504 505
void grobot::set_gotocomplete() {
  // set the gotocomplete flag  
  if ((gotop1 + gotom1 + gotop2) > 0) {
    // there were problems
    gotocomplete = -1;
  } else {
    // goto completed
    gotocomplete = 1;
  }
}
506