Tarek Lule / CreaBotLib

My Version of CreaBotLib

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