File content as of revision 6:4d8938b686a6:

/**
 * @file CreaBot.h
 * @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
 
 * @author Tarek Lule, Francois Druilhe, et al.
 * @date 21. Nov. 2018.
 * @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 /**< Clamp maximum advancement speed = 30cm/sec, was MAX_SPEED */
#define DEFAULT_SPEED_CM_SEC 2.0f /**< Default advancement speed = 2.0cm/sec, was DEFAULT_SPEED */
#define PI 3.141592f
#define DEPTH_FIFO 256  /**< Initialize the depth of the command FIFO to 256 */

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


/** \enum BotStatus 
* \brief Possible states of two motors of Bot
*  */
typedef enum {
    AlLSTOP = 0,/**< All Motors have stopped */  
    LMOT_RUNS,  /**< Left Motor still runs */  
    RMOT_RUNS,  /**< Right Motor still runs */  
    MOTORS_RUN  /**< Both Motors still run */  
} BotStatus;

/** \struct BotCommand
* \brief Structure of a Motor Command.

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

/** \class CommandFIFO
 * \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: 
    /** Class Creator: initializes an empties FIFO
    *
    * Ring buffer is allocated statically. Read and Write Index are set to 0. */
    CommandFIFO();
    
    /** 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;};

    /** 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.
    *
    *@return <BotCommand*> Pointer to the reserved BotCommand to write to. 
    */
    BotCommand *put();

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

    /** 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.
    *
    *@return <BotCommand*> Pointer to the oldest BotCommand. 
    */
    BotCommand *get();
    
    /** Access FIFO used count. 
    *
    *@return <int>  the number of commands in the FIFO */
    int getCount() {return count;}
    
     /** Check if FIFO is full. 
    * 
    * Space is limited to DEPTH_FIFO=256 BotCommand elements.
    * Should be checked before trying to put new elements
    *
    * @return <bool>  True if FIFO is full*/
    bool isFull() {return count>=DEPTH_FIFO;}
    
     /** Check if FIFO is empty. 
    * 
    * Should be checked before trying to get new elements
    * 
    * @return <bool>  True if FIFO is empty*/
    bool isEmpty() {return count<=0;}
    
  private:
    int readIdx;  /**< Index in FIFO array where to get the oldest element from. */
    int writeIdx; /**< Index in FIFO array where to put the next new element to. */
    int Count;    /**< Counts the number of elements in array used. */
    BotCommand cmd[DEPTH_FIFO]; /**< Actual static FIFO array where all elements reside. */
    BotCommand cmd_idle; /**< A Constant Idle command  */
};

/** \class Creabot

* \brief Synchronous Control of 2 motors as part of a two wheel robot
 *
 * Example:
 * @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) {
 *     };
 * }
 * @endcode
 */
 
class Creabot {
public:
   /** Create a Creabot object with 2 motors
     *
     *  @param left Motor object declared before corresponding to left motor of the Creabot
     *  @param right Motor object declared before corresponding to right motor of the Creabot
     *  @param diameter_wheel_cm diameter in dist_cm of the wheels (both supposed to be at same diameter)
     *  @param distance_wheel_cm distance in dist_cm between center of left wheel and center of right wheel
     */
    Creabot(Motor *left, Motor *right, float diameter_wheel_cm, float distance_wheel_cm);
    
    /** Property access to Motor state */
    BotState getState() {return MotState};
    
    void setSpeed(float speed_cm_sec);
    void waitEndMove();
    void waitEndMove(uint32_t delay_us); // watchdog
    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 move(BotCmdVerb moveType, float angle_or_cm);
    void move(BotCmdVerb moveType, float angle_deg, float dist_cm);
    void fifo(BotCmdVerb moveType, float angle_or_cm);
    void fifo(BotCmdVerb moveType, float angle_deg, float dist_cm);
    void flushFifo();
    void spirale(float b, float turns);
    int moveInFifo();
    void executeFifo();
    void stopMove();
         
private:
    void moveForward(float dist_cm);
    void moveBackward(float dist_cm);
    void rotate(float angle_deg);
    void moveRight(float angle_deg);
    void moveRight(float angle_deg, float center_cm);
    void moveLeft(float angle_deg);
    void moveLeft(float angle_deg, float center_cm);
    uint32_t computeSpeed(Motor *motor, float speed_cm_sec);
    
    void (*extCallBack)(int status);
    void moveMotorLeft(motorDir dir, float angle_deg);
    void moveMotorRight(motorDir dir, float angle_deg);
    void executeCommand(BotCommand *cmd);
    void setSpeedMot(Motor *motor, float speed_cm_sec);

    
  private: 
    // The two motors and their physical properties:
    // Two call back functions that are called back by the motors when they have stopped
    void cbLeftMotStopped();
    void cbRightMotStopped();
    void AllMotorsStopped();
    Motor *motor_left;
    Motor *motor_right;
    float diameter_wheel;
    float distance_wheel;
    float perimeter_wheel;
    float perimeter_bot;
    float degree_wheel_per_cm;
    float degree_bot_per_cm;
    float ratio_wheel_bot;


    float last_speed;  // last requested speed
    CommandFIFO fifoBot;
    BotCommand current_cmd;
    Ticker botTicker;
    bool executingFifo;
    
    BotState MotState;
  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