Tarek Lule / CreaBotLib

My Version of CreaBotLib

Fork of CreaBotLib by CreaLab

CreaBot.h@6:4d8938b686a6, 2018-11-28 (annotated)

Committer:
sepp_nepp
Date:
Wed Nov 28 09:46:41 2018 +0000
Revision:
6:4d8938b686a6
Parent:
5:efe80c5db389
Child:
7:3a793ddc3490
Update with some changes to documentation. ; Not yet tried on actual robot. ; Some bugs in my doxygen code means documentation generation fails.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sepp_nepp 6:4d8938b686a6 1 /**
sepp_nepp 6:4d8938b686a6 2 * @file CreaBot.h
sepp_nepp 6:4d8938b686a6 3 * @brief File contains Creabot Library.
sepp_nepp 6:4d8938b686a6 4
sepp_nepp 6:4d8938b686a6 5 * CreaBot.h contains the class Motor, and related enums and structs.
sepp_nepp 6:4d8938b686a6 6 * Includes "mbed.h" and "motor.h"
sepp_nepp 6:4d8938b686a6 7
sepp_nepp 6:4d8938b686a6 8 * Refactorings:
sepp_nepp 6:4d8938b686a6 9 * All variables with suffixes "_cm_sec" = speeds in centimeters per second
sepp_nepp 6:4d8938b686a6 10 * MotCommand -> BotCommand
sepp_nepp 6:4d8938b686a6 11 * cmdbot_t -> BotCmdVerb
sepp_nepp 6:4d8938b686a6 12 * cm -> dist_cm
sepp_nepp 6:4d8938b686a6 13
sepp_nepp 6:4d8938b686a6 14 * @author Tarek Lule, Francois Druilhe, et al.
sepp_nepp 6:4d8938b686a6 15 * @date 21. Nov. 2018.
sepp_nepp 6:4d8938b686a6 16 * @see https://os.mbed.com/users/sepp_nepp/code/CreaBotLib/ */
sepp_nepp 6:4d8938b686a6 17
sepp_nepp 6:4d8938b686a6 18 // -------------------- Motor ---------------------------
sepp_nepp 6:4d8938b686a6 19
garphil 0:a7fb03c9ea9d 20 #ifndef CREABOT_H
garphil 0:a7fb03c9ea9d 21 #define CREABOT_H
garphil 0:a7fb03c9ea9d 22
garphil 0:a7fb03c9ea9d 23 #include "mbed.h"
garphil 0:a7fb03c9ea9d 24 #include "motor.h"
garphil 0:a7fb03c9ea9d 25
sepp_nepp 6:4d8938b686a6 26 #define MAX_SPEED_CM_SEC 30.0f /**< Clamp maximum advancement speed = 30cm/sec, was MAX_SPEED */
sepp_nepp 6:4d8938b686a6 27 #define DEFAULT_SPEED_CM_SEC 2.0f /**< Default advancement speed = 2.0cm/sec, was DEFAULT_SPEED */
garphil 0:a7fb03c9ea9d 28 #define PI 3.141592f
sepp_nepp 6:4d8938b686a6 29 #define DEPTH_FIFO 256 /**< Initialize the depth of the command FIFO to 256 */
garphil 0:a7fb03c9ea9d 30
sepp_nepp 6:4d8938b686a6 31 /** \enum BotCmdVerb
sepp_nepp 6:4d8938b686a6 32 * \brief Robot Commands Verbs, gives the movement direction
sepp_nepp 6:4d8938b686a6 33 * */
sepp_nepp 6:4d8938b686a6 34 typedef enum {
sepp_nepp 6:4d8938b686a6 35 IDLE = 0, /**< Command to do nothing */
sepp_nepp 6:4d8938b686a6 36 FORWARD, /**< Advance the robot straight forward */
sepp_nepp 6:4d8938b686a6 37 BACKWARD, /**< Advance the robot straight backwards. */
sepp_nepp 6:4d8938b686a6 38 ROTATE, /**< Rotate around its own axis */
sepp_nepp 6:4d8938b686a6 39 LEFT, /**< Advance in a left curve */
sepp_nepp 6:4d8938b686a6 40 RIGHT, /**< Advance in a right curve */
sepp_nepp 6:4d8938b686a6 41 REVLEFT, /**< Reverse in a left curve */
sepp_nepp 6:4d8938b686a6 42 REVRIGHT /**< Reverse in a right curve */
sepp_nepp 6:4d8938b686a6 43 } BotCmdVerb;
sepp_nepp 6:4d8938b686a6 44
sepp_nepp 5:efe80c5db389 45
sepp_nepp 6:4d8938b686a6 46 /** \enum BotStatus
sepp_nepp 6:4d8938b686a6 47 * \brief Possible states of two motors of Bot
sepp_nepp 6:4d8938b686a6 48 * */
sepp_nepp 6:4d8938b686a6 49 typedef enum {
sepp_nepp 6:4d8938b686a6 50 AlLSTOP = 0,/**< All Motors have stopped */
sepp_nepp 6:4d8938b686a6 51 LMOT_RUNS, /**< Left Motor still runs */
sepp_nepp 6:4d8938b686a6 52 RMOT_RUNS, /**< Right Motor still runs */
sepp_nepp 6:4d8938b686a6 53 MOTORS_RUN /**< Both Motors still run */
sepp_nepp 6:4d8938b686a6 54 } BotStatus;
sepp_nepp 6:4d8938b686a6 55
sepp_nepp 6:4d8938b686a6 56 /** \struct BotCommand
sepp_nepp 6:4d8938b686a6 57 * \brief Structure of a Motor Command.
garphil 0:a7fb03c9ea9d 58
sepp_nepp 6:4d8938b686a6 59 * The command structure is put into command FIFO, and treated by the FIFO-Handler */
sepp_nepp 6:4d8938b686a6 60 typedef struct {
sepp_nepp 6:4d8938b686a6 61 BotCmdVerb command; /**< General Command to give movement direction.*/
sepp_nepp 6:4d8938b686a6 62 float dist_cm; /**< Distance in dist_cm for translational movements .*/
sepp_nepp 6:4d8938b686a6 63 float angle_deg; /**< Angle in degree for rotational movement .*/
sepp_nepp 6:4d8938b686a6 64 void set(BotCmdVerb Acommand, float Adist_cm, float Aangle_deg); /**< Helper; set structure fields to values */
sepp_nepp 6:4d8938b686a6 65 void set(BotCmdVerb Acommand, float Adist_cm); /**< Helper; set structure fields to values */
sepp_nepp 6:4d8938b686a6 66 } BotCommand;
sepp_nepp 6:4d8938b686a6 67
sepp_nepp 6:4d8938b686a6 68 /** \class CommandFIFO
sepp_nepp 6:4d8938b686a6 69 * \brief 256 elements deep Command FIFO, to puts BotCommand in a queue.
sepp_nepp 6:4d8938b686a6 70 *
sepp_nepp 6:4d8938b686a6 71 * Internally stores all BotCommand in a static 256 array used as a circular ring buffer.
sepp_nepp 6:4d8938b686a6 72 * Adds incoming commands at the head of the ring buffer at position writeIdx,
sepp_nepp 6:4d8938b686a6 73 * Gets oldest commands from the tail of the ring buffer at position readIdx,
sepp_nepp 6:4d8938b686a6 74 * Keeps track of the occupied size in count.
sepp_nepp 6:4d8938b686a6 75 * BotCommands are passed back as pointers.
sepp_nepp 5:efe80c5db389 76 */
sepp_nepp 5:efe80c5db389 77 class CommandFIFO {
sepp_nepp 5:efe80c5db389 78 public:
sepp_nepp 6:4d8938b686a6 79 /** Class Creator: initializes an empties FIFO
sepp_nepp 6:4d8938b686a6 80 *
sepp_nepp 6:4d8938b686a6 81 * Ring buffer is allocated statically. Read and Write Index are set to 0. */
sepp_nepp 6:4d8938b686a6 82 CommandFIFO();
sepp_nepp 6:4d8938b686a6 83
sepp_nepp 6:4d8938b686a6 84 /** Empty the FIFO.
sepp_nepp 6:4d8938b686a6 85 *
sepp_nepp 6:4d8938b686a6 86 * Since ring buffer is static, it suffice to set all pointers to 0, commands are left in memory. */
sepp_nepp 5:efe80c5db389 87 void empty() {readIdx=writeIdx=count=0;};
sepp_nepp 6:4d8938b686a6 88
sepp_nepp 6:4d8938b686a6 89 /** Reserve a new element at the head of the FIFO
sepp_nepp 6:4d8938b686a6 90 *
sepp_nepp 6:4d8938b686a6 91 *If FIFO is full, it passes back NULL
sepp_nepp 6:4d8938b686a6 92 *Otherwise Advances the write index once and returns a pointer the next free command struct.
sepp_nepp 6:4d8938b686a6 93 *The caller then has to fills the command structure at that position.
sepp_nepp 6:4d8938b686a6 94 *Do not free memory associated to the pointer.
sepp_nepp 6:4d8938b686a6 95 *
sepp_nepp 6:4d8938b686a6 96 *@return <BotCommand*> Pointer to the reserved BotCommand to write to.
sepp_nepp 6:4d8938b686a6 97 */
sepp_nepp 6:4d8938b686a6 98 BotCommand *put();
sepp_nepp 6:4d8938b686a6 99
sepp_nepp 6:4d8938b686a6 100 void put(BotCmdVerb Acommand, float Adist_cm, float Aangle_deg);
sepp_nepp 6:4d8938b686a6 101
sepp_nepp 6:4d8938b686a6 102 /** Get and remove the oldest element at the tail of the FIFO
sepp_nepp 6:4d8938b686a6 103 *
sepp_nepp 6:4d8938b686a6 104 *If FIFO is empty, it passes back NULL
sepp_nepp 6:4d8938b686a6 105 *Otherwise advances the read index once and returns a pointer the oldest stored command in the FIFO.
sepp_nepp 6:4d8938b686a6 106 *Do not free memory associated with the pointer.
sepp_nepp 6:4d8938b686a6 107 *
sepp_nepp 6:4d8938b686a6 108 *@return <BotCommand*> Pointer to the oldest BotCommand.
sepp_nepp 6:4d8938b686a6 109 */
sepp_nepp 6:4d8938b686a6 110 BotCommand *get();
sepp_nepp 6:4d8938b686a6 111
sepp_nepp 6:4d8938b686a6 112 /** Access FIFO used count.
sepp_nepp 6:4d8938b686a6 113 *
sepp_nepp 6:4d8938b686a6 114 *@return <int> the number of commands in the FIFO */
sepp_nepp 5:efe80c5db389 115 int getCount() {return count;}
sepp_nepp 6:4d8938b686a6 116
sepp_nepp 6:4d8938b686a6 117 /** Check if FIFO is full.
sepp_nepp 6:4d8938b686a6 118 *
sepp_nepp 6:4d8938b686a6 119 * Space is limited to DEPTH_FIFO=256 BotCommand elements.
sepp_nepp 6:4d8938b686a6 120 * Should be checked before trying to put new elements
sepp_nepp 6:4d8938b686a6 121 *
sepp_nepp 6:4d8938b686a6 122 * @return <bool> True if FIFO is full*/
sepp_nepp 5:efe80c5db389 123 bool isFull() {return count>=DEPTH_FIFO;}
sepp_nepp 6:4d8938b686a6 124
sepp_nepp 6:4d8938b686a6 125 /** Check if FIFO is empty.
sepp_nepp 6:4d8938b686a6 126 *
sepp_nepp 6:4d8938b686a6 127 * Should be checked before trying to get new elements
sepp_nepp 6:4d8938b686a6 128 *
sepp_nepp 6:4d8938b686a6 129 * @return <bool> True if FIFO is empty*/
sepp_nepp 5:efe80c5db389 130 bool isEmpty() {return count<=0;}
sepp_nepp 6:4d8938b686a6 131
sepp_nepp 5:efe80c5db389 132 private:
sepp_nepp 6:4d8938b686a6 133 int readIdx; /**< Index in FIFO array where to get the oldest element from. */
sepp_nepp 6:4d8938b686a6 134 int writeIdx; /**< Index in FIFO array where to put the next new element to. */
sepp_nepp 6:4d8938b686a6 135 int Count; /**< Counts the number of elements in array used. */
sepp_nepp 6:4d8938b686a6 136 BotCommand cmd[DEPTH_FIFO]; /**< Actual static FIFO array where all elements reside. */
sepp_nepp 6:4d8938b686a6 137 BotCommand cmd_idle; /**< A Constant Idle command */
garphil 0:a7fb03c9ea9d 138 };
garphil 0:a7fb03c9ea9d 139
sepp_nepp 6:4d8938b686a6 140 /** \class Creabot
garphil 0:a7fb03c9ea9d 141
sepp_nepp 6:4d8938b686a6 142 * \brief Synchronous Control of 2 motors as part of a two wheel robot
garphil 0:a7fb03c9ea9d 143 *
garphil 0:a7fb03c9ea9d 144 * Example:
garphil 0:a7fb03c9ea9d 145 * @code
garphil 0:a7fb03c9ea9d 146 * // --- Define the Four PINs & Time of movement used for Motor drive -----
garphil 0:a7fb03c9ea9d 147 * 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 148 * Motor motorRight(PA_5,PA_4,PA_3,PA_1);
garphil 0:a7fb03c9ea9d 149 * Creabot mybot(&motorLeft, &motorRight, 10.0f,13.0f); // insert the motors and indicate wheel diameter and distance between wheels
garphil 0:a7fb03c9ea9d 150 *
garphil 0:a7fb03c9ea9d 151 * int main() {
garphil 0:a7fb03c9ea9d 152 *
garphil 0:a7fb03c9ea9d 153 * mybot.setSpeed(12.5); // 12.5cm/s
garphil 0:a7fb03c9ea9d 154 * mybot.move(FORWARD,10); // Go forward of 10cm
garphil 0:a7fb03c9ea9d 155 * mybot.waitEndMove(); // Wait end of Move
garphil 0:a7fb03c9ea9d 156 * 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 157 * mybot.move(BACKWARD,40); // Stop immediately the rotation and go backward of 40cm
garphil 0:a7fb03c9ea9d 158 * mybot.waitEndMove(); // Wait end of Backward
garphil 0:a7fb03c9ea9d 159 * mybot.moveAndWait(LEFT,60); // Move Left of 60° in circle, center being the left wheel (off). Wait end of move
garphil 0:a7fb03c9ea9d 160 * mybot.waitEndMove(); // Not needed, as already waited...
garphil 0:a7fb03c9ea9d 161 * 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 162 * mybot.moveAndWait(ROTATE,90);
garphil 0:a7fb03c9ea9d 163 * mybot.move(ROTATE,-90); // Opposite direction.
garphil 0:a7fb03c9ea9d 164 * mybot.waitEndMove(60); // with watchdog => if after 60s the move is not ended, continue the execution
garphil 0:a7fb03c9ea9d 165 * mybot.stopMove(); // Stop the movement before end...
garphil 0:a7fb03c9ea9d 166 *
garphil 0:a7fb03c9ea9d 167 * // 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 168 * mybot.fifo(FORWARD,10); // Already starting...
garphil 0:a7fb03c9ea9d 169 * mybot.fifo(BACKWARD,10); // will wait end of previous command to go
garphil 0:a7fb03c9ea9d 170 * mybot.fifo(ROTATE,120.0);
garphil 0:a7fb03c9ea9d 171 * mybot.fifo(LEFT, 30, 120);
garphil 0:a7fb03c9ea9d 172 * mybot.fifo(RIGHT, 25);
garphil 0:a7fb03c9ea9d 173 * mybot.waitEndMove(100000); // wait until fifo end... can flush anytime with stopMove...
garphil 0:a7fb03c9ea9d 174 * mybot.stopMove(); // before end... Flush the fifo and remove all instructions…
garphil 0:a7fb03c9ea9d 175 *
garphil 0:a7fb03c9ea9d 176 * while(1) {
garphil 0:a7fb03c9ea9d 177 * };
garphil 0:a7fb03c9ea9d 178 * }
garphil 0:a7fb03c9ea9d 179 * @endcode
garphil 0:a7fb03c9ea9d 180 */
sepp_nepp 5:efe80c5db389 181
garphil 0:a7fb03c9ea9d 182 class Creabot {
garphil 0:a7fb03c9ea9d 183 public:
sepp_nepp 6:4d8938b686a6 184 /** Create a Creabot object with 2 motors
garphil 0:a7fb03c9ea9d 185 *
sepp_nepp 5:efe80c5db389 186 * @param left Motor object declared before corresponding to left motor of the Creabot
garphil 0:a7fb03c9ea9d 187 * @param right Motor object declared before corresponding to right motor of the Creabot
sepp_nepp 6:4d8938b686a6 188 * @param diameter_wheel_cm diameter in dist_cm of the wheels (both supposed to be at same diameter)
sepp_nepp 6:4d8938b686a6 189 * @param distance_wheel_cm distance in dist_cm between center of left wheel and center of right wheel
garphil 0:a7fb03c9ea9d 190 */
garphil 0:a7fb03c9ea9d 191 Creabot(Motor *left, Motor *right, float diameter_wheel_cm, float distance_wheel_cm);
sepp_nepp 6:4d8938b686a6 192
sepp_nepp 6:4d8938b686a6 193 /** Property access to Motor state */
sepp_nepp 6:4d8938b686a6 194 BotState getState() {return MotState};
sepp_nepp 6:4d8938b686a6 195
sepp_nepp 6:4d8938b686a6 196 void setSpeed(float speed_cm_sec);
garphil 0:a7fb03c9ea9d 197 void waitEndMove();
garphil 0:a7fb03c9ea9d 198 void waitEndMove(uint32_t delay_us); // watchdog
sepp_nepp 6:4d8938b686a6 199 void setCallBack(void (*Acallback)(int status));
sepp_nepp 6:4d8938b686a6 200 void moveAndWait(BotCmdVerb moveType, float angle_or_cm);
sepp_nepp 6:4d8938b686a6 201 void moveAndWait(BotCmdVerb moveType, float angle_deg, float dist_cm);
sepp_nepp 6:4d8938b686a6 202 void move(BotCmdVerb moveType, float angle_or_cm);
sepp_nepp 6:4d8938b686a6 203 void move(BotCmdVerb moveType, float angle_deg, float dist_cm);
sepp_nepp 6:4d8938b686a6 204 void fifo(BotCmdVerb moveType, float angle_or_cm);
sepp_nepp 6:4d8938b686a6 205 void fifo(BotCmdVerb moveType, float angle_deg, float dist_cm);
garphil 0:a7fb03c9ea9d 206 void flushFifo();
garphil 4:531b1120d3ec 207 void spirale(float b, float turns);
garphil 0:a7fb03c9ea9d 208 int moveInFifo();
garphil 0:a7fb03c9ea9d 209 void executeFifo();
sepp_nepp 5:efe80c5db389 210 void stopMove();
garphil 0:a7fb03c9ea9d 211
garphil 0:a7fb03c9ea9d 212 private:
sepp_nepp 6:4d8938b686a6 213 void moveForward(float dist_cm);
sepp_nepp 6:4d8938b686a6 214 void moveBackward(float dist_cm);
sepp_nepp 6:4d8938b686a6 215 void rotate(float angle_deg);
sepp_nepp 6:4d8938b686a6 216 void moveRight(float angle_deg);
sepp_nepp 6:4d8938b686a6 217 void moveRight(float angle_deg, float center_cm);
sepp_nepp 6:4d8938b686a6 218 void moveLeft(float angle_deg);
sepp_nepp 6:4d8938b686a6 219 void moveLeft(float angle_deg, float center_cm);
sepp_nepp 6:4d8938b686a6 220 uint32_t computeSpeed(Motor *motor, float speed_cm_sec);
garphil 0:a7fb03c9ea9d 221
garphil 0:a7fb03c9ea9d 222 void (*extCallBack)(int status);
sepp_nepp 6:4d8938b686a6 223 void moveMotorLeft(motorDir dir, float angle_deg);
sepp_nepp 6:4d8938b686a6 224 void moveMotorRight(motorDir dir, float angle_deg);
sepp_nepp 6:4d8938b686a6 225 void executeCommand(BotCommand *cmd);
sepp_nepp 6:4d8938b686a6 226 void setSpeedMot(Motor *motor, float speed_cm_sec);
garphil 0:a7fb03c9ea9d 227
sepp_nepp 5:efe80c5db389 228
sepp_nepp 6:4d8938b686a6 229 private:
sepp_nepp 6:4d8938b686a6 230 // The two motors and their physical properties:
sepp_nepp 6:4d8938b686a6 231 // Two call back functions that are called back by the motors when they have stopped
sepp_nepp 6:4d8938b686a6 232 void cbLeftMotStopped();
sepp_nepp 6:4d8938b686a6 233 void cbRightMotStopped();
sepp_nepp 6:4d8938b686a6 234 void AllMotorsStopped();
garphil 0:a7fb03c9ea9d 235 Motor *motor_left;
garphil 0:a7fb03c9ea9d 236 Motor *motor_right;
garphil 0:a7fb03c9ea9d 237 float diameter_wheel;
garphil 0:a7fb03c9ea9d 238 float distance_wheel;
garphil 0:a7fb03c9ea9d 239 float perimeter_wheel;
garphil 0:a7fb03c9ea9d 240 float perimeter_bot;
garphil 0:a7fb03c9ea9d 241 float degree_wheel_per_cm;
garphil 0:a7fb03c9ea9d 242 float degree_bot_per_cm;
garphil 0:a7fb03c9ea9d 243 float ratio_wheel_bot;
sepp_nepp 5:efe80c5db389 244
sepp_nepp 5:efe80c5db389 245
sepp_nepp 5:efe80c5db389 246 float last_speed; // last requested speed
sepp_nepp 5:efe80c5db389 247 CommandFIFO fifoBot;
sepp_nepp 6:4d8938b686a6 248 BotCommand current_cmd;
garphil 0:a7fb03c9ea9d 249 Ticker botTicker;
garphil 0:a7fb03c9ea9d 250 bool executingFifo;
sepp_nepp 5:efe80c5db389 251
sepp_nepp 6:4d8938b686a6 252 BotState MotState;
sepp_nepp 5:efe80c5db389 253 private:
sepp_nepp 5:efe80c5db389 254 /* members that were not used in last release
sepp_nepp 5:efe80c5db389 255 * float macro_move_parameter;
sepp_nepp 6:4d8938b686a6 256 * BotCommand macro_move;
sepp_nepp 6:4d8938b686a6 257 * void setSpeedLeft(float speed_cm_sec);
sepp_nepp 6:4d8938b686a6 258 * void setSpeedRight(float speed_cm_sec); */
garphil 0:a7fb03c9ea9d 259 };
garphil 0:a7fb03c9ea9d 260
garphil 0:a7fb03c9ea9d 261 #endif