Tarek Lule / CreaBotLib

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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