File content as of revision 0:a7fb03c9ea9d:

#ifndef CREABOT_H
#define CREABOT_H

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

#define MAX_SPEED 8 //cm per second
#define DEFAULT_SPEED 0.5 //cm per second
#define PI 3.141592f
#define DEPTH_FIFO 6

typedef enum {FORWARD, BACKWARD, ROTATE, LEFT, RIGHT, IDLE} cmdbot_t;
class CommandBot {
    public:
    cmdbot_t command;
    float angle;
    float cm;
    
    void add_command(cmdbot_t cmd, float p1, float p2) {command = cmd; switch (command) {case FORWARD:;case BACKWARD: cm=p1; angle=0; break; case ROTATE: angle = p1; break; case LEFT: case RIGHT: angle = p1; cm=p2;break; case IDLE:break; default: break;}};
};

class MyFifoCmd {
    
    public: 
    MyFifoCmd() {start = end = size = 0;cmd_idle.command = IDLE;cmd_idle.angle=0;cmd_idle.cm=0;};
    void empty() {start=end=size=0;};
    CommandBot *put() {if(size<DEPTH_FIFO) {int old_end = end;end++;if(end==DEPTH_FIFO)end=0;size++;return &cmd[old_end];} else return NULL;};
    CommandBot *get() {if(size>0) {int old_start = start; start++;if(start==DEPTH_FIFO)start=0; size--; return &cmd[old_start];} else return NULL;}
    int getSize() {return size;}
    private:
    int start, end, size;
    CommandBot cmd[DEPTH_FIFO];
    CommandBot cmd_idle;
};


/** Asynchronous Control of 2 motors part of a 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 to synchronize 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 cm of the wheels (both supposed to be at same diameter)
     *  @param distance_wheel_cm distance in cm between center of left wheel and center of right wheel
     */
    Creabot(Motor *left, Motor *right, float diameter_wheel_cm, float distance_wheel_cm);
    int getStatus();
    void setSpeed(float cm_per_second);
    void waitEndMove();
    void waitEndMove(uint32_t delay_us); // watchdog
    void setCallBack(void (*callback)(int status));
    void moveAndWait(cmdbot_t moveType, float angle_or_cm);
    void moveAndWait(cmdbot_t moveType, float angle, float cm);
    void move(cmdbot_t moveType, float angle_or_cm);
    void move(cmdbot_t moveType, float angle, float cm);
    void fifo(cmdbot_t moveType, float angle_or_cm);
    void fifo(cmdbot_t moveType, float angle, float cm);
    void flushFifo();
    int moveInFifo();
    void executeFifo();
   void stopMove();
         
private:
    void moveForward(float cm);
    void moveBackward(float cm);
    void rotate(float angle);
    void moveRight(float angle);
    void moveRight(float angle, float center_cm);
    void moveLeft(float angle);
    void moveLeft(float angle, float center_cm);
     void callBackLeft();
    void callBackRight();
    void callBack();
    uint32_t computeSpeed(Motor *motor, float cm_per_second);
    void setSpeedLeft(float cm_per_second);
    void setSpeedRight(float cm_per_second);
    
    void (*extCallBack)(int status);
    void moveMotorLeft(MotorDir dir, float angle);
    void moveMotorRight(MotorDir dir, float angle);
    void executeCommand(CommandBot *cmd);

        
    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 current_speed;
    int status;
    bool callLeft;
    bool callRight;
    bool endMove;
    MyFifoCmd fifoBot;
    CommandBot current_cmd;
    Ticker botTicker;
    bool executingFifo;
};

#endif