CreaBot.h

00001 /*
00002  * \file CreaBot.h
00003  * \brief File contains Creabot Library.
00004  
00005  * CreaBot.h contains the Creabot class, and required enums and structs.
00006  * Imports "mbed.h" and "CreaMot.h"
00007  
00008  * Refactorings:
00009  * All variables with suffixes "_cm_sec" = speeds in centimeters per second.
00010  * MotCommand -> BotCommand.
00011  * cmdbot_t -> BotCmdVerb.
00012  * cm -> dist_cm.
00013  * 'motorxx' -> 'wheelxxx': each motor now becomes a 'wheel with a diameter'
00014  * FIFO queue management merged with Creabot Class, now called Queue 
00015  * Queue does not use another extra ticker anymore. Instead triggers from Motor End commands. 
00016  * Queue is worked through, and Callback is not called, until queue is empty. 
00017  *
00018  * @author Tarek Lule based on work of Francois Druilhe, et al.
00019  * @date 21. April 2019
00020  * @see https://os.mbed.com/users/sepp_nepp/code/CreaBotLib/  */
00021  
00022  // -------------------- wheel ---------------------------
00023  
00024 #ifndef CREABOT_H
00025 #define CREABOT_H
00026 
00027 #include "mbed.h"
00028 #include "CreaMot.h"
00029 
00030 #define DEFAULT_SPEED_CM_SEC 2.0f /**< Default advancement speed = 2.0cm/sec, was DEFAULT_SPEED */
00031 #define DEPTH_Queue 256  /**< Initialize the depth of the command Queue to 256 */
00032 
00033 /** \enum BotCmdVerb 
00034 * \brief Robot Commands Verbs, gives the movement direction
00035 * IDLE is no longer supported   */
00036 typedef enum {
00037     FORWARD,    /**< Advance the robot straight forward   */  
00038     BACKWARD,   /**< Reverse the robot straight backwards */  
00039     ROTATE,     /**< Rotate around its own axis*/  
00040     LEFT,       /**< Advance in a left curve   */
00041     RIGHT,      /**< Advance in a right curve  */
00042     BACKLEFT,   /**< Reverse in a left curve   */
00043     BACKRIGHT   /**< Reverse in a right curve  */
00044 } BotCmdVerb;
00045 
00046 
00047 /** \enum TWheelsState
00048 * \brief Possible states of the two wheels of the CreaBot
00049 * Can be ored together */
00050 typedef enum {
00051     LRWHEELS_STOP = 0, /**< All wheels have stopped */  
00052     LWHEEL_RUNS   = 1, /**< Left  wheel  runs */  
00053     RWHEEL_RUNS   = 2, /**< Right wheel  runs */  
00054     LRWHEELS_RUN  = 3, /**< Both  wheels run */  
00055 } TWheelsState;
00056 
00057 /** \struct BotCommand
00058 * \brief Structure of a CreaBot Command.
00059 * The command structure is put into command Queue, and treated by the Queue-Handler */
00060 typedef struct {
00061     BotCmdVerb command; /**< Command type that gives movement direction.*/
00062     float dist_cm;      /**< Distance in dist_cm for translational movements .*/
00063     float turn_angle_deg;    /**< Angle in degree for rotational movement .*/
00064     void set(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm); /**< Helper; set structure fields to values  */
00065     void set(BotCmdVerb Acommand, float Aparam); /**< Helper; set structure fields to values  */
00066 } BotCommand;
00067 
00068 
00069 /** \class Creabot
00070 
00071 * \brief Synchronous Control of 2 wheels as part of a two wheel robot
00072 *
00073 * Handles two instances of CreaMot from motor.h library simultaneously. 
00074 * Using the set distance between the wheels, allows to run pecise curves. 
00075 * 
00076 * A first set of movement functions starting with qXXX are using command verbs,
00077 *   these commands are queued up in a waiting queue, 
00078 *   and are executed one by one in programmed order. 
00079 * 
00080 * A second set of movement functions starting with iXXX are also using command verbs,
00081 *   however these commands are executed immediately, 
00082 *   they override each other if issued while the previous movement still ongoing
00083 *
00084 * A third set of movements functions starting with moveXXXX are provided, 
00085 *   each function performs one specific movement, also immediately. 
00086 *   So they also override each other, and collide with queued commands.
00087 *
00088 * A callback is supplied to react to the end of all programmed movements. 
00089 *
00090 * Example:
00091 * @code
00092 * // --- Define the Four PINs & Time of movement used for wheel drive -----
00093 * 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)
00094 * CreaMot wheelRight(PA_5,PA_4,PA_3,PA_1);
00095 * CreaBot mybot(&wheelLeft, &WheelRight, 10.0f, 13.0f); // insert the wheels and indicate wheel diameter (10cm) and distance between wheels (13cm)
00096 * 
00097 * int main() {
00098 * 
00099 *     mybot.setSpeed(12.5); // Preset speed to 12.5cm/s
00100 *     mybot.iMove(FORWARD,10); // Go forward of 10cm
00101 *     mybot.iWaitEnd(); // Wait end of Move
00102 *     mybot.iMove(ROTATE,90); // Start rotation of 90° around the center between wheels (two wheels running in same direction at same speed)
00103 *     mybot.iMove(BACKWARD,40); // Stop immediately the rotation and go backward of 40cm
00104 *     mybot.iWaitEnd(); // Wait end of Backward
00105 *     mybot.iMoveAndWait(LEFT,60); // Move Left of 60° in circle, center being the left wheel (off). Wait end of move
00106 *     mybot.iWaitEnd();  // Not needed, as already waited... 
00107 *     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. 
00108 *     mybot.iMoveAndWait(ROTATE,90);
00109 *     mybot.iMove(ROTATE,-90); // Opposite direction.
00110 *     mybot.iWaitEnd(6); // with time-out => if after 6s the move is not ended, continue the execution
00111 *     mybot.iStop(); // Stop the movement in case it was not ended before ...
00112 *     
00113 *     // 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.
00114 *     mybot.qMove(FORWARD,10); // Already starting...
00115 *     mybot.qMove(BACKWARD,10); // will wait end of previous command to go
00116 *     mybot.qMove(ROTATE,120.0);
00117 *     mybot.qMove(LEFT, 30, 120);
00118 *     mybot.qMove(RIGHT, 25);
00119 *     ... // insert other code here that executes while the motors continue to move
00120 *     mybot.iWaitEnd(100000); // wait until Queue end... can flush anytime with stopMove...
00121 *     mybot.qEmpty(); // before end... Flush the Queue and remove all instructions…
00122 * 
00123 *     while(1) {
00124 *     };
00125 * }
00126 * @endcode
00127 */
00128  
00129 class Creabot {
00130 public:
00131    /** Create a Creabot object with 2 wheels
00132      *
00133      *  @param[in] <left>           CreaMot object, corresponding to left wheel of the Creabot
00134      *  @param[in] <right>          CreaMot object, corresponding to right wheel of the Creabot
00135      *  @param[in] <wheel_diam_cm>  Diameter in cm of the wheels (both must be the same diameter)
00136      *  @param[in] <bot_diam_cm>    Distance cm between center of left wheel and center of right wheel
00137      */
00138     Creabot(CreaMot *left, CreaMot *right, float wheel_diam_cm, float bot_diam_cm);
00139     
00140     /** Property access to wheel state, indicating which of the wheels are moving */
00141     TWheelsState getState() {return wheelsState; };
00142     
00143     /** Setup a Callback method. It is called when all programmed movements are finished. */
00144     void setCallBack(void (*Acallback)(int status)) { extCallBack = Acallback; };
00145 
00146     /** High level: set bot-speed parameter for all future wheel commands. 
00147     * The set speed is used for immediate as well as the queued movements.
00148     * In a curve movement it determines the speed of the outer wheel.  
00149      *
00150      *  @param[in] <AbotSpeed_cm_sec>     requested movement speed  */
00151     void setSpeed(float AbotSpeed_cm_sec);
00152 
00153 public:
00154 
00155     /** High level, queued:  move bot according to command and parameter
00156     *
00157     * Composes a BotCommand and appends it to the queue for queued execution.
00158     * Use for commands that need only one parameter: FORWARD, BACKWARD, ROTATE
00159     * For details refer to docu of moveForward(), moveBackward(), moveRotate().
00160     * Preset the speed using setSpeed().
00161     *
00162     * @param[in] <Acommand> Requested movement, of type BotCmdVerb.
00163     * @param[in] <Aparam>   Requested amount, defines angle for ROTATE, or distance for FORWARD, BACKWARD.
00164     */
00165     void qMove(BotCmdVerb Acommand, float Aparam);
00166     
00167     /** High level, queued :  move bot according to command and parameters
00168     *
00169     * Composes a BotCommand and appends it to the queue for queued execution.
00170     * Use for commands that need two parameters: LEFT, RIGHT, BACKLEFT, BACKRIGHT
00171     * For details refer to docu of moveLeft(), moveRight(), moveBackLeft(), moveBackRight().
00172     *
00173     * Reserves a new command element at the head of the Queue
00174     *
00175     * Adds incoming commands at the head of the ring buffer at position writeIdx, 
00176     * If Queue is full, it passes back NULL 
00177     * Otherwise Advances the write index once and returns a pointer the next free command struct.
00178     * The caller then must fill the command structure at that position. 
00179     * Do not free memory associated to the pointer.
00180     *
00181     * @param[in] <Acommand> Requested movement, of type BotCmdVerb.
00182     * @param[in] <Aturn_angle_deg>   Requested angle in degrees.
00183     * @param[in] <Adist_cm>   Requested distance in centimeters.
00184     */
00185     void qMove(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm);
00186     
00187     /** Execute and remove the oldest element at the tail of the Queue
00188     *
00189     * If Queue is empty, nothing happens
00190     * Otherwise executes oldest commands from the tail of the ring buffer at position readIdx,
00191     * Then advances the read index once.  */
00192     void qExecuteNext();
00193 
00194     /** Public access of Queue used Count. 
00195     *
00196     * @return <int>  the number of commands in the Queue */
00197     int qCount() {return Count;}
00198 
00199     /* Immediately end all queue activites, and the motor
00200     * Does not call the external callback handler */
00201     void qStopAll();
00202 
00203 private: 
00204     /** Empty the Queue.
00205     * Since the ring buffer is static, it suffice to set all pointers to 0, 
00206     * Commands are left in memory.   */
00207     void qReset() { qBlock = true; readIdx=writeIdx=Count=0; qBlock = false; qCollide = false;};
00208 
00209     /** Check if Queue is full. 
00210     * 
00211     * Space is limited to DEPTH_Queue=256 BotCommand elements.
00212     * If in doubt it should be checked before trying to add new elements
00213     * @return <bool>  True if Queue is full*/
00214     bool qIsFull() {return Count>=DEPTH_Queue;}
00215     
00216     /** Check if Queue is empty. 
00217     *
00218     * Should be checked before trying to get new elements
00219     * @return <bool>  True if Queue is empty*/
00220     bool qIsEmpty() {return Count<=0;}
00221     
00222     /** 256 elements deep Command Queue, to put BotCommand in a queue.
00223     * Stores all BotCommand in this static 256 array used as a circular ring buffer. */
00224     BotCommand cmd[DEPTH_Queue]; /**< Actual static Queue array where all elements reside. */
00225     
00226     int writeIdx; /**< Index in Queue array where to put the next new element to. */
00227     int readIdx;  /**< Index in Queue array where to get the oldest element from. */
00228     int Count;    /**< Counts and keeps track of the number of elements in array used. */
00229     bool qBlock;  /**< Blocks read access while a write is ongoing. */
00230     bool qCollide;/**< Indicates a colliding access to the queue. */
00231     
00232 public:
00233     /** High level, immediate:  move bot and wait for movement end. 
00234     * Simple wrapper for iMove() and iWaitEnd().
00235     * Refer to those methods for further docs.
00236     */
00237     void iMoveAndWait(BotCmdVerb Acommand, float Aparam);
00238     
00239     /** High level, immediate:  move bot and wait for movement end. 
00240     * Simple wrapper for iMove() and iWaitEnd().
00241     * Refer to those methods for further docs.
00242     */
00243     void iMoveAndWait(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm);
00244 
00245     /** High level, immediate:  move bot according to command and parameter
00246     * Composes a BotCommand and passes it to executeCommand().
00247     * Use for commands that need only one parameter: FORWARD, BACKWARD, ROTATE
00248     * For details refer to docu of moveForward(), moveBackward(), moveRotate().
00249     * Preset the speed using setSpeed().
00250     * Warning: Collides with queued commands. 
00251     *
00252     * @param[in] <Acommand> Requested movement, of type BotCmdVerb.
00253     * @param[in] <Aparam>   Requested amount, defines angle for ROTATE, or distance for FORWARD, BACKWARD.
00254     */
00255     void iMove(BotCmdVerb Acommand, float Aparam);
00256     
00257     /** High level, immediate:  move bot according to command and parameters
00258     * Composes a BotCommand and passes it to executeCommand().
00259     * Use for commands that need two parameters: LEFT, RIGHT, BACKLEFT, BACKRIGHT
00260     * For details refer to docu of moveLeft(), moveRight(), moveBackLeft(), moveBackRight().
00261     * Preset the speed using setSpeed().
00262     * Warning: Collides with queued commands. 
00263     *
00264     * @param[in] <Acommand> Requested movement, of type BotCmdVerb.
00265     * @param[in] <Aturn_angle_deg>   Requested angle in degrees.
00266     * @param[in] <Adist_cm>   Requested distance in centimeters.
00267     */
00268     void iMove(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm);
00269     
00270     /** High level, immediate: move bot according to prefilled command structures.
00271     * Recommended to use iMove() methods to fill the command structure correctly. 
00272     * Branches to the moveXXXX() methods. For details see docs for those methods. 
00273     * Preset the speed using setSpeed().
00274     * Warning: Collides with queued commands if called individually. 
00275     *
00276     * @param[in] <*cmd> Pointer to type BotCommand, the prefilled command structure,
00277     */
00278     void iExeCommand(BotCommand *cmd);
00279 
00280     /** High Level: spends all time with waits, 
00281     * returns only once wheelState = LRWHEELS_STOP, 
00282     * not recommended due its blocking behavior */
00283     void iWaitEnd();
00284     
00285     /** High Level: spends all time with waits, 
00286     * returns only once wheelState = LRWHEELS_STOP,
00287     * but waits no more than max_wait_ms milli seconds. 
00288     * Not recommended due its blocking behavior  */
00289     void iWaitEnd(uint32_t max_wait_ms); // time-out
00290 
00291     /** High level, immediate: Both wheels get stopped, and turned off
00292     * updates the wheel state wheelsState, does not call the callback handler!
00293     * It does not empty the queue. 
00294     * Adding new elements into the queue after calling iStop() 
00295     * Would continue using all commands still in the queue.  */
00296     void iStop();
00297 
00298 public: 
00299     
00300     /** Mid level, immediate:  advance bot straight forward for a given distance,
00301     * Preset the speed using setSpeed().
00302     * Warning: Collides with queued commands. 
00303     */
00304     void moveForward(float dist_cm);
00305     
00306     /** Mid level, immediate:  reverse bot straight backwards for a given distance,
00307     * Preset the speed using setSpeed().
00308     * Warning: Collides with queued commands. 
00309     */
00310     void moveBackward(float dist_cm);
00311     
00312     /* Mid level, immediate: turn bot forward right, 
00313     * around a radius twice the bot size ,
00314     * Same as  moveRight(turn_angle_deg, 0);
00315     * Preset the speed using setSpeed().
00316     * Warning: Collides with queued commands. 
00317     */
00318     void moveRight(float turn_angle_deg);
00319     
00320     /** Mid level, immediate: turn bot forward right, 
00321     * around a radius that is center_cm away from the right wheel,
00322     * Preset the speed using setSpeed().
00323     * Warning: Collides with queued commands. 
00324     */
00325     void moveRight(float turn_angle_deg, float center_cm);
00326     
00327     /** Mid level, immediate: turn bot forward left, 
00328     * around a radius twice the bot size,
00329     * Same as  moveLeft(turn_angle_deg, 0);
00330     * Preset the speed using setSpeed().
00331     * Warning: Collides with queued commands. 
00332     */
00333     void moveLeft(float turn_angle_deg);
00334     
00335     /** Mid level, immediate: turn bot forward left, 
00336     * around a radius that is center_cm away from the left wheel,
00337     * Preset the speed using setSpeed().
00338     * Warning: Collides with queued commands. 
00339     */
00340     void moveLeft(float turn_angle_deg, float center_cm); 
00341     
00342     /* Mid level, immediate: turn bot backwards right, 
00343     * around a radius twice the bot size ,
00344     * Same as  moveBackRight(turn_angle_deg, 0);
00345     * Preset the speed using setSpeed().
00346     * Warning: Collides with queued commands. 
00347     */
00348     void moveBackRight(float turn_angle_deg);
00349     
00350     /** Mid level, immediate: turn bot backwards right, 
00351     * around a radius that is center_cm away from the right wheel,
00352     * Preset the speed using setSpeed().
00353     * Warning: Collides with queued commands. 
00354     */
00355     void moveBackRight(float turn_angle_deg, float center_cm);
00356     
00357     /** Mid level, immediate: turn bot backwards left, 
00358     * around a radius twice the bot size,
00359     * Same as  moveBackLeft(turn_angle_deg, 0);
00360     * Preset the speed using setSpeed().
00361     * Warning: Collides with queued commands. 
00362     */
00363     void moveBackLeft(float turn_angle_deg);
00364     
00365     /** Mid level, immediate: turn bot backwards left, 
00366     around a radius that is center_cm away from the left wheel,
00367     * Preset the speed using setSpeed().
00368     * Warning: Collides with queued commands. 
00369     */
00370     void moveBackLeft(float turn_angle_deg, float center_cm);
00371 
00372     /** Mid level, immediate: turn bot on its place for a given angle.
00373     * positive angles turn right, negative angles turn left,
00374     * Preset the speed using setSpeed().
00375     * Warning: Collides with queued commands. 
00376     */
00377     void moveRotate(float turn_angle_deg);
00378 
00379   private: 
00380 
00381     /** Low level, immediate: sets Both wheel speeds   */ 
00382     void wheelsSetSpeed(float mot_speed_cm_sec);
00383     
00384     /** Callback when left wheel stops; 
00385     * updates the wheel state wheelsState */
00386     void wheelLeftStoppedCB();
00387     
00388     /** Callback when right wheel stops. 
00389     * updates the wheel state wheelsState */
00390     void wheelRightStoppedCB();
00391     
00392     /** Callback when all wheels have stopped. 
00393     * updates the wheel state wheelsState, switch off all Phases */
00394     void wheelsAllStoppedCB();
00395 
00396     /** Callback function pointer: 
00397     * If set non-NULL it is called when All wheels Stopped()*/
00398     void (*extCallBack)(int status);
00399 
00400     /** Wheel status register:
00401     * Remembers which of the wheels still run */
00402     TWheelsState wheelsState; 
00403 
00404     /** geometric properties of the bot */
00405     float botDiameter;
00406     
00407     /** Ratio of wheel diameter and bot diameter */
00408     float ratio_wheel_bot;
00409     
00410     /** last requested bot speed */ 
00411     float botSpeed_cm_sec;  
00412 
00413     /** The two wheels, as pointers to their classes */
00414     CreaMot *wheelLeft;
00415     CreaMot *wheelRight;
00416 };
00417 
00418 
00419 #endif