Tarek Lule / CreaBotLib

My Version of CreaBotLib

Fork of CreaBotLib by CreaLab

CreaBot.cpp@9:efe9f76d6f76, 2019-01-03 (annotated)

Committer:
sepp_nepp
Date:
Thu Jan 03 23:16:46 2019 +0000
Revision:
9:efe9f76d6f76
Parent:
7:3a793ddc3490
Child:
10:79509113310a
Publish all those major refactorings done on this library. ; In alpha state, for code review. No debug done so far.

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 9:efe9f76d6f76 26 Creabot::Creabot(motor *left, motor *right, float diameter_wheel_cm, float bot_diam_cm):
sepp_nepp 9:efe9f76d6f76 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 9:efe9f76d6f76 32 wheelLeft.setDiam(wheel_diam_cm);
sepp_nepp 9:efe9f76d6f76 33 wheelRight.setDiam(wheel_diam_cm);
sepp_nepp 9:efe9f76d6f76 34 // setup the bot parameters:
sepp_nepp 9:efe9f76d6f76 35 botDiameter = bot_diam_cm;
sepp_nepp 7:3a793ddc3490 36 ratio_wheel_bot=bot_diam_cm/wheel_diam_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 9:efe9f76d6f76 95 pExecuteNext();
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 9:efe9f76d6f76 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 9:efe9f76d6f76 126 { botMove(moveType,Aparam);
sepp_nepp 9:efe9f76d6f76 127 botWaitEnd();
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 9:efe9f76d6f76 131 { botMove(moveType,Aturn_angle_deg,Adist_cm);
sepp_nepp 9:efe9f76d6f76 132 botWaitEnd();
sepp_nepp 5:efe80c5db389 133 }
sepp_nepp 5:efe80c5db389 134
sepp_nepp 9:efe9f76d6f76 135 void Creabot::iMove(BotCmdVerb moveType, float Aparam)
sepp_nepp 9:efe9f76d6f76 136 { botMove(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 7:3a793ddc3490 141 executeCommand(&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 9:efe9f76d6f76 145 * Recommended to use botMove() 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 case IDLE:
garphil 0:a7fb03c9ea9d 173 default:
garphil 0:a7fb03c9ea9d 174 break;
garphil 0:a7fb03c9ea9d 175 };
sepp_nepp 5:efe80c5db389 176 }; // executeCommand
sepp_nepp 5:efe80c5db389 177
sepp_nepp 9:efe9f76d6f76 178 /* spends all time with waits, returns only once wheelState = LRWHEELS_STOP */
sepp_nepp 9:efe9f76d6f76 179 void Creabot::iWaitEnd()
sepp_nepp 9:efe9f76d6f76 180 { while(wheelsState!=LRWHEELS_STOP) wait(0.1); }
sepp_nepp 6:4d8938b686a6 181
sepp_nepp 9:efe9f76d6f76 182 /* spends all time with waits, returns only once wheelState = LRWHEELS_STOP,
sepp_nepp 9:efe9f76d6f76 183 but maximum max_wait_ms milli seconds */
sepp_nepp 9:efe9f76d6f76 184 void Creabot::iWaitEnd(uint32_t max_wait_ms)
sepp_nepp 9:efe9f76d6f76 185 { uint32_t waited=0;
sepp_nepp 9:efe9f76d6f76 186 while( (wheelsState!=LRWHEELS_STOP) && (waited < max_wait_ms) )
sepp_nepp 9:efe9f76d6f76 187 { wait(0.1); waited += 100; }
sepp_nepp 9:efe9f76d6f76 188 } // botWaitEnd
garphil 0:a7fb03c9ea9d 189
sepp_nepp 9:efe9f76d6f76 190 void Creabot::iStop()
sepp_nepp 9:efe9f76d6f76 191 { // Call the StopRun method, so wheels detach their tickers
sepp_nepp 9:efe9f76d6f76 192 wheelLeft->StopRun();
sepp_nepp 9:efe9f76d6f76 193 wheelRight->StopRun();
sepp_nepp 9:efe9f76d6f76 194 // wheelsAllStoppedCB could handle the rest of the cleanup, but would also trigger external callback
sepp_nepp 9:efe9f76d6f76 195 // Turn motor power off!
sepp_nepp 9:efe9f76d6f76 196 wheelLeft->WheelOFF();
sepp_nepp 9:efe9f76d6f76 197 wheelRight->WheelOFF();
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 9:efe9f76d6f76 215 { if (dist_cm<0) moveBackward(-dist_cm);
sepp_nepp 7:3a793ddc3490 216 else {
sepp_nepp 9:efe9f76d6f76 217 float rot_angle_deg = dist_cm*wheeldims.degree_per_cm;
sepp_nepp 9:efe9f76d6f76 218 wheelsSetSpeed(botSpeed_cm_sec);
sepp_nepp 9:efe9f76d6f76 219 wheelLeftRun (rot_angle_deg);
sepp_nepp 9:efe9f76d6f76 220 wheelRightRun(rot_angle_deg);
sepp_nepp 7:3a793ddc3490 221 }
sepp_nepp 7:3a793ddc3490 222 }
sepp_nepp 7:3a793ddc3490 223
sepp_nepp 9:efe9f76d6f76 224 // Mid level control function: reverse bot straight backwards for a given distance
sepp_nepp 7:3a793ddc3490 225 void Creabot::moveBackward(float dist_cm)
sepp_nepp 7:3a793ddc3490 226 {
sepp_nepp 7:3a793ddc3490 227 if(dist_cm<0) moveForward(-dist_cm);
sepp_nepp 7:3a793ddc3490 228 else {
sepp_nepp 9:efe9f76d6f76 229 float rot_angle_deg = dist_cm*wheeldims.degree_per_cm;
sepp_nepp 9:efe9f76d6f76 230 wheelsSetSpeed(botSpeed_cm_sec);
sepp_nepp 9:efe9f76d6f76 231 wheelLeftRun (-rot_angle_deg);
sepp_nepp 9:efe9f76d6f76 232 wheelRightRun(-rot_angle_deg);
sepp_nepp 7:3a793ddc3490 233 }
sepp_nepp 7:3a793ddc3490 234 }
sepp_nepp 7:3a793ddc3490 235
sepp_nepp 7:3a793ddc3490 236 /* Mid level control function: turn bot forward right,
sepp_nepp 9:efe9f76d6f76 237 around a radius twice the bot size */
sepp_nepp 9:efe9f76d6f76 238 void Creabot::moveRight(float turn_angle_deg)
sepp_nepp 5:efe80c5db389 239 {
sepp_nepp 9:efe9f76d6f76 240 wheelLeft->setSpeed_cm_sec(botSpeed_cm_sec);
sepp_nepp 9:efe9f76d6f76 241 wheelLeftRun(COUNTERCLOCKWISE, wheelLeft.calcAngle(turn_angle_deg,botDiameter));
garphil 0:a7fb03c9ea9d 242 }
garphil 0:a7fb03c9ea9d 243
sepp_nepp 7:3a793ddc3490 244 /* Mid level control function: turn bot forward right,
sepp_nepp 7:3a793ddc3490 245 around a radius that is center_cm away from the right wheel*/
sepp_nepp 9:efe9f76d6f76 246 void Creabot::moveRight(float turn_angle_deg, float turn_radius_cm)
sepp_nepp 9:efe9f76d6f76 247 { float wheelAngLeft = wheelLeft.calcAngle(turn_angle_deg,turn_radius_cm+botDiameter);
sepp_nepp 9:efe9f76d6f76 248 float wheelAngRight = wheelRight.calcAngle(turn_angle_deg,turn_radius_cm);
sepp_nepp 9:efe9f76d6f76 249 wheelLeft->setSpeed_cm_sec(botSpeed_cm_sec);
sepp_nepp 9:efe9f76d6f76 250 wheelRight->setSpeed_cm_sec(botSpeed_cm_sec * wheelAngRight/wheelAngLeft);
sepp_nepp 9:efe9f76d6f76 251 wheelLeftRun(COUNTERCLOCKWISE, angleLeft);
sepp_nepp 9:efe9f76d6f76 252 wheelRightRun(CLOCKWISE, angleRight);
sepp_nepp 5:efe80c5db389 253 }
sepp_nepp 5:efe80c5db389 254
sepp_nepp 9:efe9f76d6f76 255
sepp_nepp 7:3a793ddc3490 256 /* Mid level control function: turn bot forward left,
sepp_nepp 7:3a793ddc3490 257 around a radius twice the bot size */
sepp_nepp 9:efe9f76d6f76 258 void Creabot::moveLeft(float turn_angle_deg)
sepp_nepp 5:efe80c5db389 259 {
sepp_nepp 9:efe9f76d6f76 260 moveLeft(turn_angle_deg,0);
garphil 4:531b1120d3ec 261 }
garphil 4:531b1120d3ec 262
sepp_nepp 7:3a793ddc3490 263 /* Mid level control function: turn bot forward left,
sepp_nepp 7:3a793ddc3490 264 around a radius that is center_cm away from the left wheel*/
sepp_nepp 9:efe9f76d6f76 265 void Creabot::moveLeft(float turn_angle_deg, float center_cm)
garphil 0:a7fb03c9ea9d 266 {
sepp_nepp 7:3a793ddc3490 267 if (center_cm<0) center_cm=0; /* TO remove? */
sepp_nepp 9:efe9f76d6f76 268 float perimeter_outer = 2.0f*turn_angle_deg*(center_cm+botDiameter)*PI/360.0f;
sepp_nepp 9:efe9f76d6f76 269 float perimeter_inner = 2.0f*turn_angle_deg*(center_cm)*PI/360.0f;
sepp_nepp 9:efe9f76d6f76 270 float angleLeft = perimeter_inner*wheeldims.degree_per_cm;
sepp_nepp 9:efe9f76d6f76 271 float angleRight = perimeter_outer*wheeldims.degree_per_cm;
sepp_nepp 5:efe80c5db389 272 float ratio = angleLeft/angleRight;
sepp_nepp 9:efe9f76d6f76 273 float speedRLeft = botSpeed_cm_sec * ratio;
sepp_nepp 9:efe9f76d6f76 274 wheelLeft->setSpeed_cm_sec(speedRLeft);
sepp_nepp 9:efe9f76d6f76 275 wheelRight->setSpeed_cm_sec(botSpeed_cm_sec);
sepp_nepp 9:efe9f76d6f76 276 wheelLeftRun(COUNTERCLOCKWISE, angleLeft);
sepp_nepp 9:efe9f76d6f76 277 wheelRightRun(CLOCKWISE, angleRight);
garphil 0:a7fb03c9ea9d 278 }
garphil 0:a7fb03c9ea9d 279
sepp_nepp 7:3a793ddc3490 280 /* Mid level control function: turn bot on its place for a given angle.
sepp_nepp 7:3a793ddc3490 281 positive angles turn right, negative angles turn left*/
sepp_nepp 9:efe9f76d6f76 282 void Creabot::moveRotate(float angleBot)
garphil 0:a7fb03c9ea9d 283 {
sepp_nepp 7:3a793ddc3490 284 float angleWheel = angleBot*ratio_wheel_bot;
sepp_nepp 9:efe9f76d6f76 285 setSpeed(botSpeed_cm_sec);
sepp_nepp 7:3a793ddc3490 286 if(angleBot<0) {
sepp_nepp 9:efe9f76d6f76 287 wheelLeftRun(CLOCKWISE, -angleWheel);
sepp_nepp 9:efe9f76d6f76 288 wheelRightRun(CLOCKWISE, -angleWheel);
sepp_nepp 7:3a793ddc3490 289 } else {
sepp_nepp 9:efe9f76d6f76 290 wheelLeftRun(COUNTERCLOCKWISE, angleWheel);
sepp_nepp 9:efe9f76d6f76 291 wheelRightRun(COUNTERCLOCKWISE, angleWheel);
sepp_nepp 6:4d8938b686a6 292 }
sepp_nepp 5:efe80c5db389 293 }
sepp_nepp 5:efe80c5db389 294
sepp_nepp 7:3a793ddc3490 295 //******************************************************************************
sepp_nepp 9:efe9f76d6f76 296 // low level wheel functions, only filters for rot_angle_deg>0, then passes
sepp_nepp 9:efe9f76d6f76 297 // to wheel Class; and remembers that a wheel moves
sepp_nepp 7:3a793ddc3490 298 //******************************************************************************
sepp_nepp 7:3a793ddc3490 299
sepp_nepp 9:efe9f76d6f76 300 /* Low level access functions: sets both wheel speeds immediately */
sepp_nepp 9:efe9f76d6f76 301 void Creabot::wheelsSetSpeed(float mot_speed_cm_sec)
sepp_nepp 5:efe80c5db389 302 {
sepp_nepp 9:efe9f76d6f76 303 wheelLeft->setSpeed_cm_sec(mot_speed_cm_sec);
sepp_nepp 9:efe9f76d6f76 304 wheelRight->setSpeed_cm_sec(mot_speed_cm_sec);
sepp_nepp 5:efe80c5db389 305 }
sepp_nepp 5:efe80c5db389 306
sepp_nepp 7:3a793ddc3490 307
sepp_nepp 9:efe9f76d6f76 308 void Creabot::wheelLeftRun(float rot_angle_deg)
sepp_nepp 9:efe9f76d6f76 309 { if (rot_angle_deg>=0) wheelLeft->RunDegrees(wheelLeft->defaultDirection, rot_angle_deg);
sepp_nepp 9:efe9f76d6f76 310 else wheelLeft->RunDegrees(! wheelLeft->defaultDirection, -rot_angle_deg);
sepp_nepp 9:efe9f76d6f76 311
sepp_nepp 9:efe9f76d6f76 312 if ( (wheelsState==RWHEEL_RUNS) || (wheelsState==LRWHEELS_RUN) ) wheelsState=LRWHEELS_RUN;
sepp_nepp 9:efe9f76d6f76 313 else wheelsState=LWHEEL_RUNS;
sepp_nepp 7:3a793ddc3490 314 }
sepp_nepp 5:efe80c5db389 315 }
sepp_nepp 5:efe80c5db389 316
sepp_nepp 9:efe9f76d6f76 317 void Creabot::wheelRightRun(float rot_angle_deg)
sepp_nepp 9:efe9f76d6f76 318 { if(rot_angle_deg>0) {
sepp_nepp 9:efe9f76d6f76 319 wheelRight->RunDegrees(dir, rot_angle_deg);
sepp_nepp 9:efe9f76d6f76 320 if ( (wheelsState==LWHEEL_RUNS) || (wheelsState==LRWHEELS_RUN) ) wheelsState=LRWHEELS_RUN;
sepp_nepp 9:efe9f76d6f76 321 else wheelsState=RWHEEL_RUNS;
sepp_nepp 7:3a793ddc3490 322 }
sepp_nepp 7:3a793ddc3490 323 }
sepp_nepp 6:4d8938b686a6 324
sepp_nepp 9:efe9f76d6f76 325 void Creabot::wheelRunCentimeters(AWheel *Wheel, float dist_cm)
sepp_nepp 9:efe9f76d6f76 326 { AWheel->RunCentimeters(dist_cm); // Direction used is defaultDirection
sepp_nepp 9:efe9f76d6f76 327 wheelsState = wheelsState | AWheel->StateValue; // bitwise or of state value, can be either LWHEEL_RUNS or RWHEEL_RUNS
sepp_nepp 6:4d8938b686a6 328 }
sepp_nepp 6:4d8938b686a6 329
sepp_nepp 7:3a793ddc3490 330 //******************************************************************************
sepp_nepp 9:efe9f76d6f76 331 // low level wheel functions, handles callbacks from wheels
sepp_nepp 7:3a793ddc3490 332 // *****************************************************************************
sepp_nepp 9:efe9f76d6f76 333
sepp_nepp 9:efe9f76d6f76 334 void Creabot::wheelLeftStoppedCB()
sepp_nepp 9:efe9f76d6f76 335 { wheelsState = wheelsState & ! wheelLeft->StateValue; // mask out Left Wheel status bit
sepp_nepp 9:efe9f76d6f76 336 if (wheelsState == LRWHEELS_STOP) // all wheels are now known to have stopped
sepp_nepp 9:efe9f76d6f76 337 wheelsAllStoppedCB();
sepp_nepp 6:4d8938b686a6 338 }
sepp_nepp 6:4d8938b686a6 339
sepp_nepp 9:efe9f76d6f76 340 void Creabot::wheelRightStoppedCB()
sepp_nepp 9:efe9f76d6f76 341 { wheelsState = wheelsState & ! wheelRight->StateValue; // mask out Left Wheel status bit
sepp_nepp 9:efe9f76d6f76 342 if (wheelsState == LRWHEELS_STOP) // all wheels are now known to have stopped
sepp_nepp 9:efe9f76d6f76 343 wheelsAllStoppedCB();
sepp_nepp 7:3a793ddc3490 344 }
sepp_nepp 7:3a793ddc3490 345
sepp_nepp 9:efe9f76d6f76 346 void Creabot::wheelsAllStoppedCB()
sepp_nepp 9:efe9f76d6f76 347 { // assume that already at entry wheelsState == LRWHEELS_STOP
sepp_nepp 9:efe9f76d6f76 348 // Try to execute next entry in queue, if one is present
sepp_nepp 9:efe9f76d6f76 349 if ( !qBlock )
sepp_nepp 9:efe9f76d6f76 350 { qExecuteNext();// not blocked, so can execute now
sepp_nepp 9:efe9f76d6f76 351 // if wheels are still STOPed, that means no command to execute was left in the queue
sepp_nepp 9:efe9f76d6f76 352 if (wheelsState==LRWHEELS_STOP)
sepp_nepp 9:efe9f76d6f76 353 { // Turn motor power off!
sepp_nepp 9:efe9f76d6f76 354 wheelLeft->WheelOFF();
sepp_nepp 9:efe9f76d6f76 355 wheelRight->WheelOFF();
sepp_nepp 9:efe9f76d6f76 356 // Call the externall callback function to inform about the end of all movements
sepp_nepp 9:efe9f76d6f76 357 if(extCallBack!=NULL) extCallBack( wheelsState );
sepp_nepp 9:efe9f76d6f76 358 } // if wheelsState==LRWHEELS_STOP
sepp_nepp 9:efe9f76d6f76 359 } // if ( !qBlock )
sepp_nepp 9:efe9f76d6f76 360 else qCollide = true; // Remember the collision to execute the next command.
sepp_nepp 6:4d8938b686a6 361 }
sepp_nepp 6:4d8938b686a6 362
sepp_nepp 7:3a793ddc3490 363
sepp_nepp 7:3a793ddc3490 364