Tarek Lule / CreaBotLib

My Version of CreaBotLib

Fork of CreaBotLib by CreaLab

CreaBot.h@5:efe80c5db389, 2018-10-31 (annotated)

Committer:
sepp_nepp
Date:
Wed Oct 31 14:36:07 2018 +0000
Revision:
5:efe80c5db389
Parent:
4:531b1120d3ec
Child:
6:4d8938b686a6
First publish of my CreaBotLib Version

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 1:974d020bb582 8 #define MAX_SPEED 30 //cm per second
garphil 1:974d020bb582 9 #define DEFAULT_SPEED 2 //cm per second
garphil 0:a7fb03c9ea9d 10 #define PI 3.141592f
garphil 1:974d020bb582 11 #define DEPTH_FIFO 256
garphil 0:a7fb03c9ea9d 12
garphil 0:a7fb03c9ea9d 13 typedef enum {FORWARD, BACKWARD, ROTATE, LEFT, RIGHT, IDLE} cmdbot_t;
sepp_nepp 5:efe80c5db389 14
sepp_nepp 5:efe80c5db389 15 /** Command structure as put into command FIFO, or treated by the FIFO-Handler */
sepp_nepp 5:efe80c5db389 16 class MotCommand{
sepp_nepp 5:efe80c5db389 17 public:
garphil 0:a7fb03c9ea9d 18 cmdbot_t command;
garphil 0:a7fb03c9ea9d 19 float angle;
garphil 0:a7fb03c9ea9d 20 float cm;
sepp_nepp 5:efe80c5db389 21 /** Assigns the parameters to the class members, depending on command type */
sepp_nepp 5:efe80c5db389 22 void assign(cmdbot_t cmd, float p1, float p2);
garphil 0:a7fb03c9ea9d 23 };
garphil 0:a7fb03c9ea9d 24
sepp_nepp 5:efe80c5db389 25 /** Command FIFO, 256 elements deep, puts MotCommands in a queue,
sepp_nepp 5:efe80c5db389 26 * handles the MotCommands one by one using ticker and callback.
sepp_nepp 5:efe80c5db389 27 * Internally stores all commands in a static 256 array
sepp_nepp 5:efe80c5db389 28 * Only passes pointers forward and backward.
sepp_nepp 5:efe80c5db389 29 */
sepp_nepp 5:efe80c5db389 30 class CommandFIFO {
sepp_nepp 5:efe80c5db389 31 public:
sepp_nepp 5:efe80c5db389 32 /** Create empty FIFO */
sepp_nepp 5:efe80c5db389 33 CommandFIFO() {readIdx = writeIdx = count = 0; cmd_idle.command = IDLE; cmd_idle.angle=0; cmd_idle.cm=0;};
sepp_nepp 5:efe80c5db389 34 /** Empty the FIFO by setting pointers to 0, commands are left in memory. */
sepp_nepp 5:efe80c5db389 35 void empty() {readIdx=writeIdx=count=0;};
sepp_nepp 5:efe80c5db389 36 /** If FIFO is full, it passes back NULL
sepp_nepp 5:efe80c5db389 37 Otherwise Advances the write index once and returns a pointer the next command to write.
sepp_nepp 5:efe80c5db389 38 The caller then fills the command structure at that position.
sepp_nepp 5:efe80c5db389 39 */
sepp_nepp 5:efe80c5db389 40 MotCommand *put();
sepp_nepp 5:efe80c5db389 41 /** If FIFO is empty, it passes back NULL
sepp_nepp 5:efe80c5db389 42 Otherwise Advances the read index once and returns a pointer the next command in the FIFO.
sepp_nepp 5:efe80c5db389 43 Do not free memory associated with the pointer.
sepp_nepp 5:efe80c5db389 44 */
sepp_nepp 5:efe80c5db389 45 MotCommand *get();
sepp_nepp 5:efe80c5db389 46 /** Returns the number of commands in the FIFO, previously called 'getSize' */
sepp_nepp 5:efe80c5db389 47 int getCount() {return count;}
sepp_nepp 5:efe80c5db389 48 /** Returns true when the FIFO is full */
sepp_nepp 5:efe80c5db389 49 bool isFull() {return count>=DEPTH_FIFO;}
sepp_nepp 5:efe80c5db389 50 /** Returns true when the FIFO is empty */
sepp_nepp 5:efe80c5db389 51 bool isEmpty() {return count<=0;}
sepp_nepp 5:efe80c5db389 52 private:
sepp_nepp 5:efe80c5db389 53 int readIdx, writeIdx, count;
sepp_nepp 5:efe80c5db389 54 MotCommand cmd[DEPTH_FIFO];
sepp_nepp 5:efe80c5db389 55 MotCommand cmd_idle;
garphil 0:a7fb03c9ea9d 56 };
garphil 0:a7fb03c9ea9d 57
garphil 0:a7fb03c9ea9d 58
garphil 0:a7fb03c9ea9d 59 /** Asynchronous Control of 2 motors part of a robot
garphil 0:a7fb03c9ea9d 60 *
garphil 0:a7fb03c9ea9d 61 * Example:
garphil 0:a7fb03c9ea9d 62 * @code
garphil 0:a7fb03c9ea9d 63 * // --- Define the Four PINs & Time of movement used for Motor drive -----
garphil 0:a7fb03c9ea9d 64 * 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 65 * Motor motorRight(PA_5,PA_4,PA_3,PA_1);
garphil 0:a7fb03c9ea9d 66 * Creabot mybot(&motorLeft, &motorRight, 10.0f,13.0f); // insert the motors and indicate wheel diameter and distance between wheels
garphil 0:a7fb03c9ea9d 67 *
garphil 0:a7fb03c9ea9d 68 * int main() {
garphil 0:a7fb03c9ea9d 69 *
garphil 0:a7fb03c9ea9d 70 * mybot.setSpeed(12.5); // 12.5cm/s
garphil 0:a7fb03c9ea9d 71 * mybot.move(FORWARD,10); // Go forward of 10cm
garphil 0:a7fb03c9ea9d 72 * mybot.waitEndMove(); // Wait end of Move
garphil 0:a7fb03c9ea9d 73 * 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 74 * mybot.move(BACKWARD,40); // Stop immediately the rotation and go backward of 40cm
garphil 0:a7fb03c9ea9d 75 * mybot.waitEndMove(); // Wait end of Backward
garphil 0:a7fb03c9ea9d 76 * mybot.moveAndWait(LEFT,60); // Move Left of 60° in circle, center being the left wheel (off). Wait end of move
garphil 0:a7fb03c9ea9d 77 * mybot.waitEndMove(); // Not needed, as already waited...
garphil 0:a7fb03c9ea9d 78 * 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 79 * mybot.moveAndWait(ROTATE,90);
garphil 0:a7fb03c9ea9d 80 * mybot.move(ROTATE,-90); // Opposite direction.
garphil 0:a7fb03c9ea9d 81 * mybot.waitEndMove(60); // with watchdog => if after 60s the move is not ended, continue the execution
garphil 0:a7fb03c9ea9d 82 * mybot.stopMove(); // Stop the movement before end...
garphil 0:a7fb03c9ea9d 83 *
garphil 0:a7fb03c9ea9d 84 * // 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 85 * mybot.fifo(FORWARD,10); // Already starting...
garphil 0:a7fb03c9ea9d 86 * mybot.fifo(BACKWARD,10); // will wait end of previous command to go
garphil 0:a7fb03c9ea9d 87 * mybot.fifo(ROTATE,120.0);
garphil 0:a7fb03c9ea9d 88 * mybot.fifo(LEFT, 30, 120);
garphil 0:a7fb03c9ea9d 89 * mybot.fifo(RIGHT, 25);
garphil 0:a7fb03c9ea9d 90 * mybot.waitEndMove(100000); // wait until fifo end... can flush anytime with stopMove...
garphil 0:a7fb03c9ea9d 91 * mybot.stopMove(); // before end... Flush the fifo and remove all instructions…
garphil 0:a7fb03c9ea9d 92 *
garphil 0:a7fb03c9ea9d 93 * while(1) {
garphil 0:a7fb03c9ea9d 94 * };
garphil 0:a7fb03c9ea9d 95 * }
garphil 0:a7fb03c9ea9d 96 * @endcode
garphil 0:a7fb03c9ea9d 97 */
sepp_nepp 5:efe80c5db389 98
garphil 0:a7fb03c9ea9d 99 class Creabot {
garphil 0:a7fb03c9ea9d 100 public:
garphil 0:a7fb03c9ea9d 101 /** Create a Creabot object to synchronize 2 motors
garphil 0:a7fb03c9ea9d 102 *
sepp_nepp 5:efe80c5db389 103 * @param left Motor object declared before corresponding to left motor of the Creabot
garphil 0:a7fb03c9ea9d 104 * @param right Motor object declared before corresponding to right motor of the Creabot
garphil 0:a7fb03c9ea9d 105 * @param diameter_wheel_cm diameter in cm of the wheels (both supposed to be at same diameter)
garphil 0:a7fb03c9ea9d 106 * @param distance_wheel_cm distance in cm between center of left wheel and center of right wheel
garphil 0:a7fb03c9ea9d 107 */
garphil 0:a7fb03c9ea9d 108 Creabot(Motor *left, Motor *right, float diameter_wheel_cm, float distance_wheel_cm);
garphil 0:a7fb03c9ea9d 109 int getStatus();
garphil 0:a7fb03c9ea9d 110 void setSpeed(float cm_per_second);
garphil 0:a7fb03c9ea9d 111 void waitEndMove();
garphil 0:a7fb03c9ea9d 112 void waitEndMove(uint32_t delay_us); // watchdog
garphil 0:a7fb03c9ea9d 113 void setCallBack(void (*callback)(int status));
garphil 0:a7fb03c9ea9d 114 void moveAndWait(cmdbot_t moveType, float angle_or_cm);
garphil 0:a7fb03c9ea9d 115 void moveAndWait(cmdbot_t moveType, float angle, float cm);
garphil 0:a7fb03c9ea9d 116 void move(cmdbot_t moveType, float angle_or_cm);
garphil 0:a7fb03c9ea9d 117 void move(cmdbot_t moveType, float angle, float cm);
garphil 0:a7fb03c9ea9d 118 void fifo(cmdbot_t moveType, float angle_or_cm);
garphil 0:a7fb03c9ea9d 119 void fifo(cmdbot_t moveType, float angle, float cm);
garphil 0:a7fb03c9ea9d 120 void flushFifo();
garphil 4:531b1120d3ec 121 void spirale(float b, float turns);
garphil 0:a7fb03c9ea9d 122 int moveInFifo();
garphil 0:a7fb03c9ea9d 123 void executeFifo();
sepp_nepp 5:efe80c5db389 124 void stopMove();
garphil 0:a7fb03c9ea9d 125
garphil 0:a7fb03c9ea9d 126 private:
garphil 0:a7fb03c9ea9d 127 void moveForward(float cm);
garphil 0:a7fb03c9ea9d 128 void moveBackward(float cm);
garphil 0:a7fb03c9ea9d 129 void rotate(float angle);
garphil 0:a7fb03c9ea9d 130 void moveRight(float angle);
garphil 0:a7fb03c9ea9d 131 void moveRight(float angle, float center_cm);
garphil 0:a7fb03c9ea9d 132 void moveLeft(float angle);
garphil 0:a7fb03c9ea9d 133 void moveLeft(float angle, float center_cm);
garphil 0:a7fb03c9ea9d 134 void callBack();
garphil 0:a7fb03c9ea9d 135 uint32_t computeSpeed(Motor *motor, float cm_per_second);
garphil 0:a7fb03c9ea9d 136
garphil 0:a7fb03c9ea9d 137 void (*extCallBack)(int status);
sepp_nepp 5:efe80c5db389 138 void moveMotorLeft(motorDir dir, float angle);
sepp_nepp 5:efe80c5db389 139 void moveMotorRight(motorDir dir, float angle);
sepp_nepp 5:efe80c5db389 140 void executeCommand(MotCommand *cmd);
sepp_nepp 5:efe80c5db389 141 void setSpeedMot(Motor *motor, float cm_per_second);
garphil 0:a7fb03c9ea9d 142
sepp_nepp 5:efe80c5db389 143
sepp_nepp 5:efe80c5db389 144 private: // The two motors and their physical properties:
sepp_nepp 5:efe80c5db389 145 void callBackLeft();
sepp_nepp 5:efe80c5db389 146 void callBackRight();
garphil 0:a7fb03c9ea9d 147 Motor *motor_left;
garphil 0:a7fb03c9ea9d 148 Motor *motor_right;
garphil 0:a7fb03c9ea9d 149 float diameter_wheel;
garphil 0:a7fb03c9ea9d 150 float distance_wheel;
garphil 0:a7fb03c9ea9d 151 float perimeter_wheel;
garphil 0:a7fb03c9ea9d 152 float perimeter_bot;
garphil 0:a7fb03c9ea9d 153 float degree_wheel_per_cm;
garphil 0:a7fb03c9ea9d 154 float degree_bot_per_cm;
garphil 0:a7fb03c9ea9d 155 float ratio_wheel_bot;
sepp_nepp 5:efe80c5db389 156
sepp_nepp 5:efe80c5db389 157
sepp_nepp 5:efe80c5db389 158 float last_speed; // last requested speed
garphil 0:a7fb03c9ea9d 159 int status;
sepp_nepp 5:efe80c5db389 160 bool endMoveLeft;
sepp_nepp 5:efe80c5db389 161 bool endMoveRight;
garphil 0:a7fb03c9ea9d 162 bool endMove;
sepp_nepp 5:efe80c5db389 163 CommandFIFO fifoBot;
sepp_nepp 5:efe80c5db389 164 MotCommand current_cmd;
garphil 0:a7fb03c9ea9d 165 Ticker botTicker;
garphil 0:a7fb03c9ea9d 166 bool executingFifo;
sepp_nepp 5:efe80c5db389 167
sepp_nepp 5:efe80c5db389 168 private:
sepp_nepp 5:efe80c5db389 169 /* members that were not used in last release
sepp_nepp 5:efe80c5db389 170 * float macro_move_parameter;
sepp_nepp 5:efe80c5db389 171 * MotCommand macro_move;
sepp_nepp 5:efe80c5db389 172 * void setSpeedLeft(float cm_per_second);
sepp_nepp 5:efe80c5db389 173 * void setSpeedRight(float cm_per_second); */
garphil 0:a7fb03c9ea9d 174 };
garphil 0:a7fb03c9ea9d 175
garphil 0:a7fb03c9ea9d 176 #endif