Tarek Lule / CreaBotLib

My Version of CreaBotLib

Fork of CreaBotLib by CreaLab

CreaBot.h@0:a7fb03c9ea9d, 2017-07-26 (annotated)

Committer:
garphil
Date:
Wed Jul 26 15:37:58 2017 +0000
Revision:
0:a7fb03c9ea9d
Child:
1:974d020bb582
First Version, missing ARC and some exhaustive test...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garphil 0:a7fb03c9ea9d 1
garphil 0:a7fb03c9ea9d 2 #ifndef CREABOT_H
garphil 0:a7fb03c9ea9d 3 #define CREABOT_H
garphil 0:a7fb03c9ea9d 4
garphil 0:a7fb03c9ea9d 5 #include "mbed.h"
garphil 0:a7fb03c9ea9d 6 #include "motor.h"
garphil 0:a7fb03c9ea9d 7
garphil 0:a7fb03c9ea9d 8 #define MAX_SPEED 8 //cm per second
garphil 0:a7fb03c9ea9d 9 #define DEFAULT_SPEED 0.5 //cm per second
garphil 0:a7fb03c9ea9d 10 #define PI 3.141592f
garphil 0:a7fb03c9ea9d 11 #define DEPTH_FIFO 6
garphil 0:a7fb03c9ea9d 12
garphil 0:a7fb03c9ea9d 13 typedef enum {FORWARD, BACKWARD, ROTATE, LEFT, RIGHT, IDLE} cmdbot_t;
garphil 0:a7fb03c9ea9d 14 class CommandBot {
garphil 0:a7fb03c9ea9d 15 public:
garphil 0:a7fb03c9ea9d 16 cmdbot_t command;
garphil 0:a7fb03c9ea9d 17 float angle;
garphil 0:a7fb03c9ea9d 18 float cm;
garphil 0:a7fb03c9ea9d 19
garphil 0:a7fb03c9ea9d 20 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;}};
garphil 0:a7fb03c9ea9d 21 };
garphil 0:a7fb03c9ea9d 22
garphil 0:a7fb03c9ea9d 23 class MyFifoCmd {
garphil 0:a7fb03c9ea9d 24
garphil 0:a7fb03c9ea9d 25 public:
garphil 0:a7fb03c9ea9d 26 MyFifoCmd() {start = end = size = 0;cmd_idle.command = IDLE;cmd_idle.angle=0;cmd_idle.cm=0;};
garphil 0:a7fb03c9ea9d 27 void empty() {start=end=size=0;};
garphil 0:a7fb03c9ea9d 28 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;};
garphil 0:a7fb03c9ea9d 29 CommandBot *get() {if(size>0) {int old_start = start; start++;if(start==DEPTH_FIFO)start=0; size--; return &cmd[old_start];} else return NULL;}
garphil 0:a7fb03c9ea9d 30 int getSize() {return size;}
garphil 0:a7fb03c9ea9d 31 private:
garphil 0:a7fb03c9ea9d 32 int start, end, size;
garphil 0:a7fb03c9ea9d 33 CommandBot cmd[DEPTH_FIFO];
garphil 0:a7fb03c9ea9d 34 CommandBot cmd_idle;
garphil 0:a7fb03c9ea9d 35 };
garphil 0:a7fb03c9ea9d 36
garphil 0:a7fb03c9ea9d 37
garphil 0:a7fb03c9ea9d 38 /** Asynchronous Control of 2 motors part of a robot
garphil 0:a7fb03c9ea9d 39 *
garphil 0:a7fb03c9ea9d 40 * Example:
garphil 0:a7fb03c9ea9d 41 * @code
garphil 0:a7fb03c9ea9d 42 * // --- Define the Four PINs & Time of movement used for Motor drive -----
garphil 0:a7fb03c9ea9d 43 * 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)
garphil 0:a7fb03c9ea9d 44 * Motor motorRight(PA_5,PA_4,PA_3,PA_1);
garphil 0:a7fb03c9ea9d 45 * Creabot mybot(&motorLeft, &motorRight, 10.0f,13.0f); // insert the motors and indicate wheel diameter and distance between wheels
garphil 0:a7fb03c9ea9d 46 *
garphil 0:a7fb03c9ea9d 47 * int main() {
garphil 0:a7fb03c9ea9d 48 *
garphil 0:a7fb03c9ea9d 49 * mybot.setSpeed(12.5); // 12.5cm/s
garphil 0:a7fb03c9ea9d 50 * mybot.move(FORWARD,10); // Go forward of 10cm
garphil 0:a7fb03c9ea9d 51 * mybot.waitEndMove(); // Wait end of Move
garphil 0:a7fb03c9ea9d 52 * mybot.move(ROTATE,90); // Start rotation of 90° around the center between wheels (two wheels running in same direction at same speed)
garphil 0:a7fb03c9ea9d 53 * mybot.move(BACKWARD,40); // Stop immediately the rotation and go backward of 40cm
garphil 0:a7fb03c9ea9d 54 * mybot.waitEndMove(); // Wait end of Backward
garphil 0:a7fb03c9ea9d 55 * mybot.moveAndWait(LEFT,60); // Move Left of 60° in circle, center being the left wheel (off). Wait end of move
garphil 0:a7fb03c9ea9d 56 * mybot.waitEndMove(); // Not needed, as already waited...
garphil 0:a7fb03c9ea9d 57 * 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.
garphil 0:a7fb03c9ea9d 58 * mybot.moveAndWait(ROTATE,90);
garphil 0:a7fb03c9ea9d 59 * mybot.move(ROTATE,-90); // Opposite direction.
garphil 0:a7fb03c9ea9d 60 * mybot.waitEndMove(60); // with watchdog => if after 60s the move is not ended, continue the execution
garphil 0:a7fb03c9ea9d 61 * mybot.stopMove(); // Stop the movement before end...
garphil 0:a7fb03c9ea9d 62 *
garphil 0:a7fb03c9ea9d 63 * // 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.
garphil 0:a7fb03c9ea9d 64 * mybot.fifo(FORWARD,10); // Already starting...
garphil 0:a7fb03c9ea9d 65 * mybot.fifo(BACKWARD,10); // will wait end of previous command to go
garphil 0:a7fb03c9ea9d 66 * mybot.fifo(ROTATE,120.0);
garphil 0:a7fb03c9ea9d 67 * mybot.fifo(LEFT, 30, 120);
garphil 0:a7fb03c9ea9d 68 * mybot.fifo(RIGHT, 25);
garphil 0:a7fb03c9ea9d 69 * mybot.waitEndMove(100000); // wait until fifo end... can flush anytime with stopMove...
garphil 0:a7fb03c9ea9d 70 * mybot.stopMove(); // before end... Flush the fifo and remove all instructions…
garphil 0:a7fb03c9ea9d 71 *
garphil 0:a7fb03c9ea9d 72 * while(1) {
garphil 0:a7fb03c9ea9d 73 * };
garphil 0:a7fb03c9ea9d 74 * }
garphil 0:a7fb03c9ea9d 75 * @endcode
garphil 0:a7fb03c9ea9d 76 */
garphil 0:a7fb03c9ea9d 77 class Creabot {
garphil 0:a7fb03c9ea9d 78 public:
garphil 0:a7fb03c9ea9d 79 /** Create a Creabot object to synchronize 2 motors
garphil 0:a7fb03c9ea9d 80 *
garphil 0:a7fb03c9ea9d 81 * @param left Motor object declared before corresponding to left motor of the Creabot
garphil 0:a7fb03c9ea9d 82 * @param right Motor object declared before corresponding to right motor of the Creabot
garphil 0:a7fb03c9ea9d 83 * @param diameter_wheel_cm diameter in cm of the wheels (both supposed to be at same diameter)
garphil 0:a7fb03c9ea9d 84 * @param distance_wheel_cm distance in cm between center of left wheel and center of right wheel
garphil 0:a7fb03c9ea9d 85 */
garphil 0:a7fb03c9ea9d 86 Creabot(Motor *left, Motor *right, float diameter_wheel_cm, float distance_wheel_cm);
garphil 0:a7fb03c9ea9d 87 int getStatus();
garphil 0:a7fb03c9ea9d 88 void setSpeed(float cm_per_second);
garphil 0:a7fb03c9ea9d 89 void waitEndMove();
garphil 0:a7fb03c9ea9d 90 void waitEndMove(uint32_t delay_us); // watchdog
garphil 0:a7fb03c9ea9d 91 void setCallBack(void (*callback)(int status));
garphil 0:a7fb03c9ea9d 92 void moveAndWait(cmdbot_t moveType, float angle_or_cm);
garphil 0:a7fb03c9ea9d 93 void moveAndWait(cmdbot_t moveType, float angle, float cm);
garphil 0:a7fb03c9ea9d 94 void move(cmdbot_t moveType, float angle_or_cm);
garphil 0:a7fb03c9ea9d 95 void move(cmdbot_t moveType, float angle, float cm);
garphil 0:a7fb03c9ea9d 96 void fifo(cmdbot_t moveType, float angle_or_cm);
garphil 0:a7fb03c9ea9d 97 void fifo(cmdbot_t moveType, float angle, float cm);
garphil 0:a7fb03c9ea9d 98 void flushFifo();
garphil 0:a7fb03c9ea9d 99 int moveInFifo();
garphil 0:a7fb03c9ea9d 100 void executeFifo();
garphil 0:a7fb03c9ea9d 101 void stopMove();
garphil 0:a7fb03c9ea9d 102
garphil 0:a7fb03c9ea9d 103 private:
garphil 0:a7fb03c9ea9d 104 void moveForward(float cm);
garphil 0:a7fb03c9ea9d 105 void moveBackward(float cm);
garphil 0:a7fb03c9ea9d 106 void rotate(float angle);
garphil 0:a7fb03c9ea9d 107 void moveRight(float angle);
garphil 0:a7fb03c9ea9d 108 void moveRight(float angle, float center_cm);
garphil 0:a7fb03c9ea9d 109 void moveLeft(float angle);
garphil 0:a7fb03c9ea9d 110 void moveLeft(float angle, float center_cm);
garphil 0:a7fb03c9ea9d 111 void callBackLeft();
garphil 0:a7fb03c9ea9d 112 void callBackRight();
garphil 0:a7fb03c9ea9d 113 void callBack();
garphil 0:a7fb03c9ea9d 114 uint32_t computeSpeed(Motor *motor, float cm_per_second);
garphil 0:a7fb03c9ea9d 115 void setSpeedLeft(float cm_per_second);
garphil 0:a7fb03c9ea9d 116 void setSpeedRight(float cm_per_second);
garphil 0:a7fb03c9ea9d 117
garphil 0:a7fb03c9ea9d 118 void (*extCallBack)(int status);
garphil 0:a7fb03c9ea9d 119 void moveMotorLeft(MotorDir dir, float angle);
garphil 0:a7fb03c9ea9d 120 void moveMotorRight(MotorDir dir, float angle);
garphil 0:a7fb03c9ea9d 121 void executeCommand(CommandBot *cmd);
garphil 0:a7fb03c9ea9d 122
garphil 0:a7fb03c9ea9d 123
garphil 0:a7fb03c9ea9d 124 Motor *motor_left;
garphil 0:a7fb03c9ea9d 125 Motor *motor_right;
garphil 0:a7fb03c9ea9d 126 float diameter_wheel;
garphil 0:a7fb03c9ea9d 127 float distance_wheel;
garphil 0:a7fb03c9ea9d 128 float perimeter_wheel;
garphil 0:a7fb03c9ea9d 129 float perimeter_bot;
garphil 0:a7fb03c9ea9d 130 float degree_wheel_per_cm;
garphil 0:a7fb03c9ea9d 131 float degree_bot_per_cm;
garphil 0:a7fb03c9ea9d 132 float ratio_wheel_bot;
garphil 0:a7fb03c9ea9d 133 float current_speed;
garphil 0:a7fb03c9ea9d 134 int status;
garphil 0:a7fb03c9ea9d 135 bool callLeft;
garphil 0:a7fb03c9ea9d 136 bool callRight;
garphil 0:a7fb03c9ea9d 137 bool endMove;
garphil 0:a7fb03c9ea9d 138 MyFifoCmd fifoBot;
garphil 0:a7fb03c9ea9d 139 CommandBot current_cmd;
garphil 0:a7fb03c9ea9d 140 Ticker botTicker;
garphil 0:a7fb03c9ea9d 141 bool executingFifo;
garphil 0:a7fb03c9ea9d 142 };
garphil 0:a7fb03c9ea9d 143
garphil 0:a7fb03c9ea9d 144 #endif