simplepath.cc 2.57 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 12 13 14 15 16 17 18 19 20 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 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 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
/* simplepath.cc
 * methods for spathseg class; simple path segment generator
 *
 * Dan Flickinger
 *
 * 2004/11/16
 * 2004/11/16
 */
 
#include "simplepath.h"

spathseg::spathseg() {
  // default constructor
  
  pgrobot = new grobot();
  
  // pivot 360 degrees and go nowhere
  s_length = 2 * M_PI;
  s_radius = 0.0f;
  
  s_Ivelocity = 0.0f;
  s_Fvelocity = 0.0f;
}


 
spathseg::~spathseg() {
  
  delete pgrobot;
  
}



spathseg::spathseg(grobot *g,
                   float s_l,
                   float s_r,
                   float s_iv,
                   float s_fv) {
  // set up a path segment
   
  
  pgrobot = g;
  
  s_length = s_l;
  s_radius = s_r;
  
  s_Ivelocity = s_iv;
  s_Fvelocity = s_fv;

}



int spathseg::execute() {
   // execute this path segment
  
   int returnval = 0;
   int execseg = 1;
   
   float pathperc = 0.0f;    // percent of path completed
   
   float c_velocity = 0.0f; // current velocity
   
   // check the values set to determine if a
   // primitive behavior should be executed instead
   if (s_Ivelocity == 0.0f && s_Fvelocity == 0.0f) {
     if (s_radius == 0.0f) {
       // pivot segment
       pgrobot->pbPivot(s_length);
       returnval = 0; // FIXME: need better return value here
       execseg = 0;
     }
     if (fabs(s_radius) >= 100.0f) {
       // move segment
       pgrobot->pbMove(s_length);
       returnval = 0; // FIXME: need better return value here
       execseg = 0;
     }
   }
    
   
   if (execseg == 1) {
     // execute the path segment using setvPath(float, float)
        
     // reset the odometers
     pgrobot->resetPosition();
   
     // start the robot along the path segment
     while (pathperc <= 1.0f) {
       // traveled distance along arc is still less than desired
    
       // calculate the percent of the path completed
       pathperc = pgrobot->getArclen() / s_length;
     
       // calculate the current velocity and radius
       c_velocity = s_Ivelocity + pathperc * (s_Fvelocity - s_Ivelocity);
     
       // send the current velocity and radius to the robot
       pgrobot->setvPath(c_velocity, s_radius);
 
     
       returnval = pgrobot->getGstatus();
       if (returnval != 0x000) {
         // this segment failed for some reason, E-stop and get out of here
         estop();
         break;
       }
      
       // get some sleep
       pgrobot->sleepy();
     }
   }
  
  return returnval;
}
 


void spathseg::estop() {
  // immediately stop the robot
  
  pgrobot->setWheels(0.0f, 0.0f);
}