My Version of CreaBotLib
Fork of CreaBotLib by
CreaBot.cpp
- Committer:
- sepp_nepp
- Date:
- 2019-04-18
- Revision:
- 12:530772639065
- Parent:
- 11:5a94af0afa12
File content as of revision 12:530772639065:
#include "CreaBot.h" // ***************************************************************** // BotCommand Structure Helper // ***************************************************************** // Struct Helper; set structure fields to values */ void BotCommand::set(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm) { command = Acommand; dist_cm = Adist_cm; turn_angle_deg= Aturn_angle_deg; }; // Struct Helper; set structure fields to values */ void BotCommand::set(BotCmdVerb Acommand, float Aparam) { command = Acommand; dist_cm = Aparam; turn_angle_deg= Aparam; }; // ***************************************************************** // Creabot Class // ***************************************************************** Creabot::Creabot(CreaMot *left, CreaMot *right, float diameter_wheel_cm, float bot_diam_cm): wheelLeft(left), wheelRight(right) { // TODO: To catch for illegal dimensions (<= 0) // Initialize wheel geometires wheelLeft ->setDiamCM( diameter_wheel_cm ); wheelRight->setDiamCM( diameter_wheel_cm ); // setup the bot parameters: botDiameter = bot_diam_cm; ratio_wheel_bot = bot_diam_cm/diameter_wheel_cm; botSpeed_cm_sec = DEFAULT_SPEED_CM_SEC; // Attach the Crabot functions as callbacks to the wheels wheelLeft ->callbackSet(this, &Creabot::wheelLeftStoppedCB); wheelRight->callbackSet(this, &Creabot::wheelRightStoppedCB); // Initialize own callback as empty extCallBack = NULL; // setup the default Directions and State Values: // used below as variable instead of always passing a direction wheelLeft ->defaultDirection = COUNTERCLOCKWISE; wheelLeft ->StateValue = LWHEEL_RUNS; wheelRight->defaultDirection = CLOCKWISE; wheelRight->StateValue = RWHEEL_RUNS; // Initialize the own status fields wheelsState = LRWHEELS_STOP; qReset(); } // ***************************************************************** // Queue Queue management // ***************************************************************** void Creabot::qMove(BotCmdVerb Acommand, float Aparam) { qMove(Acommand, Aparam, Aparam); } /* Add a new movement to the queue, if not full. * ATTENTION: * qExecuteNext() is called asynchronsously by the motor Callback wheelsAllStoppedCB() * qExecuteNext() tries to read access the next command in the queue. * A simultaneous read and write access to the same resources can lead to * conflict and must be carefully avoided. * This conflict concerns the read-modify-write access of variable Count. * It is avoided by using the qBlock variable. With qBlock set during increment * a simultaneous read access is blocked * Any changes to those write and read handlers must take into account that * wheelsAllStoppedCB() and qExecuteNext() may be called any time while * executing qMove command. */ void Creabot::qMove(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm) { if( !qIsFull() ) { cmd[writeIdx].set(Acommand, Aturn_angle_deg, Adist_cm); writeIdx++; if (writeIdx==DEPTH_Queue) writeIdx=0; qCollide = false; // colliding access to qCollide cannot happen while qBlock = false // if callback happens here, there is no collosion, the last old command gets already executed. qBlock = true; // if callback happens here while qBlock = true, the collision is remembered in qCollide Count++; qBlock = false; // if callback happens here there is no more blockage and the next (maybe the new) command is already being executed // in this case qCollide = false, and wheelsState!=LRWHEELS_STOP if ( qCollide || (wheelsState==LRWHEELS_STOP) ) // callback cannot happen here since the wheelState is already STOP, or Collide has happened before! qExecuteNext(); }; } // Queue is worked through while still any elements inside // ATTENTION read above consideration of conflicting accesses between qMove() and wheelsAllStoppedCB() // Think careful if a case can occur that pExecuteNext is called twice immediately following each other, // once by qMove() and once by wheelsAllStoppedCB() ??? void Creabot::qExecuteNext() { if ( !qIsEmpty() ) // see if something in the queue to execute { // Execute and then remove the oldest element at the tail of the Queue iExeCommand( &cmd[readIdx] ); readIdx++; if (readIdx==DEPTH_Queue) readIdx=0; Count--; }; } // Immediately end All, the queue and the motor void Creabot::qStopAll() { qReset(); // empty the queue iStop(); // stop the motors, do not call external callback } //************************************************************************** // Using the BotCmdVerb to encode different commands, done immediately //************************************************************************** void Creabot::iMoveAndWait(BotCmdVerb moveType, float Aparam) { iMove(moveType,Aparam,Aparam); iWaitEnd(); } void Creabot::iMoveAndWait(BotCmdVerb moveType, float Aturn_angle_deg, float Adist_cm) { iMove(moveType,Aturn_angle_deg,Adist_cm); iWaitEnd(); } void Creabot::iMove(BotCmdVerb moveType, float Aparam) { iMove(moveType, Aparam, Aparam); } void Creabot::iMove(BotCmdVerb moveType, float Aturn_angle_deg, float Adist_cm) { BotCommand Anew_cmd; Anew_cmd.set(moveType, Aturn_angle_deg, Adist_cm); iExeCommand(&Anew_cmd); } /** High level, immediate: move bot according to prefilled command structures. * Recommended to use iMove() methods to fill the command structure correctly. * Branches to the moveXXXX() methods. For details see docs for those methods. * Warning: Collides with queued commands if called individually. * @param[in] <*cmd> Pointer to type BotCommand, the prefilled command structure,*/ void Creabot::iExeCommand(BotCommand *cmd) { switch (cmd->command) { case FORWARD: moveForward(cmd->dist_cm); break; case BACKWARD: moveBackward(cmd->dist_cm); break; case LEFT: moveLeft(cmd->turn_angle_deg, cmd->dist_cm); break; case RIGHT: moveRight(cmd->turn_angle_deg, cmd->dist_cm); break; case BACKLEFT: moveBackLeft(cmd->turn_angle_deg, cmd->dist_cm); break; case BACKRIGHT: moveBackRight(cmd->turn_angle_deg, cmd->dist_cm); break; case ROTATE: moveRotate(cmd->turn_angle_deg); break; default: break; }; }; // executeCommand /* spends all time with waits, returns only once wheelState = LRWHEELS_STOP */ void Creabot::iWaitEnd() { while(wheelsState!=LRWHEELS_STOP) wait(0.1); } /* spends all time with waits, returns only once wheelState = LRWHEELS_STOP, but maximum max_wait_ms milli seconds */ void Creabot::iWaitEnd(uint32_t max_wait_ms) { uint32_t waited=0; while( (wheelsState!=LRWHEELS_STOP) && (waited < max_wait_ms) ) { wait(0.1); waited += 100; } } // botWaitEnd void Creabot::iStop() { // Call the StopRun method, so wheels detach their tickers wheelLeft->StopRun(); wheelRight->StopRun(); // wheelsAllStoppedCB could handle the rest of the cleanup, but would also trigger external callback // Turn motor power off! wheelLeft->MotorOFF(); wheelRight->MotorOFF(); wheelsState = LRWHEELS_STOP; } // ***************************************************************** // Creabot Mid level control functions: // all directly access the wheel, no Queue */ // ***************************************************************** // set botSpeed_cm_sec parameter for next high level commands void Creabot::setSpeed(float AbotSpeed_cm_sec) { if (AbotSpeed_cm_sec > MAX_SPEED_CM_SEC) botSpeed_cm_sec = MAX_SPEED_CM_SEC; else if (AbotSpeed_cm_sec<MIN_SPEED_CM_SEC) botSpeed_cm_sec = MIN_SPEED_CM_SEC; else botSpeed_cm_sec = AbotSpeed_cm_sec; // Low level setting of wheel speed is done inside SetMotsSpeed: } // Mid level control function: advance bot straight forward for a given distance void Creabot::moveForward(float dist_cm) { wheelsSetSpeed(botSpeed_cm_sec); wheelRight->RunDist_cm(dist_cm ); wheelLeft ->RunDist_cm(dist_cm ); // Direction = defaultDirection // negative distances are run by RunDist_cm in opposite direction! wheelsState = LRWHEELS_RUN; } // Mid level control function: reverse bot straight backwards for a given distance void Creabot::moveBackward(float dist_cm) { wheelsSetSpeed(botSpeed_cm_sec); wheelRight->RunDist_cm(-dist_cm); wheelLeft ->RunDist_cm(-dist_cm);// Direction = defaultDirection // negative distance are run by RunDist_cm in opposite direction! wheelsState = LRWHEELS_RUN; } /* Mid level control function: turn bot forward right, around a radius equl the bot size; i.e. turn around the right wheel */ void Creabot::moveRight(float turn_angle_deg) { wheelLeft->setSpeed_cm_sec(botSpeed_cm_sec); wheelLeft->RunTurnAngle(turn_angle_deg,botDiameter); wheelsState = LWHEEL_RUNS; } /* Mid level control function: turn bot forward right, around a radius that is turn_radius_cm away from the right wheel*/ void Creabot::moveRight(float turn_angle_deg, float turn_radius_cm) { // If Center_cm is too small the right wheel has practically nothing to do ;) if (fabs(double(turn_radius_cm))<0.1) { moveRight(turn_angle_deg); } else { wheelLeft ->setSpeed_cm_sec(botSpeed_cm_sec); wheelRight->setSpeed_cm_sec(botSpeed_cm_sec * turn_radius_cm/turn_radius_cm+botDiameter); wheelRight->RunTurnAngle(turn_angle_deg,turn_radius_cm ); wheelLeft ->RunTurnAngle(turn_angle_deg,turn_radius_cm+botDiameter);// Direction = defaultDirection // negative distance are run by RunTurnAngle in opposite direction! wheelsState = LRWHEELS_RUN; } // else } /* Mid level control function: turn bot backwards Right, around a radius that is turn_radius_cm away from the Right wheel */ void Creabot::moveBackRight(float turn_angle_deg, float turn_radius_cm) { moveRight( -turn_angle_deg , turn_radius_cm); } /* Mid level control function: turn bot backwards Right, around a radius equal to the bot size; = the Right wheel stands still */ void Creabot::moveBackRight(float turn_angle_deg) { moveRight( - turn_angle_deg); } /* Mid level control function: turn bot forward left, around a radius equal to the bot size; = the left wheel stands still */ void Creabot::moveLeft(float turn_angle_deg) { wheelRight->setSpeed_cm_sec(botSpeed_cm_sec); wheelRight->RunTurnAngle(turn_angle_deg,botDiameter); wheelsState = RWHEEL_RUNS; } /* Mid level control function: turn bot forward left, around a radius that is turn_radius_cm away from the left wheel */ void Creabot::moveLeft(float turn_angle_deg, float turn_radius_cm) { // If Center_cm is too small the left wheel has practically nothing to do ;) if (fabs(double(turn_radius_cm))<0.1) { moveLeft(turn_angle_deg); } else { wheelLeft -> setSpeed_cm_sec(botSpeed_cm_sec * turn_radius_cm/(turn_radius_cm+botDiameter)); wheelRight-> setSpeed_cm_sec(botSpeed_cm_sec); wheelLeft -> RunTurnAngle(turn_angle_deg,turn_radius_cm); wheelRight-> RunTurnAngle(turn_angle_deg,turn_radius_cm+botDiameter);// Direction = defaultDirection // negative distance are run by RunTurnAngle in opposite direction! wheelsState = LRWHEELS_RUN; } } /* Mid level control function: turn bot backwards left, around a radius that is turn_radius_cm away from the left wheel */ void Creabot::moveBackLeft(float turn_angle_deg, float turn_radius_cm) { moveLeft( - turn_angle_deg, turn_radius_cm); } /* Mid level control function: turn bot backwards left, around a radius equal to the bot size; = the left wheel stands still */ void Creabot::moveBackLeft(float turn_angle_deg) { moveLeft( - turn_angle_deg); } /* Mid level control function: turn bot on its place for a given angle. positive angles turn right, negative angles turn left*/ void Creabot::moveRotate(float angleBot) { float angleWheel = angleBot*ratio_wheel_bot; setSpeed(botSpeed_cm_sec); if(angleBot<0) { wheelRight->RunDegrees( angleWheel); wheelLeft ->RunDegrees(-angleWheel);// Direction = defaultDirection // negative distance are run by RunDegrees in opposite direction! } else { wheelLeft ->RunDegrees( angleWheel); wheelRight->RunDegrees(-angleWheel);// Direction = defaultDirection // negative distance are run by RunDegrees in opposite direction! } wheelsState = LRWHEELS_RUN; } //****************************************************************************** // low level wheel functions, only filters for rot_angle_deg>0, then passes // to wheel Class; and remembers that a wheel moves //****************************************************************************** /* Low level access functions: sets both wheel speeds immediately */ void Creabot::wheelsSetSpeed(float mot_speed_cm_sec) { wheelLeft ->setSpeed_cm_sec(mot_speed_cm_sec); wheelRight->setSpeed_cm_sec(mot_speed_cm_sec); } //****************************************************************************** // low level wheel functions, handles callbacks from wheels // ***************************************************************************** void Creabot::wheelLeftStoppedCB() { wheelsState = TWheelsState( wheelsState & LWHEEL_RUNS ); // mask out Left Wheel status bit if (wheelsState == LRWHEELS_STOP) // all wheels are now known to have stopped wheelsAllStoppedCB(); } void Creabot::wheelRightStoppedCB() { wheelsState = TWheelsState( wheelsState & RWHEEL_RUNS ); // mask out Left Wheel status bit if (wheelsState == LRWHEELS_STOP) // all wheels are now known to have stopped wheelsAllStoppedCB(); } void Creabot::wheelsAllStoppedCB() { // assume that already at entry wheelsState == LRWHEELS_STOP // Try to execute next entry in queue, if one is present if ( !qBlock ) { qExecuteNext();// not blocked, so can execute now // if wheels are still STOPed, that means no command to execute was left in the queue if (wheelsState==LRWHEELS_STOP) { // Turn motor power off! wheelLeft->MotorOFF(); wheelRight->MotorOFF(); // Call the externall callback function to inform about the end of all movements if(extCallBack!=NULL) extCallBack( wheelsState ); } // if wheelsState==LRWHEELS_STOP } // if ( !qBlock ) else qCollide = true; // Remember the collision to execute the next command. }