File content as of revision 12:530772639065:

/*
 * \file CreaBot.h
 * \brief File contains Creabot Library.
 
 * CreaBot.h contains the Creabot class, and required enums and structs.
 * Imports "mbed.h" and "CreaMot.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 "CreaMot.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 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[in] <left>           CreaMot object, corresponding to left wheel of the Creabot
     *  @param[in] <right>          CreaMot object, corresponding to right wheel of the Creabot
     *  @param[in] <wheel_diam_cm>  Diameter in cm of the wheels (both must be the same diameter)
     *  @param[in] <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.  
     *
     *  @param[in] <AbotSpeed_cm_sec>     requested movement speed  */
    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().
    * 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().
    *
    * 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() 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() 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().
    * 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().
    * 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