CreaBot.cpp

00001 #include "CreaBot.h"
00002 
00003 // *****************************************************************
00004 //    BotCommand Structure Helper
00005 // *****************************************************************
00006 
00007 // Struct Helper; set structure fields to values  */
00008 void BotCommand::set(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm) 
00009 {   command  = Acommand; 
00010     dist_cm  = Adist_cm; 
00011     turn_angle_deg= Aturn_angle_deg;
00012 }; 
00013 
00014 // Struct Helper; set structure fields to values  */
00015 void BotCommand::set(BotCmdVerb Acommand, float Aparam) 
00016 {   command  = Acommand; 
00017     dist_cm  = Aparam; 
00018     turn_angle_deg= Aparam;
00019 }; 
00020 
00021 
00022 // *****************************************************************
00023 //    Creabot Class
00024 // *****************************************************************
00025 
00026 Creabot::Creabot(CreaMot *left, CreaMot *right, float diameter_wheel_cm, float bot_diam_cm):
00027       wheelLeft(left), wheelRight(right) 
00028 {
00029     // TODO: To catch for illegal dimensions (<= 0)
00030     
00031     // Initialize wheel geometires
00032     wheelLeft ->setDiamCM( diameter_wheel_cm );
00033     wheelRight->setDiamCM( diameter_wheel_cm );
00034     // setup the bot parameters:
00035     botDiameter     = bot_diam_cm;
00036     ratio_wheel_bot = bot_diam_cm/diameter_wheel_cm;
00037     botSpeed_cm_sec = DEFAULT_SPEED_CM_SEC;
00038 
00039     // Attach the Crabot functions as callbacks to the wheels
00040     wheelLeft ->callbackSet(this, &Creabot::wheelLeftStoppedCB);   
00041     wheelRight->callbackSet(this, &Creabot::wheelRightStoppedCB);
00042     // Initialize own callback as empty
00043     extCallBack = NULL;
00044 
00045     // setup the default Directions and State Values: 
00046     // used below as variable instead of always passing a direction
00047     wheelLeft ->defaultDirection = COUNTERCLOCKWISE;
00048     wheelLeft ->StateValue  = LWHEEL_RUNS;
00049     wheelRight->defaultDirection = CLOCKWISE;
00050     wheelRight->StateValue = RWHEEL_RUNS;
00051 
00052     // Initialize the own status fields
00053     wheelsState  = LRWHEELS_STOP;
00054     qReset();
00055 }
00056 
00057 
00058 // *****************************************************************
00059 //    Queue Queue management
00060 // *****************************************************************
00061 
00062 void Creabot::qMove(BotCmdVerb Acommand, float Aparam)
00063 {
00064     qMove(Acommand, Aparam, Aparam);
00065 }
00066 
00067 /* Add a new movement to the queue, if not full. 
00068 *  ATTENTION: 
00069 *   qExecuteNext() is called asynchronsously by the motor Callback wheelsAllStoppedCB()
00070 *   qExecuteNext() tries to read access the next command in the queue.  
00071 *   A simultaneous read and write access to the same resources can lead to 
00072 *   conflict and must be carefully avoided.   
00073 *   This conflict concerns the read-modify-write access of variable Count. 
00074 *   It is avoided by using the qBlock variable. With qBlock set during increment
00075 *   a simultaneous read access is blocked
00076 *   Any changes to those write and read handlers must take into account that 
00077 *   wheelsAllStoppedCB() and qExecuteNext() may be called any time while 
00078 *   executing qMove command. 
00079 */
00080 void Creabot::qMove(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm)
00081 {   if( !qIsFull() ) 
00082       { cmd[writeIdx].set(Acommand, Aturn_angle_deg, Adist_cm);
00083         writeIdx++;
00084         if (writeIdx==DEPTH_Queue) writeIdx=0;
00085         qCollide = false; // colliding access to qCollide cannot happen while qBlock = false
00086         // if callback happens here, there is no collosion, the last old command gets already executed. 
00087         qBlock = true; 
00088         // if callback happens here while qBlock = true, the collision is remembered in qCollide
00089         Count++;
00090         qBlock = false;
00091         // if callback happens here there is no more blockage and the next (maybe the new) command is already being executed
00092         // in this case qCollide = false, and wheelsState!=LRWHEELS_STOP
00093         if ( qCollide  || (wheelsState==LRWHEELS_STOP) )
00094             // callback cannot happen here since the wheelState is already STOP, or Collide has happened before!
00095             qExecuteNext();
00096       };
00097 }
00098 
00099 
00100 // Queue is worked through while still any elements inside
00101 // ATTENTION read above consideration of conflicting accesses between qMove() and wheelsAllStoppedCB()
00102 // Think careful if a case can occur that pExecuteNext is called twice immediately following each other, 
00103 //   once by qMove() and once by  wheelsAllStoppedCB() ???
00104 void Creabot::qExecuteNext()
00105 {   if ( !qIsEmpty() ) //  see if something in the queue to execute
00106       { // Execute and then remove the oldest element at the tail of the Queue
00107         iExeCommand( &cmd[readIdx] );
00108         readIdx++;
00109         if (readIdx==DEPTH_Queue) readIdx=0;
00110         Count--;
00111       };
00112 }
00113 
00114 // Immediately end All, the queue and the motor
00115 void Creabot::qStopAll()
00116 {   qReset(); // empty the queue
00117     iStop(); // stop the motors, do not call external callback
00118 }    
00119 
00120 
00121 //**************************************************************************
00122 //    Using the BotCmdVerb to encode different commands, done immediately
00123 //**************************************************************************
00124 
00125 void Creabot::iMoveAndWait(BotCmdVerb moveType, float Aparam)
00126 {   iMove(moveType,Aparam,Aparam);
00127     iWaitEnd();
00128 }
00129 
00130 void Creabot::iMoveAndWait(BotCmdVerb moveType, float Aturn_angle_deg, float Adist_cm)
00131 {   iMove(moveType,Aturn_angle_deg,Adist_cm);
00132     iWaitEnd();
00133 }
00134 
00135 void Creabot::iMove(BotCmdVerb moveType, float Aparam)
00136 {   iMove(moveType, Aparam, Aparam);   }
00137 
00138 void Creabot::iMove(BotCmdVerb moveType,  float Aturn_angle_deg, float Adist_cm)
00139 {   BotCommand Anew_cmd;
00140     Anew_cmd.set(moveType, Aturn_angle_deg, Adist_cm);
00141     iExeCommand(&Anew_cmd);
00142 }
00143 
00144 /** High level, immediate: move bot according to prefilled command structures.
00145 * Recommended to use iMove() methods to fill the command structure correctly. 
00146 * Branches to the moveXXXX() methods. For details see docs for those methods. 
00147 * Warning: Collides with queued commands if called individually. 
00148 * @param[in] <*cmd> Pointer to type BotCommand, the prefilled command structure,*/
00149 void Creabot::iExeCommand(BotCommand *cmd) {
00150           switch (cmd->command) {
00151             case FORWARD:
00152                 moveForward(cmd->dist_cm);
00153                 break;
00154             case BACKWARD:
00155                 moveBackward(cmd->dist_cm);
00156                 break;
00157             case LEFT:
00158                 moveLeft(cmd->turn_angle_deg, cmd->dist_cm);
00159                 break;
00160             case RIGHT:
00161                 moveRight(cmd->turn_angle_deg, cmd->dist_cm);
00162                 break;
00163             case BACKLEFT:
00164                 moveBackLeft(cmd->turn_angle_deg, cmd->dist_cm);
00165                 break;
00166             case BACKRIGHT:
00167                 moveBackRight(cmd->turn_angle_deg, cmd->dist_cm);
00168                 break;
00169             case ROTATE:
00170                 moveRotate(cmd->turn_angle_deg);
00171                 break;
00172             default:
00173                 break;
00174         };
00175 }; // executeCommand
00176 
00177 /* spends all time with waits, returns only once wheelState = LRWHEELS_STOP  */
00178 void Creabot::iWaitEnd()
00179 {   while(wheelsState!=LRWHEELS_STOP) wait(0.1);   }
00180 
00181 /*  spends all time with waits, returns only once wheelState = LRWHEELS_STOP,
00182   but maximum  max_wait_ms milli seconds */
00183 void Creabot::iWaitEnd(uint32_t max_wait_ms)
00184 {   uint32_t waited=0;
00185     while( (wheelsState!=LRWHEELS_STOP) &&  (waited < max_wait_ms) ) 
00186       { wait(0.1);  waited += 100; }
00187 } // botWaitEnd
00188 
00189 void Creabot::iStop()
00190 {   // Call the StopRun method, so wheels detach their tickers
00191     wheelLeft->StopRun();
00192     wheelRight->StopRun();
00193     // wheelsAllStoppedCB could handle the rest of the cleanup, but would also trigger external callback
00194     // Turn motor power off!
00195     wheelLeft->MotorOFF();
00196     wheelRight->MotorOFF();
00197     wheelsState = LRWHEELS_STOP;
00198 }
00199 
00200 // *****************************************************************
00201 //   Creabot Mid level control functions:  
00202 //     all directly access the wheel, no Queue */ 
00203 // *****************************************************************
00204 
00205 // set botSpeed_cm_sec parameter for next high level commands 
00206 void Creabot::setSpeed(float AbotSpeed_cm_sec)
00207 {   if (AbotSpeed_cm_sec > MAX_SPEED_CM_SEC) botSpeed_cm_sec = MAX_SPEED_CM_SEC;
00208         else if (AbotSpeed_cm_sec<MIN_SPEED_CM_SEC) botSpeed_cm_sec = MIN_SPEED_CM_SEC;
00209         else botSpeed_cm_sec = AbotSpeed_cm_sec;
00210     // Low level setting of wheel speed is done inside SetMotsSpeed: 
00211 }
00212 
00213 // Mid level control function:  advance bot straight forward for a given distance
00214 void Creabot::moveForward(float dist_cm)
00215 {   
00216     wheelsSetSpeed(botSpeed_cm_sec);
00217     wheelRight->RunDist_cm(dist_cm );
00218     wheelLeft ->RunDist_cm(dist_cm ); // Direction = defaultDirection
00219     // negative distances are run by RunDist_cm in opposite direction!
00220     wheelsState = LRWHEELS_RUN;
00221 }
00222 
00223 // Mid level control function:  reverse bot straight backwards for a given distance
00224 void Creabot::moveBackward(float dist_cm)
00225 {
00226     wheelsSetSpeed(botSpeed_cm_sec);
00227     wheelRight->RunDist_cm(-dist_cm);
00228     wheelLeft ->RunDist_cm(-dist_cm);// Direction = defaultDirection
00229     // negative distance are run by RunDist_cm in opposite direction!
00230     wheelsState = LRWHEELS_RUN;
00231 }
00232 
00233 /* Mid level control function: turn bot forward right, 
00234    around a radius equl the bot size; i.e. turn around the right wheel */
00235 void Creabot::moveRight(float turn_angle_deg)
00236 {
00237     wheelLeft->setSpeed_cm_sec(botSpeed_cm_sec);
00238     wheelLeft->RunTurnAngle(turn_angle_deg,botDiameter);
00239     wheelsState = LWHEEL_RUNS;
00240 }
00241 
00242 /* Mid level control function: turn bot forward right, 
00243 around a radius that is turn_radius_cm away from the right wheel*/
00244 void Creabot::moveRight(float turn_angle_deg, float turn_radius_cm)
00245 {   // If Center_cm is too small the right wheel has practically nothing to do ;)
00246     if (fabs(double(turn_radius_cm))<0.1) { moveRight(turn_angle_deg); } 
00247     else {
00248         wheelLeft ->setSpeed_cm_sec(botSpeed_cm_sec);
00249         wheelRight->setSpeed_cm_sec(botSpeed_cm_sec * turn_radius_cm/turn_radius_cm+botDiameter);
00250         wheelRight->RunTurnAngle(turn_angle_deg,turn_radius_cm            );
00251         wheelLeft ->RunTurnAngle(turn_angle_deg,turn_radius_cm+botDiameter);// Direction = defaultDirection
00252         // negative distance are run by RunTurnAngle in opposite direction!
00253         wheelsState = LRWHEELS_RUN;
00254       } // else
00255 }
00256 
00257 /* Mid level control function: turn bot backwards Right, 
00258    around a radius that is turn_radius_cm away from the Right wheel */
00259 void Creabot::moveBackRight(float turn_angle_deg, float turn_radius_cm)
00260 { moveRight( -turn_angle_deg , turn_radius_cm); }
00261 
00262 /* Mid level control function: turn bot backwards Right, 
00263    around a radius equal to the bot size; = the Right wheel stands still */
00264 void Creabot::moveBackRight(float turn_angle_deg)
00265 { moveRight( - turn_angle_deg); }
00266 
00267 /* Mid level control function: turn bot forward left, 
00268    around a radius equal to the bot size; = the left wheel stands still */
00269 void Creabot::moveLeft(float turn_angle_deg)
00270 {
00271     wheelRight->setSpeed_cm_sec(botSpeed_cm_sec);
00272     wheelRight->RunTurnAngle(turn_angle_deg,botDiameter);
00273     wheelsState = RWHEEL_RUNS;
00274 }
00275 
00276 /* Mid level control function: turn bot forward left, 
00277    around a radius that is turn_radius_cm away from the left wheel */
00278 void Creabot::moveLeft(float turn_angle_deg, float turn_radius_cm)
00279 {   // If Center_cm is too small the left wheel has practically nothing to do ;)
00280     if (fabs(double(turn_radius_cm))<0.1) { moveLeft(turn_angle_deg); } 
00281     else {
00282         wheelLeft -> setSpeed_cm_sec(botSpeed_cm_sec * turn_radius_cm/(turn_radius_cm+botDiameter));
00283         wheelRight-> setSpeed_cm_sec(botSpeed_cm_sec);
00284         wheelLeft -> RunTurnAngle(turn_angle_deg,turn_radius_cm);
00285         wheelRight-> RunTurnAngle(turn_angle_deg,turn_radius_cm+botDiameter);// Direction = defaultDirection
00286         // negative distance are run by RunTurnAngle in opposite direction!
00287         wheelsState = LRWHEELS_RUN;
00288         }
00289 }
00290 
00291 /* Mid level control function: turn bot backwards left, 
00292    around a radius that is turn_radius_cm away from the left wheel */
00293 void Creabot::moveBackLeft(float turn_angle_deg, float turn_radius_cm)
00294 { moveLeft( - turn_angle_deg, turn_radius_cm); }
00295 
00296 /* Mid level control function: turn bot backwards left, 
00297    around a radius equal to the bot size; = the left wheel stands still */
00298 void Creabot::moveBackLeft(float turn_angle_deg)
00299 { moveLeft( - turn_angle_deg); }
00300 
00301 /* Mid level control function: turn bot on its place for a given angle.
00302 positive angles turn right, negative angles turn left*/
00303 void Creabot::moveRotate(float angleBot)
00304 {
00305     float angleWheel = angleBot*ratio_wheel_bot;
00306     setSpeed(botSpeed_cm_sec);
00307     if(angleBot<0) {
00308         wheelRight->RunDegrees( angleWheel);
00309         wheelLeft ->RunDegrees(-angleWheel);// Direction = defaultDirection
00310         // negative distance are run by RunDegrees in opposite direction!
00311     } else {
00312         wheelLeft ->RunDegrees( angleWheel);
00313         wheelRight->RunDegrees(-angleWheel);// Direction = defaultDirection
00314         // negative distance are run by RunDegrees in opposite direction!
00315     }
00316     wheelsState = LRWHEELS_RUN;
00317 }
00318 
00319 //******************************************************************************
00320 // low level wheel functions, only filters for rot_angle_deg>0, then passes 
00321 // to wheel Class; and remembers that a wheel moves
00322 //******************************************************************************
00323 
00324 /* Low level access functions: sets both wheel speeds immediately  */ 
00325 void Creabot::wheelsSetSpeed(float mot_speed_cm_sec)
00326 {
00327     wheelLeft ->setSpeed_cm_sec(mot_speed_cm_sec);
00328     wheelRight->setSpeed_cm_sec(mot_speed_cm_sec);
00329 }
00330 
00331 //******************************************************************************
00332 // low level wheel functions, handles callbacks from wheels
00333 // *****************************************************************************
00334 
00335 void Creabot::wheelLeftStoppedCB()
00336 {   wheelsState = TWheelsState( wheelsState & LWHEEL_RUNS );  // mask out Left Wheel status bit
00337     if (wheelsState == LRWHEELS_STOP) // all wheels are now known to have stopped
00338         wheelsAllStoppedCB();
00339 }
00340 
00341 void Creabot::wheelRightStoppedCB()
00342 {   wheelsState = TWheelsState( wheelsState & RWHEEL_RUNS );  // mask out Left Wheel status bit
00343     if (wheelsState == LRWHEELS_STOP) // all wheels are now known to have stopped
00344         wheelsAllStoppedCB();
00345 }
00346 
00347 void Creabot::wheelsAllStoppedCB()
00348 {   // assume that already at entry wheelsState == LRWHEELS_STOP
00349     // Try to execute next entry in queue, if one is present
00350     if ( !qBlock ) 
00351       { qExecuteNext();// not blocked, so can execute now
00352         // if wheels are still STOPed, that means no command to execute was left in the queue 
00353         if (wheelsState==LRWHEELS_STOP) 
00354           { // Turn motor power off!
00355             wheelLeft->MotorOFF();
00356             wheelRight->MotorOFF();
00357             // Call the externall callback function to inform about the end of all movements
00358             if(extCallBack!=NULL) extCallBack( wheelsState );
00359           } // if wheelsState==LRWHEELS_STOP
00360       } // if ( !qBlock ) 
00361       else qCollide = true; // Remember the collision to execute the next command. 
00362 }
00363 
00364 
00365