My Version of CreaBotLib
Fork of CreaBotLib by
CreaBot.h
- Committer:
- sepp_nepp
- Date:
- 2019-01-01
- Revision:
- 7:3a793ddc3490
- Parent:
- 6:4d8938b686a6
- Child:
- 8:3b5b0a82b429
File content as of revision 7:3a793ddc3490:
/* * at: file CreaBot.h * bs_brief File contains Creabot Library. * CreaBot.h contains the class Motor, and related enums and structs. * Includes "mbed.h" and "motor.h" * Refactorings: * All variables with suffixes "_cm_sec" = speeds in centimeters per second * MotCommand -> BotCommand * cmdbot_t -> BotCmdVerb * cm -> dist_cm * at_author Tarek Lule, Francois Druilhe, et al. * at_date 21. Nov. 2018. * at_see https://os.mbed.com/users/sepp_nepp/code/CreaBotLib/ */ // -------------------- Motor --------------------------- #ifndef CREABOT_H #define CREABOT_H #include "mbed.h" #include "motor.h" #define MAX_SPEED_CM_SEC 30.0f /*star< Clamp maximum advancement speed = 30cm/sec, was MAX_SPEED */ #define MIN_SPEED_CM_SEC 0.001f /*star< Clamp minimum advancement speed = 10um/sec */ #define DEFAULT_SPEED_CM_SEC 2.0f /*star< Default advancement speed = 2.0cm/sec, was DEFAULT_SPEED */ #define PI 3.141592f #define DEPTH_FIFO 256 /*star< Initialize the depth of the command FIFO to 256 */ /*star \enum BotCmdVerb * bs_brief Robot Commands Verbs, gives the movement direction * */ typedef enum { IDLE = 0, /*star< Command to do nothing */ FORWARD, /*star< Advance the robot straight forward */ BACKWARD, /*star< Advance the robot straight backwards. */ ROTATE, /*star< Rotate around its own axis */ LEFT, /*star< Advance in a left curve */ RIGHT, /*star< Advance in a right curve */ REVLEFT, /*star< Reverse in a left curve */ REVRIGHT /*star< Reverse in a right curve */ } BotCmdVerb; /*star \enum TMotorsState * bs_brief Possible states of the two motors of the Bot * */ typedef enum { LRMOTS_STOP = 0,/*star< All Motors have stopped */ LMOT_RUNS, /*star< Left Motor runs */ RMOT_RUNS, /*star< Right Motor runs */ LRMOTS_RUN /*star< Both Motors run */ } TMotorsState; /*star \struct BotCommand * bs_brief Structure of a Motor Command. * The command structure is put into command FIFO, and treated by the FIFO-Handler */ typedef struct { BotCmdVerb command; /*star< General Command to give movement direction.*/ float dist_cm; /*star< Distance in dist_cm for translational movements .*/ float angle_deg; /*star< Angle in degree for rotational movement .*/ void set(BotCmdVerb Acommand, float Aangle_deg, float Adist_cm); /*star< Helper; set structure fields to values */ void set(BotCmdVerb Acommand, float Aparam); /*star< Helper; set structure fields to values */ } BotCommand; /*star \struct geometries * bs_brief Some geometric encapsulation * Holds some geometric values */ typedef struct { float diam_cm; float perim_cm; float degree_per_cm; void setDiam( float Adiam_cm); /*star< Helper; set diameter and perim to values */ } circle_geos; /*star \class CommandFIFO * bs_brief 256 elements deep Command FIFO, to puts BotCommand in a queue. * * Internally stores all BotCommand in a static 256 array used as a circular ring buffer. * Adds incoming commands at the head of the ring buffer at position writeIdx, * Gets oldest commands from the tail of the ring buffer at position readIdx, * Keeps track of the occupied size in Count. * BotCommands are passed back as pointers. */ class CommandFIFO { public: /*star Class Creator: initializes an empties FIFO * * Ring buffer is allocated statically. Read and Write Index are set to 0. */ CommandFIFO(); /*star Empty the FIFO. * * Since ring buffer is static, it suffice to set all pointers to 0, commands are left in memory. */ void empty() {readIdx=writeIdx=Count=0;}; /*star Reserve a new element at the head of the FIFO * *If FIFO is full, it passes back NULL *Otherwise Advances the write index once and returns a pointer the next free command struct. *The caller then has to fills the command structure at that position. *Do not free memory associated to the pointer. * *at_return <BotCommand*> Pointer to the reserved BotCommand to write to. */ BotCommand *put(); void put(BotCmdVerb Acommand, float Adist_cm, float Aangle_deg); /*star Get and remove the oldest element at the tail of the FIFO * *If FIFO is empty, it passes back NULL *Otherwise advances the read index once and returns a pointer the oldest stored command in the FIFO. *Do not free memory associated with the pointer. * *at_return <BotCommand*> Pointer to the oldest BotCommand. */ BotCommand *get(); /*star Access FIFO used Count. * *at_return <int> the number of commands in the FIFO */ int getCount() {return Count;} /*star Check if FIFO is full. * * Space is limited to DEPTH_FIFO=256 BotCommand elements. * Should be checked before trying to put new elements * * at_return <bool> True if FIFO is full*/ bool isFull() {return Count>=DEPTH_FIFO;} /*star Check if FIFO is empty. * * Should be checked before trying to get new elements * * at_return <bool> True if FIFO is empty*/ bool isEmpty() {return Count<=0;} private: int readIdx; /*star< Index in FIFO array where to get the oldest element from. */ int writeIdx; /*star< Index in FIFO array where to put the next new element to. */ int Count; /*star< Counts the number of elements in array used. */ BotCommand cmd[DEPTH_FIFO]; /*star< Actual static FIFO array where all elements reside. */ BotCommand cmd_idle; /*star< A Constant Idle command */ }; /*star \class Creabot * bs_brief Synchronous Control of 2 motors as part of a two wheel robot * * Example: * at_code * // --- Define the Four PINs & Time of movement used for Motor drive ----- * 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) * Motor motorRight(PA_5,PA_4,PA_3,PA_1); * Creabot mybot(&motorLeft, &motorRight, 10.0f,13.0f); // insert the motors and indicate wheel diameter and distance between wheels * * int main() { * * mybot.setSpeed(12.5); // 12.5cm/s * mybot.move(FORWARD,10); // Go forward of 10cm * mybot.waitEndMove(); // Wait end of Move * mybot.move(ROTATE,90); // Start rotation of 90° around the center between wheels (two wheels running in same direction at same speed) * mybot.move(BACKWARD,40); // Stop immediately the rotation and go backward of 40cm * mybot.waitEndMove(); // Wait end of Backward * mybot.moveAndWait(LEFT,60); // Move Left of 60° in circle, center being the left wheel (off). Wait end of move * mybot.waitEndMove(); // Not needed, as already waited... * 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. * mybot.moveAndWait(ROTATE,90); * mybot.move(ROTATE,-90); // Opposite direction. * mybot.waitEndMove(60); // with watchdog => if after 60s the move is not ended, continue the execution * mybot.stopMove(); // Stop the movement before end... * * // 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. * mybot.fifo(FORWARD,10); // Already starting... * mybot.fifo(BACKWARD,10); // will wait end of previous command to go * mybot.fifo(ROTATE,120.0); * mybot.fifo(LEFT, 30, 120); * mybot.fifo(RIGHT, 25); * mybot.waitEndMove(100000); // wait until fifo end... can flush anytime with stopMove... * mybot.stopMove(); // before end... Flush the fifo and remove all instructions… * * while(1) { * }; * } * at_endcode */ class Creabot { public: /*star Create a Creabot object with 2 motors * * at_param left Motor object declared before corresponding to left motor of the Creabot * at_param right Motor object declared before corresponding to right motor of the Creabot * at_param wheel_diam_cm diameter in cm of the wheels (both muste be the same diameter) * at_param bot_diam_cm distance cm between center of left wheel and center of right wheel */ Creabot(Motor *left, Motor *right, float wheel_diam_cm, float bot_diam_cm); /*star Property access to Motor state */ TMotorsState getState() {return MotState; }; public: void qMove(BotCmdVerb moveType, float angle_or_cm); void qMove(BotCmdVerb moveType, float angle_deg, float dist_cm); void setCallBack(void (*Acallback)(int status)); void moveAndWait(BotCmdVerb moveType, float angle_or_cm); void moveAndWait(BotCmdVerb moveType, float angle_deg, float dist_cm); void flushFifo(); void spirale(float b, float turns); int moveInFifo(); void executeFifo(); void stopMove(); private: uint32_t computeSpeed(Motor *motor, float speed_cm_sec); private: CommandFIFO fifoBot; Ticker botTicker; bool executingFifo; public: void move(BotCmdVerb moveType, float angle_or_cm); void move(BotCmdVerb moveType, float angle_deg, float dist_cm); void executeCommand(BotCommand *cmd); public: /* Mid level access functions: set bot-speed parameter for next motor command*/ void setBotSpeed(float Abot_speed_cm_sec); /* Mid level control function: move bot forward for a given distance */ void moveForward(float dist_cm); /* Mid level control function: move bot backwards for a given distance */ void moveBackward(float dist_cm); /* Mid level control function: turn bot forward right, around a radius twice the bot size */ void moveRight(float angle_deg); /* Mid level control function: turn bot forward right, around a radius that is center_cm away from the right wheel*/ void moveRight(float angle_deg, float center_cm); /* Mid level control function: turn bot forward left, around a radius twice the bot size */ void moveLeft(float angle_deg); /* Mid level control function: turn bot forward left, around a radius that is center_cm away from the left wheel*/ void moveLeft(float angle_deg, float center_cm); /* Mid level control function: turn bot on its place for a given angle. positive angles turn right, negative angles turn left*/ void rotate(float angle_deg); /* Low Level: spends all time with waits, returns only once MotorState = LRMOTS_STOP, not recommended due its exclusive blocking behavior */ void waitEndMotors(); /* Low Level: spends all time with waits, returns only once MotorState = LRMOTS_STOP, but waits no more than max_wait_ms milli seconds, not recommended due its exclusive blocking behavior */ void waitEndMotors(uint32_t max_wait_ms); // watchdog private: /* geometric properties of each wheel and the bot */ circle_geos wheel,bot; float ratio_wheel_bot; float bot_speed_cm_sec; // the requested bot speed /* Low level access functions: sets both motor speeds immediately */ void setMotorsSpeed(float mot_speed_cm_sec) /* Low level motor command wrappers, speed, move and stop, state management */ void setMotorSpeed(Motor *motor, float speed_cm_sec); /*star Commands the Left Motor to Move for a given angle in a given direction * updates the Motor state MotState */ void moveMotorLeft(motorDir dir, float angle_deg); /*star Commands the Right Motor to Move for a given angle in a given direction * updates the Motor state MotState */ void moveMotorRight(motorDir dir, float angle_deg); /*star Callback when left Motor stops. * updates the Motor state MotState */ void cbLeftMotStopped(); /*star Callback when right Motor stops. * updates the Motor state MotState */ void cbRightMotStopped(); /*star Callback when all Motors have stopped. * updates the Motor state MotState, switch off all Phases */ void AllMotorsStopped(); /*star Callback function pointer: if set it is called when All Motors Stopped()*/ void (*extCallBack)(int status); /*star Motor status register, remember which of the motors still run */ TMotorsState MotState; // The two motors, as pointers to their classes Motor *motor_left; Motor *motor_right; private: /* members that were not used in last release * float macro_move_parameter; * BotCommand macro_move; * void setSpeedLeft(float speed_cm_sec); * void setSpeedRight(float speed_cm_sec); */ }; #endif