Diff: CreabotLib/CreaBot.h

Revision:

0:0e577ce96b2f

diff -r 000000000000 -r 0e577ce96b2f CreabotLib/CreaBot.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CreabotLib/CreaBot.h	Sat Feb 08 09:48:46 2020 +0000
@@ -0,0 +1,144 @@
+
+#ifndef CREABOT_H
+#define CREABOT_H
+
+#include "mbed.h"
+#include "motor.h"
+
+#define MAX_SPEED 30 //cm per second
+#define DEFAULT_SPEED 2 //cm per second
+#define PI 3.141592f
+#define DEPTH_FIFO 256
+
+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
\ No newline at end of file