CreaBot.h

00001 
00002 #ifndef CREABOT_H
00003 #define CREABOT_H
00004 
00005 #include "mbed.h"
00006 #include "motor.h"
00007 
00008 #define MAX_SPEED 30 //cm per second
00009 #define DEFAULT_SPEED 2 //cm per second
00010 #define PI 3.141592f
00011 #define DEPTH_FIFO 256
00012 
00013 typedef enum {FORWARD, BACKWARD, ROTATE, LEFT, RIGHT, IDLE} cmdbot_t;
00014 class CommandBot {
00015     public:
00016     cmdbot_t command;
00017     float angle;
00018     float cm;
00019     
00020     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;}};
00021 };
00022 
00023 class MyFifoCmd {
00024     
00025     public: 
00026     MyFifoCmd() {start = end = size = 0;cmd_idle.command = IDLE;cmd_idle.angle=0;cmd_idle.cm=0;};
00027     void empty() {start=end=size=0;};
00028     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;};
00029     CommandBot *get() {if(size>0) {int old_start = start; start++;if(start==DEPTH_FIFO)start=0; size--; return &cmd[old_start];} else return NULL;}
00030     int getSize() {return size;}
00031     private:
00032     int start, end, size;
00033     CommandBot cmd[DEPTH_FIFO];
00034     CommandBot cmd_idle;
00035 };
00036 
00037 
00038 /** Asynchronous Control of 2 motors part of a robot
00039  *
00040  * Example:
00041  * @code
00042  * // --- Define the Four PINs & Time of movement used for Motor drive -----
00043  * 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)
00044  * Motor motorRight(PA_5,PA_4,PA_3,PA_1);
00045  * Creabot mybot(&motorLeft, &motorRight, 10.0f,13.0f); // insert the motors and indicate wheel diameter and distance between wheels
00046  * 
00047  * int main() {
00048  * 
00049  *     mybot.setSpeed(12.5); // 12.5cm/s
00050  *     mybot.move(FORWARD,10); // Go forward of 10cm
00051  *     mybot.waitEndMove(); // Wait end of Move
00052  *     mybot.move(ROTATE,90); // Start rotation of 90° around the center between wheels (two wheels running in same direction at same speed)
00053  *     mybot.move(BACKWARD,40); // Stop immediately the rotation and go backward of 40cm
00054  *     mybot.waitEndMove(); // Wait end of Backward
00055  *     mybot.moveAndWait(LEFT,60); // Move Left of 60° in circle, center being the left wheel (off). Wait end of move
00056  *     mybot.waitEndMove();  // Not needed, as already waited... 
00057  *     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. 
00058  *     mybot.moveAndWait(ROTATE,90);
00059  *     mybot.move(ROTATE,-90); // Opposite direction.
00060  *     mybot.waitEndMove(60); // with watchdog => if after 60s the move is not ended, continue the execution
00061  *     mybot.stopMove(); // Stop the movement before end...
00062  *     
00063  *     // 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.
00064  *     mybot.fifo(FORWARD,10); // Already starting...
00065  *     mybot.fifo(BACKWARD,10); // will wait end of previous command to go
00066  *     mybot.fifo(ROTATE,120.0);
00067  *     mybot.fifo(LEFT, 30, 120);
00068  *     mybot.fifo(RIGHT, 25);
00069  *     mybot.waitEndMove(100000); // wait until fifo end... can flush anytime with stopMove...
00070  *     mybot.stopMove(); // before end... Flush the fifo and remove all instructions…
00071  * 
00072  *     while(1) {
00073  *     };
00074  * }
00075  * @endcode
00076  */
00077 class Creabot {
00078 public:
00079    /** Create a Creabot object to synchronize 2 motors
00080      *
00081      * @param left Motor object declared before corresponding to left motor of the Creabot
00082      *  @param right Motor object declared before corresponding to right motor of the Creabot
00083      *  @param diameter_wheel_cm diameter in cm of the wheels (both supposed to be at same diameter)
00084      *  @param distance_wheel_cm distance in cm between center of left wheel and center of right wheel
00085      */
00086     Creabot(Motor *left, Motor *right, float diameter_wheel_cm, float distance_wheel_cm);
00087     int getStatus();
00088     void setSpeed(float cm_per_second);
00089     void waitEndMove();
00090     void waitEndMove(uint32_t delay_us); // watchdog
00091     void setCallBack(void (*callback)(int status));
00092     void moveAndWait(cmdbot_t moveType, float angle_or_cm);
00093     void moveAndWait(cmdbot_t moveType, float angle, float cm);
00094     void move(cmdbot_t moveType, float angle_or_cm);
00095     void move(cmdbot_t moveType, float angle, float cm);
00096     void fifo(cmdbot_t moveType, float angle_or_cm);
00097     void fifo(cmdbot_t moveType, float angle, float cm);
00098     void flushFifo();
00099     int moveInFifo();
00100     void executeFifo();
00101    void stopMove();
00102          
00103 private:
00104     void moveForward(float cm);
00105     void moveBackward(float cm);
00106     void rotate(float angle);
00107     void moveRight(float angle);
00108     void moveRight(float angle, float center_cm);
00109     void moveLeft(float angle);
00110     void moveLeft(float angle, float center_cm);
00111      void callBackLeft();
00112     void callBackRight();
00113     void callBack();
00114     uint32_t computeSpeed(Motor *motor, float cm_per_second);
00115     void setSpeedLeft(float cm_per_second);
00116     void setSpeedRight(float cm_per_second);
00117     
00118     void (*extCallBack)(int status);
00119     void moveMotorLeft(MotorDir dir, float angle);
00120     void moveMotorRight(MotorDir dir, float angle);
00121     void executeCommand(CommandBot *cmd);
00122 
00123         
00124     Motor *motor_left;
00125     Motor *motor_right;
00126     float diameter_wheel;
00127     float distance_wheel;
00128     float perimeter_wheel;
00129     float perimeter_bot;
00130     float degree_wheel_per_cm;
00131     float degree_bot_per_cm;
00132     float ratio_wheel_bot;
00133     float current_speed;
00134     int status;
00135     bool callLeft;
00136     bool callRight;
00137     bool endMove;
00138     MyFifoCmd fifoBot;
00139     CommandBot current_cmd;
00140     Ticker botTicker;
00141     bool executingFifo;
00142 };
00143 
00144 #endif