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.cpp@12:530772639065, 2019-04-18 (annotated)

Committer:
sepp_nepp
Date:
Thu Apr 18 12:06:07 2019 +0000
Revision:
12:530772639065
Parent:
11:5a94af0afa12 
Compiles

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garphil 0:a7fb03c9ea9d 1 #include "CreaBot.h"
garphil 0:a7fb03c9ea9d 2
sepp_nepp 5:efe80c5db389 3 // *****************************************************************
sepp_nepp 6:4d8938b686a6 4 // BotCommand Structure Helper
sepp_nepp 5:efe80c5db389 5 // *****************************************************************
sepp_nepp 5:efe80c5db389 6
sepp_nepp 6:4d8938b686a6 7 // Struct Helper; set structure fields to values */
sepp_nepp 9:efe9f76d6f76 8 void BotCommand::set(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm)
sepp_nepp 6:4d8938b686a6 9 { command = Acommand;
sepp_nepp 6:4d8938b686a6 10 dist_cm = Adist_cm;
sepp_nepp 9:efe9f76d6f76 11 turn_angle_deg= Aturn_angle_deg;
sepp_nepp 6:4d8938b686a6 12 };
sepp_nepp 6:4d8938b686a6 13
sepp_nepp 6:4d8938b686a6 14 // Struct Helper; set structure fields to values */
sepp_nepp 7:3a793ddc3490 15 void BotCommand::set(BotCmdVerb Acommand, float Aparam)
sepp_nepp 6:4d8938b686a6 16 { command = Acommand;
sepp_nepp 7:3a793ddc3490 17 dist_cm = Aparam;
sepp_nepp 9:efe9f76d6f76 18 turn_angle_deg= Aparam;
sepp_nepp 5:efe80c5db389 19 };
sepp_nepp 5:efe80c5db389 20
sepp_nepp 5:efe80c5db389 21
sepp_nepp 5:efe80c5db389 22 // *****************************************************************
sepp_nepp 5:efe80c5db389 23 // Creabot Class
sepp_nepp 5:efe80c5db389 24 // *****************************************************************
sepp_nepp 5:efe80c5db389 25
sepp_nepp 10:79509113310a 26 Creabot::Creabot(CreaMot *left, CreaMot *right, float diameter_wheel_cm, float bot_diam_cm):
sepp_nepp 10:79509113310a 27 wheelLeft(left), wheelRight(right)
garphil 0:a7fb03c9ea9d 28 {
sepp_nepp 9:efe9f76d6f76 29 // TODO: To catch for illegal dimensions (<= 0)
sepp_nepp 9:efe9f76d6f76 30
sepp_nepp 9:efe9f76d6f76 31 // Initialize wheel geometires
sepp_nepp 11:5a94af0afa12 32 wheelLeft ->setDiamCM( diameter_wheel_cm );
sepp_nepp 11:5a94af0afa12 33 wheelRight->setDiamCM( diameter_wheel_cm );
sepp_nepp 9:efe9f76d6f76 34 // setup the bot parameters:
sepp_nepp 10:79509113310a 35 botDiameter = bot_diam_cm;
sepp_nepp 10:79509113310a 36 ratio_wheel_bot = bot_diam_cm/diameter_wheel_cm;
sepp_nepp 9:efe9f76d6f76 37 botSpeed_cm_sec = DEFAULT_SPEED_CM_SEC;
garphil 0:a7fb03c9ea9d 38
sepp_nepp 9:efe9f76d6f76 39 // Attach the Crabot functions as callbacks to the wheels
sepp_nepp 9:efe9f76d6f76 40 wheelLeft ->callbackSet(this, &Creabot::wheelLeftStoppedCB);
sepp_nepp 9:efe9f76d6f76 41 wheelRight->callbackSet(this, &Creabot::wheelRightStoppedCB);
sepp_nepp 6:4d8938b686a6 42 // Initialize own callback as empty
garphil 0:a7fb03c9ea9d 43 extCallBack = NULL;
sepp_nepp 6:4d8938b686a6 44
sepp_nepp 9:efe9f76d6f76 45 // setup the default Directions and State Values:
sepp_nepp 9:efe9f76d6f76 46 // used below as variable instead of always passing a direction
sepp_nepp 9:efe9f76d6f76 47 wheelLeft ->defaultDirection = COUNTERCLOCKWISE;
sepp_nepp 9:efe9f76d6f76 48 wheelLeft ->StateValue = LWHEEL_RUNS;
sepp_nepp 9:efe9f76d6f76 49 wheelRight->defaultDirection = CLOCKWISE;
sepp_nepp 9:efe9f76d6f76 50 wheelRight->StateValue = RWHEEL_RUNS;
sepp_nepp 9:efe9f76d6f76 51
sepp_nepp 6:4d8938b686a6 52 // Initialize the own status fields
sepp_nepp 9:efe9f76d6f76 53 wheelsState = LRWHEELS_STOP;
sepp_nepp 9:efe9f76d6f76 54 qReset();
garphil 0:a7fb03c9ea9d 55 }
garphil 0:a7fb03c9ea9d 56
sepp_nepp 9:efe9f76d6f76 57
sepp_nepp 9:efe9f76d6f76 58 // *****************************************************************
sepp_nepp 9:efe9f76d6f76 59 // Queue Queue management
sepp_nepp 9:efe9f76d6f76 60 // *****************************************************************
sepp_nepp 9:efe9f76d6f76 61
sepp_nepp 9:efe9f76d6f76 62 void Creabot::qMove(BotCmdVerb Acommand, float Aparam)
garphil 0:a7fb03c9ea9d 63 {
sepp_nepp 9:efe9f76d6f76 64 qMove(Acommand, Aparam, Aparam);
sepp_nepp 5:efe80c5db389 65 }
garphil 0:a7fb03c9ea9d 66
sepp_nepp 9:efe9f76d6f76 67 /* Add a new movement to the queue, if not full.
sepp_nepp 9:efe9f76d6f76 68 * ATTENTION:
sepp_nepp 9:efe9f76d6f76 69 * qExecuteNext() is called asynchronsously by the motor Callback wheelsAllStoppedCB()
sepp_nepp 9:efe9f76d6f76 70 * qExecuteNext() tries to read access the next command in the queue.
sepp_nepp 9:efe9f76d6f76 71 * A simultaneous read and write access to the same resources can lead to
sepp_nepp 9:efe9f76d6f76 72 * conflict and must be carefully avoided.
sepp_nepp 9:efe9f76d6f76 73 * This conflict concerns the read-modify-write access of variable Count.
sepp_nepp 9:efe9f76d6f76 74 * It is avoided by using the qBlock variable. With qBlock set during increment
sepp_nepp 9:efe9f76d6f76 75 * a simultaneous read access is blocked
sepp_nepp 9:efe9f76d6f76 76 * Any changes to those write and read handlers must take into account that
sepp_nepp 9:efe9f76d6f76 77 * wheelsAllStoppedCB() and qExecuteNext() may be called any time while
sepp_nepp 9:efe9f76d6f76 78 * executing qMove command.
sepp_nepp 9:efe9f76d6f76 79 */
sepp_nepp 9:efe9f76d6f76 80 void Creabot::qMove(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm)
sepp_nepp 9:efe9f76d6f76 81 { if( !qIsFull() )
sepp_nepp 9:efe9f76d6f76 82 { cmd[writeIdx].set(Acommand, Aturn_angle_deg, Adist_cm);
sepp_nepp 9:efe9f76d6f76 83 writeIdx++;
sepp_nepp 9:efe9f76d6f76 84 if (writeIdx==DEPTH_Queue) writeIdx=0;
sepp_nepp 9:efe9f76d6f76 85 qCollide = false; // colliding access to qCollide cannot happen while qBlock = false
sepp_nepp 9:efe9f76d6f76 86 // if callback happens here, there is no collosion, the last old command gets already executed.
sepp_nepp 9:efe9f76d6f76 87 qBlock = true;
sepp_nepp 9:efe9f76d6f76 88 // if callback happens here while qBlock = true, the collision is remembered in qCollide
sepp_nepp 9:efe9f76d6f76 89 Count++;
sepp_nepp 9:efe9f76d6f76 90 qBlock = false;
sepp_nepp 9:efe9f76d6f76 91 // if callback happens here there is no more blockage and the next (maybe the new) command is already being executed
sepp_nepp 9:efe9f76d6f76 92 // in this case qCollide = false, and wheelsState!=LRWHEELS_STOP
sepp_nepp 9:efe9f76d6f76 93 if ( qCollide || (wheelsState==LRWHEELS_STOP) )
sepp_nepp 9:efe9f76d6f76 94 // callback cannot happen here since the wheelState is already STOP, or Collide has happened before!
sepp_nepp 10:79509113310a 95 qExecuteNext();
sepp_nepp 9:efe9f76d6f76 96 };
sepp_nepp 5:efe80c5db389 97 }
sepp_nepp 5:efe80c5db389 98
sepp_nepp 9:efe9f76d6f76 99
sepp_nepp 9:efe9f76d6f76 100 // Queue is worked through while still any elements inside
sepp_nepp 9:efe9f76d6f76 101 // ATTENTION read above consideration of conflicting accesses between qMove() and wheelsAllStoppedCB()
sepp_nepp 9:efe9f76d6f76 102 // Think careful if a case can occur that pExecuteNext is called twice immediately following each other,
sepp_nepp 9:efe9f76d6f76 103 // once by qMove() and once by wheelsAllStoppedCB() ???
sepp_nepp 9:efe9f76d6f76 104 void Creabot::qExecuteNext()
sepp_nepp 9:efe9f76d6f76 105 { if ( !qIsEmpty() ) // see if something in the queue to execute
sepp_nepp 9:efe9f76d6f76 106 { // Execute and then remove the oldest element at the tail of the Queue
sepp_nepp 10:79509113310a 107 iExeCommand( &cmd[readIdx] );
sepp_nepp 9:efe9f76d6f76 108 readIdx++;
sepp_nepp 9:efe9f76d6f76 109 if (readIdx==DEPTH_Queue) readIdx=0;
sepp_nepp 9:efe9f76d6f76 110 Count--;
sepp_nepp 9:efe9f76d6f76 111 };
sepp_nepp 5:efe80c5db389 112 }
sepp_nepp 5:efe80c5db389 113
sepp_nepp 9:efe9f76d6f76 114 // Immediately end All, the queue and the motor
sepp_nepp 9:efe9f76d6f76 115 void Creabot::qStopAll()
sepp_nepp 9:efe9f76d6f76 116 { qReset(); // empty the queue
sepp_nepp 9:efe9f76d6f76 117 iStop(); // stop the motors, do not call external callback
sepp_nepp 9:efe9f76d6f76 118 }
sepp_nepp 9:efe9f76d6f76 119
sepp_nepp 5:efe80c5db389 120
sepp_nepp 7:3a793ddc3490 121 //**************************************************************************
sepp_nepp 7:3a793ddc3490 122 // Using the BotCmdVerb to encode different commands, done immediately
sepp_nepp 7:3a793ddc3490 123 //**************************************************************************
sepp_nepp 7:3a793ddc3490 124
sepp_nepp 9:efe9f76d6f76 125 void Creabot::iMoveAndWait(BotCmdVerb moveType, float Aparam)
sepp_nepp 10:79509113310a 126 { iMove(moveType,Aparam,Aparam);
sepp_nepp 10:79509113310a 127 iWaitEnd();
sepp_nepp 5:efe80c5db389 128 }
sepp_nepp 5:efe80c5db389 129
sepp_nepp 9:efe9f76d6f76 130 void Creabot::iMoveAndWait(BotCmdVerb moveType, float Aturn_angle_deg, float Adist_cm)
sepp_nepp 10:79509113310a 131 { iMove(moveType,Aturn_angle_deg,Adist_cm);
sepp_nepp 10:79509113310a 132 iWaitEnd();
sepp_nepp 5:efe80c5db389 133 }
sepp_nepp 5:efe80c5db389 134
sepp_nepp 9:efe9f76d6f76 135 void Creabot::iMove(BotCmdVerb moveType, float Aparam)
sepp_nepp 10:79509113310a 136 { iMove(moveType, Aparam, Aparam); }
sepp_nepp 7:3a793ddc3490 137
sepp_nepp 9:efe9f76d6f76 138 void Creabot::iMove(BotCmdVerb moveType, float Aturn_angle_deg, float Adist_cm)
sepp_nepp 7:3a793ddc3490 139 { BotCommand Anew_cmd;
sepp_nepp 9:efe9f76d6f76 140 Anew_cmd.set(moveType, Aturn_angle_deg, Adist_cm);
sepp_nepp 10:79509113310a 141 iExeCommand(&Anew_cmd);
sepp_nepp 7:3a793ddc3490 142 }
sepp_nepp 7:3a793ddc3490 143
sepp_nepp 9:efe9f76d6f76 144 /** High level, immediate: move bot according to prefilled command structures.
sepp_nepp 10:79509113310a 145 * Recommended to use iMove() methods to fill the command structure correctly.
sepp_nepp 9:efe9f76d6f76 146 * Branches to the moveXXXX() methods. For details see docs for those methods.
sepp_nepp 9:efe9f76d6f76 147 * Warning: Collides with queued commands if called individually.
sepp_nepp 9:efe9f76d6f76 148 * @param[in] <*cmd> Pointer to type BotCommand, the prefilled command structure,*/
sepp_nepp 9:efe9f76d6f76 149 void Creabot::iExeCommand(BotCommand *cmd) {
garphil 0:a7fb03c9ea9d 150 switch (cmd->command) {
garphil 0:a7fb03c9ea9d 151 case FORWARD:
sepp_nepp 6:4d8938b686a6 152 moveForward(cmd->dist_cm);
garphil 0:a7fb03c9ea9d 153 break;
garphil 0:a7fb03c9ea9d 154 case BACKWARD:
sepp_nepp 6:4d8938b686a6 155 moveBackward(cmd->dist_cm);
garphil 0:a7fb03c9ea9d 156 break;
garphil 0:a7fb03c9ea9d 157 case LEFT:
sepp_nepp 9:efe9f76d6f76 158 moveLeft(cmd->turn_angle_deg, cmd->dist_cm);
garphil 0:a7fb03c9ea9d 159 break;
garphil 0:a7fb03c9ea9d 160 case RIGHT:
sepp_nepp 9:efe9f76d6f76 161 moveRight(cmd->turn_angle_deg, cmd->dist_cm);
sepp_nepp 9:efe9f76d6f76 162 break;
sepp_nepp 9:efe9f76d6f76 163 case BACKLEFT:
sepp_nepp 9:efe9f76d6f76 164 moveBackLeft(cmd->turn_angle_deg, cmd->dist_cm);
sepp_nepp 9:efe9f76d6f76 165 break;
sepp_nepp 9:efe9f76d6f76 166 case BACKRIGHT:
sepp_nepp 9:efe9f76d6f76 167 moveBackRight(cmd->turn_angle_deg, cmd->dist_cm);
sepp_nepp 9:efe9f76d6f76 168 break;
sepp_nepp 9:efe9f76d6f76 169 case ROTATE:
sepp_nepp 9:efe9f76d6f76 170 moveRotate(cmd->turn_angle_deg);
garphil 0:a7fb03c9ea9d 171 break;
garphil 0:a7fb03c9ea9d 172 default:
garphil 0:a7fb03c9ea9d 173 break;
garphil 0:a7fb03c9ea9d 174 };
sepp_nepp 5:efe80c5db389 175 }; // executeCommand
sepp_nepp 5:efe80c5db389 176
sepp_nepp 9:efe9f76d6f76 177 /* spends all time with waits, returns only once wheelState = LRWHEELS_STOP */
sepp_nepp 9:efe9f76d6f76 178 void Creabot::iWaitEnd()
sepp_nepp 9:efe9f76d6f76 179 { while(wheelsState!=LRWHEELS_STOP) wait(0.1); }
sepp_nepp 6:4d8938b686a6 180
sepp_nepp 9:efe9f76d6f76 181 /* spends all time with waits, returns only once wheelState = LRWHEELS_STOP,
sepp_nepp 9:efe9f76d6f76 182 but maximum max_wait_ms milli seconds */
sepp_nepp 9:efe9f76d6f76 183 void Creabot::iWaitEnd(uint32_t max_wait_ms)
sepp_nepp 9:efe9f76d6f76 184 { uint32_t waited=0;
sepp_nepp 9:efe9f76d6f76 185 while( (wheelsState!=LRWHEELS_STOP) && (waited < max_wait_ms) )
sepp_nepp 9:efe9f76d6f76 186 { wait(0.1); waited += 100; }
sepp_nepp 9:efe9f76d6f76 187 } // botWaitEnd
garphil 0:a7fb03c9ea9d 188
sepp_nepp 9:efe9f76d6f76 189 void Creabot::iStop()
sepp_nepp 9:efe9f76d6f76 190 { // Call the StopRun method, so wheels detach their tickers
sepp_nepp 9:efe9f76d6f76 191 wheelLeft->StopRun();
sepp_nepp 9:efe9f76d6f76 192 wheelRight->StopRun();
sepp_nepp 9:efe9f76d6f76 193 // wheelsAllStoppedCB could handle the rest of the cleanup, but would also trigger external callback
sepp_nepp 9:efe9f76d6f76 194 // Turn motor power off!
sepp_nepp 10:79509113310a 195 wheelLeft->MotorOFF();
sepp_nepp 10:79509113310a 196 wheelRight->MotorOFF();
sepp_nepp 10:79509113310a 197 wheelsState = LRWHEELS_STOP;
sepp_nepp 9:efe9f76d6f76 198 }
sepp_nepp 7:3a793ddc3490 199
sepp_nepp 7:3a793ddc3490 200 // *****************************************************************
sepp_nepp 7:3a793ddc3490 201 // Creabot Mid level control functions:
sepp_nepp 9:efe9f76d6f76 202 // all directly access the wheel, no Queue */
sepp_nepp 7:3a793ddc3490 203 // *****************************************************************
sepp_nepp 7:3a793ddc3490 204
sepp_nepp 9:efe9f76d6f76 205 // set botSpeed_cm_sec parameter for next high level commands
sepp_nepp 9:efe9f76d6f76 206 void Creabot::setSpeed(float AbotSpeed_cm_sec)
sepp_nepp 9:efe9f76d6f76 207 { if (AbotSpeed_cm_sec > MAX_SPEED_CM_SEC) botSpeed_cm_sec = MAX_SPEED_CM_SEC;
sepp_nepp 9:efe9f76d6f76 208 else if (AbotSpeed_cm_sec<MIN_SPEED_CM_SEC) botSpeed_cm_sec = MIN_SPEED_CM_SEC;
sepp_nepp 9:efe9f76d6f76 209 else botSpeed_cm_sec = AbotSpeed_cm_sec;
sepp_nepp 9:efe9f76d6f76 210 // Low level setting of wheel speed is done inside SetMotsSpeed:
sepp_nepp 7:3a793ddc3490 211 }
garphil 0:a7fb03c9ea9d 212
sepp_nepp 9:efe9f76d6f76 213 // Mid level control function: advance bot straight forward for a given distance
sepp_nepp 7:3a793ddc3490 214 void Creabot::moveForward(float dist_cm)
sepp_nepp 10:79509113310a 215 {
sepp_nepp 10:79509113310a 216 wheelsSetSpeed(botSpeed_cm_sec);
sepp_nepp 10:79509113310a 217 wheelRight->RunDist_cm(dist_cm );
sepp_nepp 10:79509113310a 218 wheelLeft ->RunDist_cm(dist_cm ); // Direction = defaultDirection
sepp_nepp 10:79509113310a 219 // negative distances are run by RunDist_cm in opposite direction!
sepp_nepp 10:79509113310a 220 wheelsState = LRWHEELS_RUN;
sepp_nepp 7:3a793ddc3490 221 }
sepp_nepp 7:3a793ddc3490 222
sepp_nepp 9:efe9f76d6f76 223 // Mid level control function: reverse bot straight backwards for a given distance
sepp_nepp 7:3a793ddc3490 224 void Creabot::moveBackward(float dist_cm)
sepp_nepp 7:3a793ddc3490 225 {
sepp_nepp 10:79509113310a 226 wheelsSetSpeed(botSpeed_cm_sec);
sepp_nepp 10:79509113310a 227 wheelRight->RunDist_cm(-dist_cm);
sepp_nepp 10:79509113310a 228 wheelLeft ->RunDist_cm(-dist_cm);// Direction = defaultDirection
sepp_nepp 10:79509113310a 229 // negative distance are run by RunDist_cm in opposite direction!
sepp_nepp 10:79509113310a 230 wheelsState = LRWHEELS_RUN;
sepp_nepp 7:3a793ddc3490 231 }
sepp_nepp 7:3a793ddc3490 232
sepp_nepp 7:3a793ddc3490 233 /* Mid level control function: turn bot forward right,
sepp_nepp 10:79509113310a 234 around a radius equl the bot size; i.e. turn around the right wheel */
sepp_nepp 9:efe9f76d6f76 235 void Creabot::moveRight(float turn_angle_deg)
sepp_nepp 5:efe80c5db389 236 {
sepp_nepp 9:efe9f76d6f76 237 wheelLeft->setSpeed_cm_sec(botSpeed_cm_sec);
sepp_nepp 10:79509113310a 238 wheelLeft->RunTurnAngle(turn_angle_deg,botDiameter);
sepp_nepp 10:79509113310a 239 wheelsState = LWHEEL_RUNS;
garphil 0:a7fb03c9ea9d 240 }
garphil 0:a7fb03c9ea9d 241
sepp_nepp 7:3a793ddc3490 242 /* Mid level control function: turn bot forward right,
sepp_nepp 10:79509113310a 243 around a radius that is turn_radius_cm away from the right wheel*/
sepp_nepp 9:efe9f76d6f76 244 void Creabot::moveRight(float turn_angle_deg, float turn_radius_cm)
sepp_nepp 10:79509113310a 245 { // If Center_cm is too small the right wheel has practically nothing to do ;)
sepp_nepp 11:5a94af0afa12 246 if (fabs(double(turn_radius_cm))<0.1) { moveRight(turn_angle_deg); }
sepp_nepp 10:79509113310a 247 else {
sepp_nepp 10:79509113310a 248 wheelLeft ->setSpeed_cm_sec(botSpeed_cm_sec);
sepp_nepp 10:79509113310a 249 wheelRight->setSpeed_cm_sec(botSpeed_cm_sec * turn_radius_cm/turn_radius_cm+botDiameter);
sepp_nepp 10:79509113310a 250 wheelRight->RunTurnAngle(turn_angle_deg,turn_radius_cm );
sepp_nepp 10:79509113310a 251 wheelLeft ->RunTurnAngle(turn_angle_deg,turn_radius_cm+botDiameter);// Direction = defaultDirection
sepp_nepp 10:79509113310a 252 // negative distance are run by RunTurnAngle in opposite direction!
sepp_nepp 10:79509113310a 253 wheelsState = LRWHEELS_RUN;
sepp_nepp 10:79509113310a 254 } // else
sepp_nepp 5:efe80c5db389 255 }
sepp_nepp 5:efe80c5db389 256
sepp_nepp 10:79509113310a 257 /* Mid level control function: turn bot backwards Right,
sepp_nepp 10:79509113310a 258 around a radius that is turn_radius_cm away from the Right wheel */
sepp_nepp 10:79509113310a 259 void Creabot::moveBackRight(float turn_angle_deg, float turn_radius_cm)
sepp_nepp 10:79509113310a 260 { moveRight( -turn_angle_deg , turn_radius_cm); }
sepp_nepp 10:79509113310a 261
sepp_nepp 10:79509113310a 262 /* Mid level control function: turn bot backwards Right,
sepp_nepp 10:79509113310a 263 around a radius equal to the bot size; = the Right wheel stands still */
sepp_nepp 10:79509113310a 264 void Creabot::moveBackRight(float turn_angle_deg)
sepp_nepp 10:79509113310a 265 { moveRight( - turn_angle_deg); }
sepp_nepp 9:efe9f76d6f76 266
sepp_nepp 7:3a793ddc3490 267 /* Mid level control function: turn bot forward left,
sepp_nepp 10:79509113310a 268 around a radius equal to the bot size; = the left wheel stands still */
sepp_nepp 9:efe9f76d6f76 269 void Creabot::moveLeft(float turn_angle_deg)
sepp_nepp 5:efe80c5db389 270 {
sepp_nepp 11:5a94af0afa12 271 wheelRight->setSpeed_cm_sec(botSpeed_cm_sec);
sepp_nepp 10:79509113310a 272 wheelRight->RunTurnAngle(turn_angle_deg,botDiameter);
sepp_nepp 10:79509113310a 273 wheelsState = RWHEEL_RUNS;
garphil 4:531b1120d3ec 274 }
garphil 4:531b1120d3ec 275
sepp_nepp 7:3a793ddc3490 276 /* Mid level control function: turn bot forward left,
sepp_nepp 10:79509113310a 277 around a radius that is turn_radius_cm away from the left wheel */
sepp_nepp 10:79509113310a 278 void Creabot::moveLeft(float turn_angle_deg, float turn_radius_cm)
sepp_nepp 10:79509113310a 279 { // If Center_cm is too small the left wheel has practically nothing to do ;)
sepp_nepp 11:5a94af0afa12 280 if (fabs(double(turn_radius_cm))<0.1) { moveLeft(turn_angle_deg); }
sepp_nepp 10:79509113310a 281 else {
sepp_nepp 10:79509113310a 282 wheelLeft -> setSpeed_cm_sec(botSpeed_cm_sec * turn_radius_cm/(turn_radius_cm+botDiameter));
sepp_nepp 10:79509113310a 283 wheelRight-> setSpeed_cm_sec(botSpeed_cm_sec);
sepp_nepp 10:79509113310a 284 wheelLeft -> RunTurnAngle(turn_angle_deg,turn_radius_cm);
sepp_nepp 10:79509113310a 285 wheelRight-> RunTurnAngle(turn_angle_deg,turn_radius_cm+botDiameter);// Direction = defaultDirection
sepp_nepp 10:79509113310a 286 // negative distance are run by RunTurnAngle in opposite direction!
sepp_nepp 10:79509113310a 287 wheelsState = LRWHEELS_RUN;
sepp_nepp 10:79509113310a 288 }
garphil 0:a7fb03c9ea9d 289 }
garphil 0:a7fb03c9ea9d 290
sepp_nepp 10:79509113310a 291 /* Mid level control function: turn bot backwards left,
sepp_nepp 10:79509113310a 292 around a radius that is turn_radius_cm away from the left wheel */
sepp_nepp 10:79509113310a 293 void Creabot::moveBackLeft(float turn_angle_deg, float turn_radius_cm)
sepp_nepp 10:79509113310a 294 { moveLeft( - turn_angle_deg, turn_radius_cm); }
sepp_nepp 10:79509113310a 295
sepp_nepp 10:79509113310a 296 /* Mid level control function: turn bot backwards left,
sepp_nepp 10:79509113310a 297 around a radius equal to the bot size; = the left wheel stands still */
sepp_nepp 10:79509113310a 298 void Creabot::moveBackLeft(float turn_angle_deg)
sepp_nepp 10:79509113310a 299 { moveLeft( - turn_angle_deg); }
sepp_nepp 10:79509113310a 300
sepp_nepp 7:3a793ddc3490 301 /* Mid level control function: turn bot on its place for a given angle.
sepp_nepp 7:3a793ddc3490 302 positive angles turn right, negative angles turn left*/
sepp_nepp 9:efe9f76d6f76 303 void Creabot::moveRotate(float angleBot)
garphil 0:a7fb03c9ea9d 304 {
sepp_nepp 7:3a793ddc3490 305 float angleWheel = angleBot*ratio_wheel_bot;
sepp_nepp 9:efe9f76d6f76 306 setSpeed(botSpeed_cm_sec);
sepp_nepp 7:3a793ddc3490 307 if(angleBot<0) {
sepp_nepp 10:79509113310a 308 wheelRight->RunDegrees( angleWheel);
sepp_nepp 10:79509113310a 309 wheelLeft ->RunDegrees(-angleWheel);// Direction = defaultDirection
sepp_nepp 10:79509113310a 310 // negative distance are run by RunDegrees in opposite direction!
sepp_nepp 7:3a793ddc3490 311 } else {
sepp_nepp 10:79509113310a 312 wheelLeft ->RunDegrees( angleWheel);
sepp_nepp 10:79509113310a 313 wheelRight->RunDegrees(-angleWheel);// Direction = defaultDirection
sepp_nepp 10:79509113310a 314 // negative distance are run by RunDegrees in opposite direction!
sepp_nepp 6:4d8938b686a6 315 }
sepp_nepp 10:79509113310a 316 wheelsState = LRWHEELS_RUN;
sepp_nepp 5:efe80c5db389 317 }
sepp_nepp 5:efe80c5db389 318
sepp_nepp 7:3a793ddc3490 319 //******************************************************************************
sepp_nepp 9:efe9f76d6f76 320 // low level wheel functions, only filters for rot_angle_deg>0, then passes
sepp_nepp 9:efe9f76d6f76 321 // to wheel Class; and remembers that a wheel moves
sepp_nepp 7:3a793ddc3490 322 //******************************************************************************
sepp_nepp 7:3a793ddc3490 323
sepp_nepp 9:efe9f76d6f76 324 /* Low level access functions: sets both wheel speeds immediately */
sepp_nepp 9:efe9f76d6f76 325 void Creabot::wheelsSetSpeed(float mot_speed_cm_sec)
sepp_nepp 5:efe80c5db389 326 {
sepp_nepp 10:79509113310a 327 wheelLeft ->setSpeed_cm_sec(mot_speed_cm_sec);
sepp_nepp 9:efe9f76d6f76 328 wheelRight->setSpeed_cm_sec(mot_speed_cm_sec);
sepp_nepp 5:efe80c5db389 329 }
sepp_nepp 5:efe80c5db389 330
sepp_nepp 7:3a793ddc3490 331 //******************************************************************************
sepp_nepp 9:efe9f76d6f76 332 // low level wheel functions, handles callbacks from wheels
sepp_nepp 7:3a793ddc3490 333 // *****************************************************************************
sepp_nepp 9:efe9f76d6f76 334
sepp_nepp 9:efe9f76d6f76 335 void Creabot::wheelLeftStoppedCB()
sepp_nepp 11:5a94af0afa12 336 { wheelsState = TWheelsState( wheelsState & LWHEEL_RUNS ); // mask out Left Wheel status bit
sepp_nepp 9:efe9f76d6f76 337 if (wheelsState == LRWHEELS_STOP) // all wheels are now known to have stopped
sepp_nepp 9:efe9f76d6f76 338 wheelsAllStoppedCB();
sepp_nepp 6:4d8938b686a6 339 }
sepp_nepp 6:4d8938b686a6 340
sepp_nepp 9:efe9f76d6f76 341 void Creabot::wheelRightStoppedCB()
sepp_nepp 11:5a94af0afa12 342 { wheelsState = TWheelsState( wheelsState & RWHEEL_RUNS ); // mask out Left Wheel status bit
sepp_nepp 9:efe9f76d6f76 343 if (wheelsState == LRWHEELS_STOP) // all wheels are now known to have stopped
sepp_nepp 9:efe9f76d6f76 344 wheelsAllStoppedCB();
sepp_nepp 7:3a793ddc3490 345 }
sepp_nepp 7:3a793ddc3490 346
sepp_nepp 9:efe9f76d6f76 347 void Creabot::wheelsAllStoppedCB()
sepp_nepp 9:efe9f76d6f76 348 { // assume that already at entry wheelsState == LRWHEELS_STOP
sepp_nepp 9:efe9f76d6f76 349 // Try to execute next entry in queue, if one is present
sepp_nepp 9:efe9f76d6f76 350 if ( !qBlock )
sepp_nepp 9:efe9f76d6f76 351 { qExecuteNext();// not blocked, so can execute now
sepp_nepp 9:efe9f76d6f76 352 // if wheels are still STOPed, that means no command to execute was left in the queue
sepp_nepp 9:efe9f76d6f76 353 if (wheelsState==LRWHEELS_STOP)
sepp_nepp 9:efe9f76d6f76 354 { // Turn motor power off!
sepp_nepp 11:5a94af0afa12 355 wheelLeft->MotorOFF();
sepp_nepp 11:5a94af0afa12 356 wheelRight->MotorOFF();
sepp_nepp 9:efe9f76d6f76 357 // Call the externall callback function to inform about the end of all movements
sepp_nepp 9:efe9f76d6f76 358 if(extCallBack!=NULL) extCallBack( wheelsState );
sepp_nepp 9:efe9f76d6f76 359 } // if wheelsState==LRWHEELS_STOP
sepp_nepp 9:efe9f76d6f76 360 } // if ( !qBlock )
sepp_nepp 9:efe9f76d6f76 361 else qCollide = true; // Remember the collision to execute the next command.
sepp_nepp 6:4d8938b686a6 362 }
sepp_nepp 6:4d8938b686a6 363
sepp_nepp 7:3a793ddc3490 364
sepp_nepp 7:3a793ddc3490 365