File content as of revision 7:3a793ddc3490:

/*
 * at: file CreaBot.h
 * bs_brief File contains Creabot Library.
 
 * CreaBot.h contains the class Motor, and related enums and structs.
 * Includes "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
 
 * at_author Tarek Lule, Francois Druilhe, et al.
 * at_date 21. Nov. 2018.
 * at_see https://os.mbed.com/users/sepp_nepp/code/CreaBotLib/  */
 
 // -------------------- Motor ---------------------------
 
#ifndef CREABOT_H
#define CREABOT_H

#include "mbed.h"
#include "motor.h"

#define MAX_SPEED_CM_SEC 30.0f /*star< Clamp maximum advancement speed = 30cm/sec, was MAX_SPEED */
#define MIN_SPEED_CM_SEC 0.001f /*star< Clamp minimum advancement speed = 10um/sec */
#define DEFAULT_SPEED_CM_SEC 2.0f /*star< Default advancement speed = 2.0cm/sec, was DEFAULT_SPEED */
#define PI 3.141592f
#define DEPTH_FIFO 256  /*star< Initialize the depth of the command FIFO to 256 */

/*star \enum BotCmdVerb 
* bs_brief Robot Commands Verbs, gives the movement direction
*  */
typedef enum {
    IDLE = 0,   /*star< Command to do nothing */  
    FORWARD,    /*star< Advance the robot straight forward */  
    BACKWARD,   /*star< Advance the robot straight backwards. */  
    ROTATE,     /*star< Rotate around its own axis */  
    LEFT,       /*star< Advance in a left curve */
    RIGHT,      /*star< Advance in a right curve */
    REVLEFT,    /*star< Reverse in a left curve */
    REVRIGHT    /*star< Reverse in a right curve */
} BotCmdVerb;


/*star \enum TMotorsState
* bs_brief Possible states of the two motors of the Bot
*  */
typedef enum {
    LRMOTS_STOP = 0,/*star< All Motors have stopped */  
    LMOT_RUNS,  /*star< Left  Motor  runs */  
    RMOT_RUNS,  /*star< Right Motor  runs */  
    LRMOTS_RUN  /*star< Both  Motors run */  
} TMotorsState;

/*star \struct BotCommand
* bs_brief Structure of a Motor Command.
* The command structure is put into command FIFO, and treated by the FIFO-Handler */
typedef struct {
    BotCmdVerb command; /*star< General Command to give movement direction.*/
    float dist_cm;      /*star< Distance in dist_cm for translational movements .*/
    float angle_deg;    /*star< Angle in degree for rotational movement .*/
    void set(BotCmdVerb Acommand, float Aangle_deg, float Adist_cm); /*star< Helper; set structure fields to values  */
    void set(BotCmdVerb Acommand, float Aparam); /*star< Helper; set structure fields to values  */
} BotCommand;

/*star \struct geometries
* bs_brief Some geometric encapsulation
* Holds some geometric values */
typedef struct {
    float diam_cm;
    float perim_cm;
    float degree_per_cm;
    void setDiam( float Adiam_cm); /*star< Helper; set diameter and perim to values  */
} circle_geos;

/*star \class CommandFIFO
 * bs_brief 256 elements deep Command FIFO, to puts BotCommand in a queue.
 * 
 *  Internally stores all BotCommand in a static 256 array used as a circular ring buffer.
 *  Adds incoming commands at the head of the ring buffer at position writeIdx, 
 *  Gets oldest commands from the tail of the ring buffer at position readIdx,
 *  Keeps track of the occupied size in Count.
 *  BotCommands are passed back as pointers.
 */
class CommandFIFO {
  public: 
    /*star Class Creator: initializes an empties FIFO
    *
    * Ring buffer is allocated statically. Read and Write Index are set to 0. */
    CommandFIFO();
    
    /*star Empty the FIFO.
    *
    * Since ring buffer is static, it suffice to set all pointers to 0, commands are left in memory. */
    void empty() {readIdx=writeIdx=Count=0;};

    /*star Reserve a new element at the head of the FIFO
    *
    *If FIFO is full, it passes back NULL 
    *Otherwise Advances the write index once and returns a pointer the next free command struct.
    *The caller then has to fills the command structure at that position. 
    *Do not free memory associated to the pointer.
    *
    *at_return <BotCommand*> Pointer to the reserved BotCommand to write to. 
    */
    BotCommand *put();

    void put(BotCmdVerb Acommand, float Adist_cm, float Aangle_deg); 

    /*star Get and remove the oldest element at the tail of the FIFO
    *
    *If FIFO is empty, it passes back NULL 
    *Otherwise advances the read index once and returns a pointer the oldest stored command in the FIFO.
    *Do not free memory associated with the pointer.
    *
    *at_return <BotCommand*> Pointer to the oldest BotCommand. 
    */
    BotCommand *get();
    
    /*star Access FIFO used Count. 
    *
    *at_return <int>  the number of commands in the FIFO */
    int getCount() {return Count;}
    
     /*star Check if FIFO is full. 
    * 
    * Space is limited to DEPTH_FIFO=256 BotCommand elements.
    * Should be checked before trying to put new elements
    *
    * at_return <bool>  True if FIFO is full*/
    bool isFull() {return Count>=DEPTH_FIFO;}
    
     /*star Check if FIFO is empty. 
    * 
    * Should be checked before trying to get new elements
    * 
    * at_return <bool>  True if FIFO is empty*/
    bool isEmpty() {return Count<=0;}
    
  private:
    int readIdx;  /*star< Index in FIFO array where to get the oldest element from. */
    int writeIdx; /*star< Index in FIFO array where to put the next new element to. */
    int Count;    /*star< Counts the number of elements in array used. */
    BotCommand cmd[DEPTH_FIFO]; /*star< Actual static FIFO array where all elements reside. */
    BotCommand cmd_idle; /*star< A Constant Idle command  */
};

/*star \class Creabot

* bs_brief Synchronous Control of 2 motors as part of a two wheel robot
 *
 * Example:
 * at_code
 * // --- Define the Four PINs & Time of movement used for Motor drive -----
 * Motor motorLeft(PA_12, PB_0, PB_1, PB_6); // Declare first the 2 motors (to avoid to have an object with 8 pins to create)
 * Motor motorRight(PA_5,PA_4,PA_3,PA_1);
 * Creabot mybot(&motorLeft, &motorRight, 10.0f,13.0f); // insert the motors and indicate wheel diameter and distance between wheels
 * 
 * int main() {
 * 
 *     mybot.setSpeed(12.5); // 12.5cm/s
 *     mybot.move(FORWARD,10); // Go forward of 10cm
 *     mybot.waitEndMove(); // Wait end of Move
 *     mybot.move(ROTATE,90); // Start rotation of 90° around the center between wheels (two wheels running in same direction at same speed)
 *     mybot.move(BACKWARD,40); // Stop immediately the rotation and go backward of 40cm
 *     mybot.waitEndMove(); // Wait end of Backward
 *     mybot.moveAndWait(LEFT,60); // Move Left of 60° in circle, center being the left wheel (off). Wait end of move
 *     mybot.waitEndMove();  // Not needed, as already waited... 
 *     mybot.moveAndWait(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.moveAndWait(ROTATE,90);
 *     mybot.move(ROTATE,-90); // Opposite direction.
 *     mybot.waitEndMove(60); // with watchdog => if after 60s the move is not ended, continue the execution
 *     mybot.stopMove(); // Stop the movement before end...
 *     
 *     // Same with a fifo of command, opposite to the move, receiving a new command will not stop the current execution, but the bot will store it.
 *     mybot.fifo(FORWARD,10); // Already starting...
 *     mybot.fifo(BACKWARD,10); // will wait end of previous command to go
 *     mybot.fifo(ROTATE,120.0);
 *     mybot.fifo(LEFT, 30, 120);
 *     mybot.fifo(RIGHT, 25);
 *     mybot.waitEndMove(100000); // wait until fifo end... can flush anytime with stopMove...
 *     mybot.stopMove(); // before end... Flush the fifo and remove all instructions…
 * 
 *     while(1) {
 *     };
 * }
 * at_endcode
 */
 
class Creabot {
public:
   /*star Create a Creabot object with 2 motors
     *
     *  at_param left Motor object declared before corresponding to left motor of the Creabot
     *  at_param right Motor object declared before corresponding to right motor of the Creabot
     *  at_param wheel_diam_cm diameter in cm of the wheels (both muste be the same diameter)
     *  at_param bot_diam_cm distance cm between center of left wheel and center of right wheel
     */
    Creabot(Motor *left, Motor *right, float wheel_diam_cm, float bot_diam_cm);
     /*star Property access to Motor state */
    TMotorsState getState() {return MotState; };
   
public:
    void qMove(BotCmdVerb moveType, float angle_or_cm);
    void qMove(BotCmdVerb moveType, float angle_deg, float dist_cm);
    
    void setCallBack(void (*Acallback)(int status));
    void moveAndWait(BotCmdVerb moveType, float angle_or_cm);
    void moveAndWait(BotCmdVerb moveType, float angle_deg, float dist_cm);
    void flushFifo();
    void spirale(float b, float turns);
    int moveInFifo();
    void executeFifo();
    void stopMove();
         
private:
    uint32_t computeSpeed(Motor *motor, float speed_cm_sec);
    
private: 
    CommandFIFO fifoBot;
    Ticker botTicker;
    bool executingFifo;

public:
    void move(BotCmdVerb moveType, float angle_or_cm);
    void move(BotCmdVerb moveType, float angle_deg, float dist_cm);
    void executeCommand(BotCommand *cmd);

public: 
    /* Mid level access functions: set bot-speed parameter for next motor command*/ 
    void setBotSpeed(float Abot_speed_cm_sec);
    
    /* Mid level control function:  move bot forward for a given distance */
    void moveForward(float dist_cm);
    /* Mid level control function:  move bot backwards for a given distance */
    void moveBackward(float dist_cm);
    /* Mid level control function: turn bot forward right, 
    around a radius twice the bot size */
    void moveRight(float angle_deg);
    /* Mid level control function: turn bot forward right, 
    around a radius that is center_cm away from the right wheel*/
    void moveRight(float angle_deg, float center_cm);
    /* Mid level control function: turn bot forward left, 
    around a radius twice the bot size */
    void moveLeft(float angle_deg);
    /* Mid level control function: turn bot forward left, 
    around a radius that is center_cm away from the left wheel*/
    void moveLeft(float angle_deg, float center_cm);
    /* Mid level control function: turn bot on its place for a given angle.
    positive angles turn right, negative angles turn left*/
    void rotate(float angle_deg);

    /* Low Level: spends all time with waits, 
    returns only once MotorState = LRMOTS_STOP, 
    not recommended due its exclusive blocking behavior */
    void waitEndMotors();
    /* Low Level: spends all time with waits, 
    returns only once MotorState = LRMOTS_STOP,
    but waits no more than max_wait_ms milli seconds, 
    not recommended due its exclusive blocking behavior  */
    void waitEndMotors(uint32_t max_wait_ms); // watchdog

  private: 
    /* geometric properties of each wheel and the bot */
    circle_geos wheel,bot;
    float ratio_wheel_bot;
    float bot_speed_cm_sec;  // the requested bot speed

    /* Low level access functions: sets both motor speeds immediately  */ 
    void setMotorsSpeed(float mot_speed_cm_sec)
    /* Low level motor command wrappers, speed, move and stop, state management */
    void setMotorSpeed(Motor *motor, float speed_cm_sec);
    /*star Commands the Left Motor to Move for a given angle in a given direction 
    * updates the Motor state MotState */
    void moveMotorLeft(motorDir dir, float angle_deg);
    /*star Commands the Right Motor to Move for a given angle in a given direction 
    * updates the Motor state MotState */
    void moveMotorRight(motorDir dir, float angle_deg);

    /*star Callback when left Motor stops. 
    * updates the Motor state MotState */
    void cbLeftMotStopped();
    /*star Callback when right Motor stops. 
    * updates the Motor state MotState */
    void cbRightMotStopped();
    /*star Callback when all Motors have stopped. 
    * updates the Motor state MotState, switch off all Phases */
    void AllMotorsStopped();

    /*star Callback function pointer: if set it is called when All Motors Stopped()*/
    void (*extCallBack)(int status);

    /*star Motor status register, remember which of the motors still run */
    TMotorsState MotState; 

    // The two motors, as pointers to their classes 
    Motor *motor_left;
    Motor *motor_right;

  private:
    /* members that were not used in last release
     * float macro_move_parameter;
     * BotCommand macro_move;
     * void setSpeedLeft(float speed_cm_sec);
     * void setSpeedRight(float speed_cm_sec); */
};

#endif