My Version of CreaBotLib
Fork of CreaBotLib by
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?
User | Revision | Line number | New 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 |