My Version of CreaBotLib
Fork of CreaBotLib by
CreaBot.h
- Committer:
- sepp_nepp
- Date:
- 2019-04-17
- Revision:
- 10:79509113310a
- Parent:
- 9:efe9f76d6f76
- Child:
- 11:5a94af0afa12
File content as of revision 10:79509113310a:
/* * \file CreaBot.h * \brief File contains Creabot Library. * CreaBot.h contains the Creabot class, and required enums and structs. * Imports "mbed.h" and "motor.h" * Refactorings: * All variables with suffixes "_cm_sec" = speeds in centimeters per second. * MotCommand -> BotCommand. * cmdbot_t -> BotCmdVerb. * cm -> dist_cm. * 'motorxx' -> 'wheelxxx': each motor now becomes a 'wheel with a diameter' * FIFO queue management merged with Creabot Class, now called Queue * Queue does not use another extra ticker anymore. Instead triggers from Motor End commands. * Queue is worked through, and Callback is not called, until queue is empty. * * @author Tarek Lule based on work of Francois Druilhe, et al. * @date 21. April 2019 * @see https://os.mbed.com/users/sepp_nepp/code/CreaBotLib/ */ // -------------------- wheel --------------------------- #ifndef CREABOT_H #define CREABOT_H #include "mbed.h" #include "motor.h" #define DEFAULT_SPEED_CM_SEC 2.0f /**< Default advancement speed = 2.0cm/sec, was DEFAULT_SPEED */ #define DEPTH_Queue 256 /**< Initialize the depth of the command Queue to 256 */ /** \enum BotCmdVerb * \brief Robot Commands Verbs, gives the movement direction * IDLE is no longer supported */ typedef enum { FORWARD, /**< Advance the robot straight forward */ BACKWARD, /**< Reverse the robot straight backwards */ ROTATE, /**< Rotate around its own axis*/ LEFT, /**< Advance in a left curve */ RIGHT, /**< Advance in a right curve */ BACKLEFT, /**< Reverse in a left curve */ BACKRIGHT /**< Reverse in a right curve */ } BotCmdVerb; /** \enum TWheelsState * \brief Possible states of the two wheels of the CreaBot * Can be ored together */ typedef enum { LRWHEELS_STOP = 0, /**< All wheels have stopped */ LWHEEL_RUNS = 1, /**< Left wheel runs */ RWHEEL_RUNS = 2, /**< Right wheel runs */ LRWHEELS_RUN = 3, /**< Both wheels run */ } TWheelsState; /** \struct BotCommand * \brief Structure of a CreaBot Command. * The command structure is put into command Queue, and treated by the Queue-Handler */ typedef struct { BotCmdVerb command; /**< Command type that gives movement direction.*/ float dist_cm; /**< Distance in dist_cm for translational movements .*/ float turn_angle_deg; /**< Angle in degree for rotational movement .*/ void set(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm); /**< Helper; set structure fields to values */ void set(BotCmdVerb Acommand, float Aparam); /**< Helper; set structure fields to values */ } BotCommand; /** \class Creabot * \brief Synchronous Control of 2 wheels as part of a two wheel robot * * Handles two instances of CreaMot from motor.h library simultaneously. * Using the set distance between the wheels, allows to run pecise curves. * A first other set of movement functions starting with qXXX are using command verbs, * these commands are queued up in a waiting queue, * and are executed one by one in programmed order. * A second set of movement functions starting with iXXX are also using command verbs, * however these commands are executed immediately, * they override each other if issued while the previous movement still ongoing * A third set of movements functions starting with moveXXXX are provided, * each function performs one specific movement, also immediately. * So they also override each other, and collide with queued commands. * A callback is supplied to react to the end of all programmed movements. * * Example: * @code * // --- Define the Four PINs & Time of movement used for wheel drive ----- * CreaMot wheelLeft(PA_12, PB_0, PB_1, PB_6); // Declare first the 2 wheels (to avoid to have an object with 8 pins to create) * CreaMot wheelRight(PA_5,PA_4,PA_3,PA_1); * Creabot mybot(&wheelLeft, &WheelRight, 10.0f, 13.0f); // insert the wheels and indicate wheel diameter (10cm) and distance between wheels (13cm) * * int main() { * * mybot.setSpeed(12.5); // Preset speed to 12.5cm/s * mybot.iMove(FORWARD,10); // Go forward of 10cm * mybot.iWaitEnd(); // Wait end of Move * mybot.iMove(ROTATE,90); // Start rotation of 90° around the center between wheels (two wheels running in same direction at same speed) * mybot.iMove(BACKWARD,40); // Stop immediately the rotation and go backward of 40cm * mybot.iWaitEnd(); // Wait end of Backward * mybot.iMoveAndWait(LEFT,60); // Move Left of 60° in circle, center being the left wheel (off). Wait end of move * mybot.iWaitEnd(); // Not needed, as already waited... * mybot.iMoveAndWait(RIGHT,45, 33); // Move Right of 45°, center being at 33cm of the right wheel. Right wheel moving slower and on a shorter distance than left one. * mybot.iMoveAndWait(ROTATE,90); * mybot.iMove(ROTATE,-90); // Opposite direction. * mybot.iWaitEnd(6); // with time-out => if after 6s the move is not ended, continue the execution * mybot.iStop(); // Stop the movement in case it was not ended before ... * * // Same with a Queue of command, opposite to the move, receiving a new command will not stop the current execution, but the bot will store it. * mybot.qMove(FORWARD,10); // Already starting... * mybot.qMove(BACKWARD,10); // will wait end of previous command to go * mybot.qMove(ROTATE,120.0); * mybot.qMove(LEFT, 30, 120); * mybot.qMove(RIGHT, 25); * ... // insert other code here that executes while the motors continue to move * mybot.iWaitEnd(100000); // wait until Queue end... can flush anytime with stopMove... * mybot.qEmpty(); // before end... Flush the Queue and remove all instructions… * * while(1) { * }; * } * @endcode */ class Creabot { public: /** Create a Creabot object with 2 wheels * * @param <left CreaMot> object, corresponding to left wheel of the Creabot * @param <right CreaMot> object, corresponding to right wheel of the Creabot * @param <wheel_diam_cm> diameter in cm of the wheels (both must be the same diameter) * @param <bot_diam_cm> distance cm between center of left wheel and center of right wheel */ Creabot(CreaMot *left, CreaMot *right, float wheel_diam_cm, float bot_diam_cm); /** Property access to wheel state, indicating which of the wheels are moving */ TWheelsState getState() {return wheelsState; }; /** Setup a Callback method. It is called when all programmed movements are finished. */ void setCallBack(void (*Acallback)(int status)) { extCallBack = Acallback; }; /** High level: set bot-speed parameter for all future wheel commands. * The set speed is used for immediate as well as the queued movements. * In a curve movement it determines the speed of the outer wheel. */ void setSpeed(float AbotSpeed_cm_sec); public: /** High level, queued: move bot according to command and parameter * Composes a BotCommand and appends it to the queue for queued execution. * Use for commands that need only one parameter: FORWARD, BACKWARD, ROTATE * For details refer to docu of moveForward(), moveBackward(), moveRotate(). * Can also be called with IDLE, but then has no effect. * Preset the speed using setSpeed().] * * @param[in] <Acommand> Requested movement, of type BotCmdVerb. * @param[in] <Aparam> Requested amount, defines angle for ROTATE, or distance for FORWARD, BACKWARD. */ void qMove(BotCmdVerb Acommand, float Aparam); /** High level, queued : move bot according to command and parameters * Composes a BotCommand and appends it to the queue for queued execution. * Use for commands that need two parameters: LEFT, RIGHT, BACKLEFT, BACKRIGHT * For details refer to docu of moveLeft(), moveRight(), moveBackLeft(), moveBackRight(). * Can also be called with IDLE, but then has no effect. * * Reserves a new command element at the head of the Queue * * Adds incoming commands at the head of the ring buffer at position writeIdx, * If Queue is full, it passes back NULL * Otherwise Advances the write index once and returns a pointer the next free command struct. * The caller then must fill the command structure at that position. * Do not free memory associated to the pointer. * * @param[in] <Acommand> Requested movement, of type BotCmdVerb. * @param[in] <Aturn_angle_deg> Requested angle in degrees. * @param[in] <Adist_cm> Requested distance in centimeters. */ void qMove(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm); /** Execute and remove the oldest element at the tail of the Queue * * If Queue is empty, nothing happens * Otherwise executes oldest commands from the tail of the ring buffer at position readIdx, * Then advances the read index once. */ void qExecuteNext(); /** Public access of Queue used Count. * * @return <int> the number of commands in the Queue */ int qCount() {return Count;} /* Immediately end all queue activites, and the motor * Does not call the external callback handler */ void qStopAll(); private: /** Empty the Queue. * Since the ring buffer is static, it suffice to set all pointers to 0, * Commands are left in memory. */ void qReset() { qBlock = true; readIdx=writeIdx=Count=0; qBlock = false; qCollide = false;}; /** Check if Queue is full. * * Space is limited to DEPTH_Queue=256 BotCommand elements. * If in doubt it should be checked before trying to add new elements * @return <bool> True if Queue is full*/ bool qIsFull() {return Count>=DEPTH_Queue;} /** Check if Queue is empty. * * Should be checked before trying to get new elements * @return <bool> True if Queue is empty*/ bool qIsEmpty() {return Count<=0;} /** 256 elements deep Command Queue, to put BotCommand in a queue. * Stores all BotCommand in this static 256 array used as a circular ring buffer. */ BotCommand cmd[DEPTH_Queue]; /**< Actual static Queue array where all elements reside. */ int writeIdx; /**< Index in Queue array where to put the next new element to. */ int readIdx; /**< Index in Queue array where to get the oldest element from. */ int Count; /**< Counts and keeps track of the number of elements in array used. */ bool qBlock; /**< Blocks read access while a write is ongoing. */ bool qCollide;/**< Indicates a colliding access to the queue. */ public: /** High level, immediate: move bot and wait for movement end. * Simple wrapper for iMove(Acommand,Aparam) and iWaitEnd(). * Refer to those methods for further docs. */ void iMoveAndWait(BotCmdVerb Acommand, float Aparam); /** High level, immediate: move bot and wait for movement end. * Simple wrapper for iMove(Acommand,Aturn_angle_deg,Adist_cm) and iWaitEnd(). * Refer to those methods for further docs. */ void iMoveAndWait(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm); /** High level, immediate: move bot according to command and parameter * Composes a BotCommand and passes it to executeCommand(). * Use for commands that need only one parameter: FORWARD, BACKWARD, ROTATE * For details refer to docu of moveForward(), moveBackward(), moveRotate(). * Can also be called with IDLE, but then has no effect. * Preset the speed using setSpeed(). * Warning: Collides with queued commands. * * @param[in] <Acommand> Requested movement, of type BotCmdVerb. * @param[in] <Aparam> Requested amount, defines angle for ROTATE, or distance for FORWARD, BACKWARD. */ void iMove(BotCmdVerb Acommand, float Aparam); /** High level, immediate: move bot according to command and parameters * Composes a BotCommand and passes it to executeCommand(). * Use for commands that need two parameters: LEFT, RIGHT, BACKLEFT, BACKRIGHT * For details refer to docu of moveLeft(), moveRight(), moveBackLeft(), moveBackRight(). * Can also be called with IDLE, but then has no effect. * Preset the speed using setSpeed(). * Warning: Collides with queued commands. * * @param[in] <Acommand> Requested movement, of type BotCmdVerb. * @param[in] <Aturn_angle_deg> Requested angle in degrees. * @param[in] <Adist_cm> Requested distance in centimeters. */ void iMove(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm); /** 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. * Preset the speed using setSpeed(). * Warning: Collides with queued commands if called individually. * * @param[in] <*cmd> Pointer to type BotCommand, the prefilled command structure, */ void iExeCommand(BotCommand *cmd); /** High Level: spends all time with waits, * returns only once wheelState = LRWHEELS_STOP, * not recommended due its blocking behavior */ void iWaitEnd(); /** High Level: spends all time with waits, * returns only once wheelState = LRWHEELS_STOP, * but waits no more than max_wait_ms milli seconds. * Not recommended due its blocking behavior */ void iWaitEnd(uint32_t max_wait_ms); // time-out /** High level, immediate: Both wheels get stopped, and turned off * updates the wheel state wheelsState, does not call the callback handler! * It does not empty the queue. * Adding new elements into the queue after calling iStop() * Would continue using all commands still in the queue. */ void iStop(); public: /** Mid level, immediate: advance bot straight forward for a given distance, * Preset the speed using setSpeed(). * Warning: Collides with queued commands. */ void moveForward(float dist_cm); /** Mid level, immediate: reverse bot straight backwards for a given distance, * Preset the speed using setSpeed(). * Warning: Collides with queued commands. */ void moveBackward(float dist_cm); /* Mid level, immediate: turn bot forward right, * around a radius twice the bot size , * Same as moveRight(turn_angle_deg, 0); * Preset the speed using setSpeed(). * Warning: Collides with queued commands. */ void moveRight(float turn_angle_deg); /** Mid level, immediate: turn bot forward right, * around a radius that is center_cm away from the right wheel, * Preset the speed using setSpeed(). * Warning: Collides with queued commands. */ void moveRight(float turn_angle_deg, float center_cm); /** Mid level, immediate: turn bot forward left, * around a radius twice the bot size, * Same as moveLeft(turn_angle_deg, 0); * Preset the speed using setSpeed(). * Warning: Collides with queued commands. */ void moveLeft(float turn_angle_deg); /** Mid level, immediate: turn bot forward left, * around a radius that is center_cm away from the left wheel, * Preset the speed using setSpeed(). * Warning: Collides with queued commands. */ void moveLeft(float turn_angle_deg, float center_cm); /* Mid level, immediate: turn bot backwards right, * around a radius twice the bot size , * Same as moveBackRight(turn_angle_deg, 0); * Preset the speed using setSpeed(). * Warning: Collides with queued commands. */ void moveBackRight(float turn_angle_deg); /** Mid level, immediate: turn bot backwards right, * around a radius that is center_cm away from the right wheel, * Preset the speed using setSpeed(). * Warning: Collides with queued commands. */ void moveBackRight(float turn_angle_deg, float center_cm); /** Mid level, immediate: turn bot backwards left, * around a radius twice the bot size, * Same as moveBackLeft(turn_angle_deg, 0); * Preset the speed using setSpeed(). * Warning: Collides with queued commands. */ void moveBackLeft(float turn_angle_deg); /** Mid level, immediate: turn bot backwards left, around a radius that is center_cm away from the left wheel, * Preset the speed using setSpeed(). * Warning: Collides with queued commands. */ void moveBackLeft(float turn_angle_deg, float center_cm); /** Mid level, immediate: turn bot on its place for a given angle. * positive angles turn right, negative angles turn left, * Preset the speed using setSpeed(). * Warning: Collides with queued commands. */ void moveRotate(float turn_angle_deg); private: /** Low level, immediate: sets Both wheel speeds */ void wheelsSetSpeed(float mot_speed_cm_sec); /** Callback when left wheel stops; * updates the wheel state wheelsState */ void wheelLeftStoppedCB(); /** Callback when right wheel stops. * updates the wheel state wheelsState */ void wheelRightStoppedCB(); /** Callback when all wheels have stopped. * updates the wheel state wheelsState, switch off all Phases */ void wheelsAllStoppedCB(); /** Callback function pointer: * If set non-NULL it is called when All wheels Stopped()*/ void (*extCallBack)(int status); /** Wheel status register: * Remembers which of the wheels still run */ TWheelsState wheelsState; /** geometric properties of the bot */ float botDiameter; /** Ratio of wheel diameter and bot diameter */ float ratio_wheel_bot; /** last requested bot speed */ float botSpeed_cm_sec; /** The two wheels, as pointers to their classes */ CreaMot *wheelLeft; CreaMot *wheelRight; }; #endif