Tarek Lule / CreaBotLib

My Version of CreaBotLib

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