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@7:3a793ddc3490, 2019-01-01 (annotated)

Committer:
sepp_nepp
Date:
Tue Jan 01 13:05:08 2019 +0000
Revision:
7:3a793ddc3490
Parent:
6:4d8938b686a6
Child:
9:efe9f76d6f76
uncommented doxy

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 7:3a793ddc3490 8 void BotCommand::set(BotCmdVerb Acommand, float Aangle_deg, float Adist_cm)
sepp_nepp 6:4d8938b686a6 9 { command = Acommand;
sepp_nepp 6:4d8938b686a6 10 dist_cm = Adist_cm;
sepp_nepp 6:4d8938b686a6 11 angle_deg= Aangle_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 7:3a793ddc3490 18 angle_deg= Aparam;
sepp_nepp 5:efe80c5db389 19 };
sepp_nepp 5:efe80c5db389 20
sepp_nepp 6:4d8938b686a6 21 // *****************************************************************
sepp_nepp 7:3a793ddc3490 22 // circle_geos Structure Helper
sepp_nepp 7:3a793ddc3490 23 // *****************************************************************
sepp_nepp 7:3a793ddc3490 24
sepp_nepp 7:3a793ddc3490 25 void circle_geos:( float Adiam_cm) /**< Helper; set diameter and perim to values */
sepp_nepp 7:3a793ddc3490 26 {
sepp_nepp 7:3a793ddc3490 27 diam_cm = Adiam_cm;
sepp_nepp 7:3a793ddc3490 28 perim_cm = PI*diam_cm;
sepp_nepp 7:3a793ddc3490 29 degree_per_cm=360.0f/perim_cm;
sepp_nepp 7:3a793ddc3490 30 }
sepp_nepp 7:3a793ddc3490 31
sepp_nepp 7:3a793ddc3490 32 // *****************************************************************
sepp_nepp 6:4d8938b686a6 33 // CommandFIFO Class
sepp_nepp 6:4d8938b686a6 34 // *****************************************************************
sepp_nepp 6:4d8938b686a6 35
sepp_nepp 6:4d8938b686a6 36 // Class Creator: initializes an empties FIFO
sepp_nepp 7:3a793ddc3490 37 CommandFIFO::CommandFIFO()
sepp_nepp 7:3a793ddc3490 38 { readIdx = writeIdx = Count = 0;
sepp_nepp 6:4d8938b686a6 39 cmd_idle.set(IDLE, 0, 0);
sepp_nepp 6:4d8938b686a6 40 }
sepp_nepp 6:4d8938b686a6 41
sepp_nepp 6:4d8938b686a6 42 // Reserve a new element at the head of the FIFO
sepp_nepp 6:4d8938b686a6 43 BotCommand *CommandFIFO::put()
sepp_nepp 6:4d8938b686a6 44 {
sepp_nepp 6:4d8938b686a6 45 if( !isFull() ) {
sepp_nepp 5:efe80c5db389 46 int old_writeIdx = writeIdx;
sepp_nepp 5:efe80c5db389 47 writeIdx++;
sepp_nepp 6:4d8938b686a6 48 if (writeIdx==DEPTH_FIFO) writeIdx=0;
sepp_nepp 7:3a793ddc3490 49 Count++;
sepp_nepp 5:efe80c5db389 50 return &cmd[old_writeIdx];
sepp_nepp 6:4d8938b686a6 51 } else
sepp_nepp 5:efe80c5db389 52 return NULL;
sepp_nepp 5:efe80c5db389 53 }; // put()
sepp_nepp 5:efe80c5db389 54
sepp_nepp 6:4d8938b686a6 55 // Get and remove the oldest element at the tail of the FIFO
sepp_nepp 6:4d8938b686a6 56 BotCommand *CommandFIFO::get()
sepp_nepp 6:4d8938b686a6 57 {
sepp_nepp 5:efe80c5db389 58 if ( !isEmpty() ) {
sepp_nepp 6:4d8938b686a6 59 int old_readIdx = readIdx;
sepp_nepp 5:efe80c5db389 60 readIdx++;
sepp_nepp 6:4d8938b686a6 61 if (readIdx==DEPTH_FIFO) readIdx=0;
sepp_nepp 7:3a793ddc3490 62 Count--;
sepp_nepp 5:efe80c5db389 63 return &cmd[old_readIdx];
sepp_nepp 6:4d8938b686a6 64 } else
sepp_nepp 5:efe80c5db389 65 return NULL;
sepp_nepp 5:efe80c5db389 66 }; // get()
sepp_nepp 5:efe80c5db389 67
sepp_nepp 5:efe80c5db389 68 // *****************************************************************
sepp_nepp 5:efe80c5db389 69 // Creabot Class
sepp_nepp 5:efe80c5db389 70 // *****************************************************************
sepp_nepp 5:efe80c5db389 71
sepp_nepp 7:3a793ddc3490 72 Creabot::Creabot(Motor *left, Motor *right, float diameter_wheel_cm, float bot_diam_cm):
sepp_nepp 7:3a793ddc3490 73 motor_left(left), motor_right(right),
garphil 0:a7fb03c9ea9d 74 {
sepp_nepp 7:3a793ddc3490 75 // Initialize wheel and bot geometires
sepp_nepp 7:3a793ddc3490 76 wheel.setDiam(wheel_diam_cm);
sepp_nepp 7:3a793ddc3490 77 bot.setDiam(bot_diam_cm);
sepp_nepp 7:3a793ddc3490 78 // some other parameters:
sepp_nepp 7:3a793ddc3490 79 ratio_wheel_bot=bot_diam_cm/wheel_diam_cm;
sepp_nepp 7:3a793ddc3490 80 bot_speed_cm_sec = DEFAULT_SPEED_CM_SEC;
garphil 0:a7fb03c9ea9d 81
sepp_nepp 6:4d8938b686a6 82 // Attach the Crabot functions as callbacks to the motors
sepp_nepp 6:4d8938b686a6 83 motor_left->callbackSet(this, &Creabot::cbLeftMotStopped);
sepp_nepp 6:4d8938b686a6 84 motor_right->callbackSet(this, &Creabot::cbRightMotStopped);
sepp_nepp 6:4d8938b686a6 85 // Initialize own callback as empty
garphil 0:a7fb03c9ea9d 86 extCallBack = NULL;
sepp_nepp 6:4d8938b686a6 87
sepp_nepp 6:4d8938b686a6 88 // Initialize the own status fields
sepp_nepp 7:3a793ddc3490 89 MotState = LRMOTS_STOP;
sepp_nepp 6:4d8938b686a6 90 executingFifo= false;
garphil 0:a7fb03c9ea9d 91 }
garphil 0:a7fb03c9ea9d 92
sepp_nepp 6:4d8938b686a6 93 void Creabot::fifo(BotCmdVerb moveType, float angle_or_cm)
garphil 0:a7fb03c9ea9d 94 {
sepp_nepp 6:4d8938b686a6 95 BotCommand *cmd = fifoBot.put();
sepp_nepp 5:efe80c5db389 96 if(cmd != NULL) {
sepp_nepp 6:4d8938b686a6 97 cmd->set(moveType, angle_or_cm, 0);
sepp_nepp 5:efe80c5db389 98 }
garphil 0:a7fb03c9ea9d 99 executeFifo();
garphil 0:a7fb03c9ea9d 100 }
garphil 0:a7fb03c9ea9d 101
sepp_nepp 6:4d8938b686a6 102 void Creabot::fifo(BotCmdVerb moveType, float angle_deg, float dist_cm)
sepp_nepp 5:efe80c5db389 103 {
sepp_nepp 6:4d8938b686a6 104 BotCommand *cmd = fifoBot.put();
sepp_nepp 5:efe80c5db389 105 if(cmd!=NULL) {
sepp_nepp 6:4d8938b686a6 106 cmd->set(moveType, angle_deg, dist_cm);
sepp_nepp 5:efe80c5db389 107 }
sepp_nepp 5:efe80c5db389 108 executeFifo();
sepp_nepp 5:efe80c5db389 109 }
garphil 0:a7fb03c9ea9d 110
garphil 0:a7fb03c9ea9d 111 void Creabot::flushFifo()
garphil 0:a7fb03c9ea9d 112 {
garphil 0:a7fb03c9ea9d 113 fifoBot.empty();
garphil 0:a7fb03c9ea9d 114 if(executingFifo) botTicker.detach();
garphil 0:a7fb03c9ea9d 115 executingFifo=false;
garphil 0:a7fb03c9ea9d 116 }
sepp_nepp 5:efe80c5db389 117
garphil 0:a7fb03c9ea9d 118 int Creabot::moveInFifo()
garphil 0:a7fb03c9ea9d 119 {
sepp_nepp 5:efe80c5db389 120 return fifoBot.getCount();
sepp_nepp 5:efe80c5db389 121 }
sepp_nepp 5:efe80c5db389 122
sepp_nepp 5:efe80c5db389 123 void Creabot::spirale(float b, float turns) {
sepp_nepp 6:4d8938b686a6 124 float r=0.0f;
sepp_nepp 6:4d8938b686a6 125 float angle_deg = 0.0f;
sepp_nepp 6:4d8938b686a6 126 while(angle_deg < float( 360.0f * turns) ) {
sepp_nepp 5:efe80c5db389 127 this->moveAndWait(RIGHT, 10, r);
sepp_nepp 5:efe80c5db389 128 r=r+b;
sepp_nepp 5:efe80c5db389 129 }
sepp_nepp 5:efe80c5db389 130 }
sepp_nepp 5:efe80c5db389 131
sepp_nepp 5:efe80c5db389 132 // This is the own callback function of the ticker, that works throught the FIFO stack
sepp_nepp 5:efe80c5db389 133 void Creabot::executeFifo()
sepp_nepp 5:efe80c5db389 134 { if (fifoBot.isEmpty()) flushFifo();
sepp_nepp 5:efe80c5db389 135 else {
sepp_nepp 5:efe80c5db389 136 if(endMove) {
sepp_nepp 6:4d8938b686a6 137 BotCommand *cmd = fifoBot.get();
sepp_nepp 5:efe80c5db389 138 executeCommand(cmd);
sepp_nepp 5:efe80c5db389 139 if (!executingFifo) botTicker.attach_us( callback( this, &Creabot::executeFifo), 3150);
sepp_nepp 5:efe80c5db389 140 executingFifo=true;
sepp_nepp 5:efe80c5db389 141 }
sepp_nepp 5:efe80c5db389 142 }
sepp_nepp 5:efe80c5db389 143 }
sepp_nepp 5:efe80c5db389 144
sepp_nepp 7:3a793ddc3490 145 //**************************************************************************
sepp_nepp 7:3a793ddc3490 146 // Using the BotCmdVerb to encode different commands, done immediately
sepp_nepp 7:3a793ddc3490 147 //**************************************************************************
sepp_nepp 7:3a793ddc3490 148
sepp_nepp 7:3a793ddc3490 149 void Creabot::moveAndWait(BotCmdVerb moveType, float Aparam)
sepp_nepp 5:efe80c5db389 150 {
sepp_nepp 7:3a793ddc3490 151 move(moveType,Aparam);
sepp_nepp 7:3a793ddc3490 152 waitEndMove();
sepp_nepp 5:efe80c5db389 153 }
sepp_nepp 5:efe80c5db389 154
sepp_nepp 7:3a793ddc3490 155 void Creabot::moveAndWait(BotCmdVerb moveType, float Aangle_deg, float Adist_cm)
sepp_nepp 5:efe80c5db389 156 {
sepp_nepp 7:3a793ddc3490 157 move(moveType,Aangle_deg,Adist_cm);
sepp_nepp 7:3a793ddc3490 158 waitEndMove();
sepp_nepp 5:efe80c5db389 159 }
sepp_nepp 5:efe80c5db389 160
sepp_nepp 7:3a793ddc3490 161
sepp_nepp 7:3a793ddc3490 162 void Creabot::move(BotCmdVerb moveType, float Aparam)
sepp_nepp 7:3a793ddc3490 163 { move(moveType, Aparam, Aparam)
sepp_nepp 7:3a793ddc3490 164 }
sepp_nepp 7:3a793ddc3490 165
sepp_nepp 7:3a793ddc3490 166 void Creabot::move(BotCmdVerb moveType, float Aangle_deg, float Adist_cm)
sepp_nepp 7:3a793ddc3490 167 { BotCommand Anew_cmd;
sepp_nepp 7:3a793ddc3490 168 Anew_cmd.set(moveType, Aangle_deg, Adist_cm);
sepp_nepp 7:3a793ddc3490 169 executeCommand(&Anew_cmd);
sepp_nepp 7:3a793ddc3490 170 }
sepp_nepp 7:3a793ddc3490 171
sepp_nepp 7:3a793ddc3490 172 // Direct Command Execution, also called by FIFO-Handler.
sepp_nepp 6:4d8938b686a6 173 void Creabot::executeCommand(BotCommand *cmd) {
garphil 0:a7fb03c9ea9d 174 switch (cmd->command) {
garphil 0:a7fb03c9ea9d 175 case FORWARD:
sepp_nepp 6:4d8938b686a6 176 moveForward(cmd->dist_cm);
garphil 0:a7fb03c9ea9d 177 break;
garphil 0:a7fb03c9ea9d 178 case BACKWARD:
sepp_nepp 6:4d8938b686a6 179 moveBackward(cmd->dist_cm);
garphil 0:a7fb03c9ea9d 180 break;
garphil 0:a7fb03c9ea9d 181 case ROTATE:
sepp_nepp 6:4d8938b686a6 182 rotate(cmd->angle_deg);
garphil 0:a7fb03c9ea9d 183 break;
garphil 0:a7fb03c9ea9d 184 case LEFT:
sepp_nepp 6:4d8938b686a6 185 moveLeft(cmd->angle_deg, cmd->dist_cm);
garphil 0:a7fb03c9ea9d 186 break;
garphil 0:a7fb03c9ea9d 187 case RIGHT:
sepp_nepp 6:4d8938b686a6 188 moveRight(cmd->angle_deg, cmd->dist_cm);
garphil 0:a7fb03c9ea9d 189 break;
garphil 0:a7fb03c9ea9d 190 case IDLE:
garphil 0:a7fb03c9ea9d 191 default:
garphil 0:a7fb03c9ea9d 192 break;
garphil 0:a7fb03c9ea9d 193 };
sepp_nepp 5:efe80c5db389 194 }; // executeCommand
sepp_nepp 5:efe80c5db389 195
sepp_nepp 6:4d8938b686a6 196 void Creabot::stopAll()
sepp_nepp 6:4d8938b686a6 197 {
sepp_nepp 6:4d8938b686a6 198 flushFifo();
sepp_nepp 6:4d8938b686a6 199 stopMotors();
sepp_nepp 6:4d8938b686a6 200 }
sepp_nepp 6:4d8938b686a6 201
garphil 0:a7fb03c9ea9d 202
sepp_nepp 7:3a793ddc3490 203
sepp_nepp 7:3a793ddc3490 204 // *****************************************************************
sepp_nepp 7:3a793ddc3490 205 // Creabot Mid level control functions:
sepp_nepp 7:3a793ddc3490 206 // all directly access the motor, no FIFO */
sepp_nepp 7:3a793ddc3490 207 // *****************************************************************
sepp_nepp 7:3a793ddc3490 208
sepp_nepp 7:3a793ddc3490 209 // set bot_speed_cm_sec parameter for next high level commands
sepp_nepp 7:3a793ddc3490 210 void Creabot::setBotSpeed(float Abot_speed_cm_sec)
sepp_nepp 7:3a793ddc3490 211 {
sepp_nepp 7:3a793ddc3490 212 if (Abot_speed_cm_sec < MAX_SPEED_CM_P_SEC) && (Abot_speed_cm_sec>MIN_SPEED_CM_SEC)
sepp_nepp 7:3a793ddc3490 213 bot_speed_cm_sec = Abot_speed_cm_sec;
sepp_nepp 7:3a793ddc3490 214 // Low level setting of motor speed is done inside SetMotsSpeed:
sepp_nepp 7:3a793ddc3490 215 }
garphil 0:a7fb03c9ea9d 216
sepp_nepp 7:3a793ddc3490 217 // Mid level control function: move bot forward for a given distance
sepp_nepp 7:3a793ddc3490 218 void Creabot::moveForward(float dist_cm)
sepp_nepp 7:3a793ddc3490 219 {
sepp_nepp 7:3a793ddc3490 220 if (dist_cm<0) moveBackward(-dist_cm);
sepp_nepp 7:3a793ddc3490 221 else {
sepp_nepp 7:3a793ddc3490 222 float angle_deg = dist_cm*wheel.degree_per_cm;
sepp_nepp 7:3a793ddc3490 223 setMotorsSpeed(bot_speed_cm_sec);
sepp_nepp 7:3a793ddc3490 224 moveMotorLeft(COUNTERCLOCKWISE, angle_deg);
sepp_nepp 7:3a793ddc3490 225 moveMotorRight(CLOCKWISE, angle_deg);
sepp_nepp 7:3a793ddc3490 226 }
sepp_nepp 7:3a793ddc3490 227 }
sepp_nepp 7:3a793ddc3490 228
sepp_nepp 7:3a793ddc3490 229 // Mid level control function: move bot backwards for a given distance
sepp_nepp 7:3a793ddc3490 230 void Creabot::moveBackward(float dist_cm)
sepp_nepp 7:3a793ddc3490 231 {
sepp_nepp 7:3a793ddc3490 232 if(dist_cm<0) moveForward(-dist_cm);
sepp_nepp 7:3a793ddc3490 233 else {
sepp_nepp 7:3a793ddc3490 234 float angle_deg = dist_cm*wheel.degree_per_cm;
sepp_nepp 7:3a793ddc3490 235 setMotorsSpeed(bot_speed_cm_sec);
sepp_nepp 7:3a793ddc3490 236 moveMotorLeft(CLOCKWISE, angle_deg);
sepp_nepp 7:3a793ddc3490 237 moveMotorRight(COUNTERCLOCKWISE, angle_deg);
sepp_nepp 7:3a793ddc3490 238 }
sepp_nepp 7:3a793ddc3490 239 }
sepp_nepp 7:3a793ddc3490 240
sepp_nepp 7:3a793ddc3490 241 /* Mid level control function: turn bot forward right,
sepp_nepp 7:3a793ddc3490 242 around a radius twice the bot size */
sepp_nepp 6:4d8938b686a6 243 void Creabot::moveRight(float angle_deg)
sepp_nepp 5:efe80c5db389 244 {
sepp_nepp 7:3a793ddc3490 245 moveRight(angle_deg,bot.diam_cm);
garphil 0:a7fb03c9ea9d 246 }
garphil 0:a7fb03c9ea9d 247
sepp_nepp 7:3a793ddc3490 248 /* Mid level control function: turn bot forward right,
sepp_nepp 7:3a793ddc3490 249 around a radius that is center_cm away from the right wheel*/
sepp_nepp 6:4d8938b686a6 250 void Creabot::moveRight(float angle_deg, float center_cm)
sepp_nepp 5:efe80c5db389 251 {
sepp_nepp 5:efe80c5db389 252 if(center_cm<0) center_cm=0; /* TO remove? */
sepp_nepp 7:3a793ddc3490 253 float perimeter_outer = 2.0f*angle_deg*(center_cm+bot.diam_cm)*PI/360.0f;
sepp_nepp 6:4d8938b686a6 254 float perimeter_inner = 2.0f*angle_deg*(center_cm)*PI/360.0f;
sepp_nepp 7:3a793ddc3490 255 float angleLeft = perimeter_outer*wheel.degree_per_cm;
sepp_nepp 7:3a793ddc3490 256 float angleRight = perimeter_inner*wheel.degree_per_cm;
sepp_nepp 5:efe80c5db389 257 float ratio = angleRight/angleLeft;
sepp_nepp 7:3a793ddc3490 258 float speedRight = bot_speed_cm_sec * ratio;
sepp_nepp 7:3a793ddc3490 259 setMotSpeed(motor_left,bot_speed_cm_sec);
sepp_nepp 7:3a793ddc3490 260 setMotSpeed(motor_right,speedRight);
sepp_nepp 5:efe80c5db389 261 moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
sepp_nepp 5:efe80c5db389 262 moveMotorRight(CLOCKWISE, angleRight);
sepp_nepp 5:efe80c5db389 263 }
sepp_nepp 5:efe80c5db389 264
sepp_nepp 7:3a793ddc3490 265 /* Mid level control function: turn bot forward left,
sepp_nepp 7:3a793ddc3490 266 around a radius twice the bot size */
sepp_nepp 6:4d8938b686a6 267 void Creabot::moveLeft(float angle_deg)
sepp_nepp 5:efe80c5db389 268 {
sepp_nepp 6:4d8938b686a6 269 moveLeft(angle_deg,0);
garphil 4:531b1120d3ec 270 }
garphil 4:531b1120d3ec 271
sepp_nepp 7:3a793ddc3490 272 /* Mid level control function: turn bot forward left,
sepp_nepp 7:3a793ddc3490 273 around a radius that is center_cm away from the left wheel*/
sepp_nepp 6:4d8938b686a6 274 void Creabot::moveLeft(float angle_deg, float center_cm)
garphil 0:a7fb03c9ea9d 275 {
sepp_nepp 7:3a793ddc3490 276 if (center_cm<0) center_cm=0; /* TO remove? */
sepp_nepp 7:3a793ddc3490 277 float perimeter_outer = 2.0f*angle_deg*(center_cm+bot.diam_cm)*PI/360.0f;
sepp_nepp 6:4d8938b686a6 278 float perimeter_inner = 2.0f*angle_deg*(center_cm)*PI/360.0f;
sepp_nepp 7:3a793ddc3490 279 float angleLeft = perimeter_inner*wheel.degree_per_cm;
sepp_nepp 7:3a793ddc3490 280 float angleRight = perimeter_outer*wheel.degree_per_cm;
sepp_nepp 5:efe80c5db389 281 float ratio = angleLeft/angleRight;
sepp_nepp 7:3a793ddc3490 282 float speedRLeft = bot_speed_cm_sec * ratio;
sepp_nepp 7:3a793ddc3490 283 setMotSpeed(motor_left,speedRLeft);
sepp_nepp 7:3a793ddc3490 284 setMotSpeed(motor_right,bot_speed_cm_sec);
sepp_nepp 5:efe80c5db389 285 moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
sepp_nepp 5:efe80c5db389 286 moveMotorRight(CLOCKWISE, angleRight);
garphil 0:a7fb03c9ea9d 287 }
garphil 0:a7fb03c9ea9d 288
sepp_nepp 7:3a793ddc3490 289 /* Mid level control function: turn bot on its place for a given angle.
sepp_nepp 7:3a793ddc3490 290 positive angles turn right, negative angles turn left*/
sepp_nepp 7:3a793ddc3490 291 void Creabot::rotate(float angleBot)
garphil 0:a7fb03c9ea9d 292 {
sepp_nepp 7:3a793ddc3490 293 float angleWheel = angleBot*ratio_wheel_bot;
sepp_nepp 7:3a793ddc3490 294 setSpeed(bot_speed_cm_sec);
sepp_nepp 7:3a793ddc3490 295 if(angleBot<0) {
sepp_nepp 7:3a793ddc3490 296 moveMotorLeft(CLOCKWISE, -angleWheel);
sepp_nepp 7:3a793ddc3490 297 moveMotorRight(CLOCKWISE, -angleWheel);
sepp_nepp 7:3a793ddc3490 298 } else {
sepp_nepp 7:3a793ddc3490 299 moveMotorLeft(COUNTERCLOCKWISE, angleWheel);
sepp_nepp 7:3a793ddc3490 300 moveMotorRight(COUNTERCLOCKWISE, angleWheel);
sepp_nepp 6:4d8938b686a6 301 }
sepp_nepp 5:efe80c5db389 302 }
sepp_nepp 5:efe80c5db389 303
sepp_nepp 7:3a793ddc3490 304 /* spends all time with waits, returns only once MotorState = LRMOTS_STOP */
sepp_nepp 7:3a793ddc3490 305 void Creabot::waitEndMotors()
sepp_nepp 7:3a793ddc3490 306 { while(MotState!=LRMOTS_STOP) wait(0.1); }
sepp_nepp 7:3a793ddc3490 307
sepp_nepp 7:3a793ddc3490 308 /* spends all time with waits, returns only once MotorState = LRMOTS_STOP,
sepp_nepp 7:3a793ddc3490 309 but maximum max_wait_ms milli seconds */
sepp_nepp 7:3a793ddc3490 310 void Creabot::waitEndMotors(uint32_t max_wait_ms)
sepp_nepp 7:3a793ddc3490 311 { uint32_t waited=0;
sepp_nepp 7:3a793ddc3490 312 while( (MotState!=LRMOTS_STOP) && (waited < max_wait_ms) )
sepp_nepp 7:3a793ddc3490 313 { wait(0.1); waited += 100; }
sepp_nepp 7:3a793ddc3490 314 } // waitEndMotors
sepp_nepp 7:3a793ddc3490 315
sepp_nepp 7:3a793ddc3490 316 //******************************************************************************
sepp_nepp 7:3a793ddc3490 317 // low level motor functions, only filters for angle_deg>0, then passes
sepp_nepp 7:3a793ddc3490 318 // to Motor Class; and remembers that a motor moves
sepp_nepp 7:3a793ddc3490 319 //******************************************************************************
sepp_nepp 7:3a793ddc3490 320
sepp_nepp 7:3a793ddc3490 321 /* Low level access functions: sets both motor speeds immediately */
sepp_nepp 7:3a793ddc3490 322 void Creabot::setMotorsSpeed(float mot_speed_cm_sec)
sepp_nepp 5:efe80c5db389 323 {
sepp_nepp 7:3a793ddc3490 324 setMotSpeed(motor_left,mot_speed_cm_sec);
sepp_nepp 7:3a793ddc3490 325 setMotSpeed(motor_right,mot_speed_cm_sec);
sepp_nepp 5:efe80c5db389 326 }
sepp_nepp 5:efe80c5db389 327
sepp_nepp 7:3a793ddc3490 328
sepp_nepp 7:3a793ddc3490 329 void Creabot::setMotorSpeed(Motor *motor, float speed_cm_sec)
sepp_nepp 7:3a793ddc3490 330 { if (speed_cm_sec < MIN_SPEED_CM_SEC) // catch too small or negative speeds
sepp_nepp 7:3a793ddc3490 331 motor->setStepTime_us( 1000000 ); // arbitrary value
sepp_nepp 7:3a793ddc3490 332 else
sepp_nepp 7:3a793ddc3490 333 { motor->setStepTime_us( wheel.perim_cm * 1000000 /
sepp_nepp 7:3a793ddc3490 334 speed_cm_sec / motor->getStepsFullTurn() ) ; }
sepp_nepp 6:4d8938b686a6 335 }
sepp_nepp 6:4d8938b686a6 336
sepp_nepp 7:3a793ddc3490 337 void Creabot::moveMotorLeft(motorDir dir, float angle_deg)
sepp_nepp 7:3a793ddc3490 338 { if(angle_deg>0) {
sepp_nepp 7:3a793ddc3490 339 motor_left->RunDegrees(dir, angle_deg);
sepp_nepp 7:3a793ddc3490 340 if (MotState==RMOT_RUNS) MotState=LRMOTS_RUN;
sepp_nepp 7:3a793ddc3490 341 else MotState=LMOT_RUNS;
sepp_nepp 7:3a793ddc3490 342 }
sepp_nepp 5:efe80c5db389 343 }
sepp_nepp 5:efe80c5db389 344
sepp_nepp 7:3a793ddc3490 345 void Creabot::moveMotorRight(motorDir dir, float angle_deg)
sepp_nepp 7:3a793ddc3490 346 { if(angle_deg>0) {
sepp_nepp 7:3a793ddc3490 347 motor_right->RunDegrees(dir, angle_deg);
sepp_nepp 7:3a793ddc3490 348 if (MotState==LMOT_RUNS) MotState=LRMOTS_RUN;
sepp_nepp 7:3a793ddc3490 349 else MotState=RMOT_RUNS;
sepp_nepp 7:3a793ddc3490 350 }
sepp_nepp 7:3a793ddc3490 351 }
sepp_nepp 6:4d8938b686a6 352
sepp_nepp 6:4d8938b686a6 353 void Creabot::stopMotors()
sepp_nepp 6:4d8938b686a6 354 {
sepp_nepp 6:4d8938b686a6 355 motor_left->MotorOFF();
sepp_nepp 6:4d8938b686a6 356 motor_right->MotorOFF();
sepp_nepp 6:4d8938b686a6 357 AllMotorsStopped()
sepp_nepp 6:4d8938b686a6 358 }
sepp_nepp 6:4d8938b686a6 359
sepp_nepp 7:3a793ddc3490 360
sepp_nepp 7:3a793ddc3490 361 //******************************************************************************
sepp_nepp 7:3a793ddc3490 362 // low level motor functions, handles callbacks from motors
sepp_nepp 7:3a793ddc3490 363 // *****************************************************************************
sepp_nepp 7:3a793ddc3490 364 void Creabot::setCallBack(void (*Acallback)(int status))
sepp_nepp 6:4d8938b686a6 365 {
sepp_nepp 7:3a793ddc3490 366 extCallBack = Acallback;
sepp_nepp 6:4d8938b686a6 367 }
sepp_nepp 6:4d8938b686a6 368
sepp_nepp 7:3a793ddc3490 369 void Creabot::cbLeftMotStopped()
sepp_nepp 7:3a793ddc3490 370 {
sepp_nepp 7:3a793ddc3490 371 if (MotState==LRMOTS_RUN) MotState = RMOT_RUNS;
sepp_nepp 7:3a793ddc3490 372 else AllMotorsStopped();
sepp_nepp 7:3a793ddc3490 373 }
sepp_nepp 7:3a793ddc3490 374
sepp_nepp 7:3a793ddc3490 375 void Creabot::cbRightMotStopped()
sepp_nepp 7:3a793ddc3490 376 {
sepp_nepp 7:3a793ddc3490 377 if (MotState==LRMOTS_RUN) MotState = LMOT_RUNS;
sepp_nepp 7:3a793ddc3490 378 else AllMotorsStopped();
sepp_nepp 7:3a793ddc3490 379 }
sepp_nepp 7:3a793ddc3490 380
sepp_nepp 7:3a793ddc3490 381 void Creabot::AllMotorsStopped()
sepp_nepp 7:3a793ddc3490 382 {
sepp_nepp 7:3a793ddc3490 383 if (MotState!=LRMOTS_STOP)
sepp_nepp 7:3a793ddc3490 384 { // Only do all this here if motor state is not already in LRMOTS_STOP!
sepp_nepp 7:3a793ddc3490 385 MotState = LRMOTS_STOP;
sepp_nepp 7:3a793ddc3490 386 if(extCallBack!=NULL) extCallBack( MotState );
sepp_nepp 6:4d8938b686a6 387 }
sepp_nepp 6:4d8938b686a6 388 }
sepp_nepp 6:4d8938b686a6 389
sepp_nepp 7:3a793ddc3490 390
sepp_nepp 7:3a793ddc3490 391