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@6:4d8938b686a6, 2018-11-28 (annotated)

Committer:
sepp_nepp
Date:
Wed Nov 28 09:46:41 2018 +0000
Revision:
6:4d8938b686a6
Parent:
5:efe80c5db389
Child:
7:3a793ddc3490
Update with some changes to documentation. ; Not yet tried on actual robot. ; Some bugs in my doxygen code means documentation generation fails.

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 6:4d8938b686a6 8 void BotCommand::set(BotCmdVerb Acommand, float Adist_cm, float Aangle_deg)
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 6:4d8938b686a6 15 void BotCommand::set(BotCmdVerb Acommand, float Adist_cm)
sepp_nepp 6:4d8938b686a6 16 { command = Acommand;
sepp_nepp 6:4d8938b686a6 17 dist_cm = Adist_cm;
sepp_nepp 6:4d8938b686a6 18 angle_deg= 0;
sepp_nepp 5:efe80c5db389 19 };
sepp_nepp 5:efe80c5db389 20
sepp_nepp 6:4d8938b686a6 21 // *****************************************************************
sepp_nepp 6:4d8938b686a6 22 // CommandFIFO Class
sepp_nepp 6:4d8938b686a6 23 // *****************************************************************
sepp_nepp 6:4d8938b686a6 24
sepp_nepp 6:4d8938b686a6 25 // Class Creator: initializes an empties FIFO
sepp_nepp 6:4d8938b686a6 26 CommandFIFO::CommandFIFO();
sepp_nepp 6:4d8938b686a6 27 { readIdx = writeIdx = count = 0;
sepp_nepp 6:4d8938b686a6 28 cmd_idle.set(IDLE, 0, 0);
sepp_nepp 6:4d8938b686a6 29 }
sepp_nepp 6:4d8938b686a6 30
sepp_nepp 6:4d8938b686a6 31 // Reserve a new element at the head of the FIFO
sepp_nepp 6:4d8938b686a6 32 BotCommand *CommandFIFO::put()
sepp_nepp 6:4d8938b686a6 33 {
sepp_nepp 6:4d8938b686a6 34 if( !isFull() ) {
sepp_nepp 5:efe80c5db389 35 int old_writeIdx = writeIdx;
sepp_nepp 5:efe80c5db389 36 writeIdx++;
sepp_nepp 6:4d8938b686a6 37 if (writeIdx==DEPTH_FIFO) writeIdx=0;
sepp_nepp 5:efe80c5db389 38 count++;
sepp_nepp 5:efe80c5db389 39 return &cmd[old_writeIdx];
sepp_nepp 6:4d8938b686a6 40 } else
sepp_nepp 5:efe80c5db389 41 return NULL;
sepp_nepp 5:efe80c5db389 42 }; // put()
sepp_nepp 5:efe80c5db389 43
sepp_nepp 6:4d8938b686a6 44 // Get and remove the oldest element at the tail of the FIFO
sepp_nepp 6:4d8938b686a6 45 BotCommand *CommandFIFO::get()
sepp_nepp 6:4d8938b686a6 46 {
sepp_nepp 5:efe80c5db389 47 if ( !isEmpty() ) {
sepp_nepp 6:4d8938b686a6 48 int old_readIdx = readIdx;
sepp_nepp 5:efe80c5db389 49 readIdx++;
sepp_nepp 6:4d8938b686a6 50 if (readIdx==DEPTH_FIFO) readIdx=0;
sepp_nepp 6:4d8938b686a6 51 count--;
sepp_nepp 5:efe80c5db389 52 return &cmd[old_readIdx];
sepp_nepp 6:4d8938b686a6 53 } else
sepp_nepp 5:efe80c5db389 54 return NULL;
sepp_nepp 5:efe80c5db389 55 }; // get()
sepp_nepp 5:efe80c5db389 56
sepp_nepp 5:efe80c5db389 57 // *****************************************************************
sepp_nepp 5:efe80c5db389 58 // Creabot Class
sepp_nepp 5:efe80c5db389 59 // *****************************************************************
sepp_nepp 5:efe80c5db389 60
sepp_nepp 5:efe80c5db389 61 Creabot::Creabot(Motor *left, Motor *right, float diameter_wheel_cm, float distance_wheel_cm):
sepp_nepp 5:efe80c5db389 62 motor_left(left), motor_right(right), diameter_wheel(diameter_wheel_cm), distance_wheel(distance_wheel_cm)
garphil 0:a7fb03c9ea9d 63 {
sepp_nepp 6:4d8938b686a6 64 // Initialize dimensions
garphil 0:a7fb03c9ea9d 65 perimeter_wheel = PI*diameter_wheel;
garphil 0:a7fb03c9ea9d 66 perimeter_bot = PI*distance_wheel;
garphil 0:a7fb03c9ea9d 67 degree_wheel_per_cm=360.0f/perimeter_wheel;
garphil 0:a7fb03c9ea9d 68 degree_bot_per_cm=360.0f/perimeter_bot;;
garphil 3:891b6a138383 69 ratio_wheel_bot=perimeter_bot/perimeter_wheel;
sepp_nepp 6:4d8938b686a6 70 setSpeed(DEFAULT_SPEED);
garphil 0:a7fb03c9ea9d 71
sepp_nepp 6:4d8938b686a6 72 // Attach the Crabot functions as callbacks to the motors
sepp_nepp 6:4d8938b686a6 73 motor_left->callbackSet(this, &Creabot::cbLeftMotStopped);
sepp_nepp 6:4d8938b686a6 74 motor_right->callbackSet(this, &Creabot::cbRightMotStopped);
sepp_nepp 6:4d8938b686a6 75
sepp_nepp 6:4d8938b686a6 76 // Initialize own callback as empty
garphil 0:a7fb03c9ea9d 77 extCallBack = NULL;
sepp_nepp 6:4d8938b686a6 78
sepp_nepp 6:4d8938b686a6 79 // Initialize the own status fields
sepp_nepp 6:4d8938b686a6 80 endMoveLeft = true;
sepp_nepp 5:efe80c5db389 81 endMoveRight = true;
sepp_nepp 6:4d8938b686a6 82 endMove = true;
sepp_nepp 6:4d8938b686a6 83 executingFifo= false;
garphil 0:a7fb03c9ea9d 84 }
garphil 0:a7fb03c9ea9d 85
sepp_nepp 6:4d8938b686a6 86 void Creabot::moveForward(float dist_cm)
garphil 0:a7fb03c9ea9d 87 {
sepp_nepp 6:4d8938b686a6 88 if (dist_cm<0) moveBackward(dist_cm);
sepp_nepp 5:efe80c5db389 89 else {
sepp_nepp 6:4d8938b686a6 90 float angle_deg = dist_cm*degree_wheel_per_cm;
sepp_nepp 5:efe80c5db389 91 setSpeed(last_speed);
sepp_nepp 6:4d8938b686a6 92 moveMotorLeft(COUNTERCLOCKWISE, angle_deg);
sepp_nepp 6:4d8938b686a6 93 moveMotorRight(CLOCKWISE, angle_deg);
sepp_nepp 5:efe80c5db389 94 }
garphil 0:a7fb03c9ea9d 95 }
garphil 0:a7fb03c9ea9d 96
sepp_nepp 6:4d8938b686a6 97 void Creabot::moveBackward(float dist_cm)
garphil 0:a7fb03c9ea9d 98 {
sepp_nepp 6:4d8938b686a6 99 if(dist_cm<0) moveForward(dist_cm);
sepp_nepp 5:efe80c5db389 100 else {
sepp_nepp 6:4d8938b686a6 101 float angle_deg = dist_cm*degree_wheel_per_cm;
sepp_nepp 5:efe80c5db389 102 setSpeed(last_speed);
sepp_nepp 6:4d8938b686a6 103 moveMotorLeft(CLOCKWISE, angle_deg);
sepp_nepp 6:4d8938b686a6 104 moveMotorRight(COUNTERCLOCKWISE, angle_deg);
sepp_nepp 5:efe80c5db389 105 }
garphil 0:a7fb03c9ea9d 106 }
garphil 0:a7fb03c9ea9d 107 void Creabot::rotate(float angleBot)
garphil 0:a7fb03c9ea9d 108 {
sepp_nepp 5:efe80c5db389 109 float angleWheel = angleBot*ratio_wheel_bot;
sepp_nepp 5:efe80c5db389 110 setSpeed(last_speed);
garphil 2:3d2d6d655d01 111 if(angleBot<0) {
garphil 2:3d2d6d655d01 112 moveMotorLeft(CLOCKWISE, -angleWheel);
garphil 2:3d2d6d655d01 113 moveMotorRight(CLOCKWISE, -angleWheel);
garphil 0:a7fb03c9ea9d 114 } else {
garphil 2:3d2d6d655d01 115 moveMotorLeft(COUNTERCLOCKWISE, angleWheel);
garphil 2:3d2d6d655d01 116 moveMotorRight(COUNTERCLOCKWISE, angleWheel);
garphil 0:a7fb03c9ea9d 117 }
garphil 0:a7fb03c9ea9d 118 }
garphil 0:a7fb03c9ea9d 119
garphil 0:a7fb03c9ea9d 120
garphil 0:a7fb03c9ea9d 121 void Creabot::waitEndMove()
garphil 0:a7fb03c9ea9d 122 {
garphil 0:a7fb03c9ea9d 123 while(!endMove || executingFifo) {
garphil 0:a7fb03c9ea9d 124 wait(0.1);
garphil 0:a7fb03c9ea9d 125 }
garphil 1:974d020bb582 126
garphil 0:a7fb03c9ea9d 127 }
garphil 0:a7fb03c9ea9d 128 void Creabot::waitEndMove(uint32_t delay_us)
garphil 0:a7fb03c9ea9d 129 {
garphil 0:a7fb03c9ea9d 130 uint32_t v=0;
garphil 0:a7fb03c9ea9d 131 while(!endMove || executingFifo) {
garphil 0:a7fb03c9ea9d 132 wait(0.1);
garphil 0:a7fb03c9ea9d 133 v++;
garphil 0:a7fb03c9ea9d 134 if((v*10000)>delay_us) endMove = true;
garphil 0:a7fb03c9ea9d 135 }
garphil 0:a7fb03c9ea9d 136 } // watchdog
garphil 0:a7fb03c9ea9d 137
sepp_nepp 6:4d8938b686a6 138 void Creabot::setCallBack(void (*Acallback)(int status))
garphil 0:a7fb03c9ea9d 139 {
sepp_nepp 6:4d8938b686a6 140 extCallBack = Acallback;
garphil 0:a7fb03c9ea9d 141 }
garphil 0:a7fb03c9ea9d 142
sepp_nepp 6:4d8938b686a6 143 void Creabot::fifo(BotCmdVerb moveType, float angle_or_cm)
garphil 0:a7fb03c9ea9d 144 {
sepp_nepp 6:4d8938b686a6 145 BotCommand *cmd = fifoBot.put();
sepp_nepp 5:efe80c5db389 146 if(cmd != NULL) {
sepp_nepp 6:4d8938b686a6 147 cmd->set(moveType, angle_or_cm, 0);
sepp_nepp 5:efe80c5db389 148 }
garphil 0:a7fb03c9ea9d 149 executeFifo();
garphil 0:a7fb03c9ea9d 150 }
garphil 0:a7fb03c9ea9d 151
sepp_nepp 6:4d8938b686a6 152 void Creabot::fifo(BotCmdVerb moveType, float angle_deg, float dist_cm)
sepp_nepp 5:efe80c5db389 153 {
sepp_nepp 6:4d8938b686a6 154 BotCommand *cmd = fifoBot.put();
sepp_nepp 5:efe80c5db389 155 if(cmd!=NULL) {
sepp_nepp 6:4d8938b686a6 156 cmd->set(moveType, angle_deg, dist_cm);
sepp_nepp 5:efe80c5db389 157 }
sepp_nepp 5:efe80c5db389 158 executeFifo();
sepp_nepp 5:efe80c5db389 159 }
garphil 0:a7fb03c9ea9d 160
sepp_nepp 6:4d8938b686a6 161 void Creabot::moveAndWait(BotCmdVerb moveType, float angle_or_cm)
garphil 0:a7fb03c9ea9d 162 {
sepp_nepp 5:efe80c5db389 163 move(moveType,angle_or_cm);
sepp_nepp 5:efe80c5db389 164 waitEndMove();
garphil 0:a7fb03c9ea9d 165 }
garphil 0:a7fb03c9ea9d 166
sepp_nepp 6:4d8938b686a6 167 void Creabot::moveAndWait(BotCmdVerb moveType, float angle_deg, float dist_cm)
garphil 0:a7fb03c9ea9d 168 {
sepp_nepp 6:4d8938b686a6 169 move(moveType,angle_deg,dist_cm);
sepp_nepp 5:efe80c5db389 170 waitEndMove();
garphil 0:a7fb03c9ea9d 171 }
garphil 0:a7fb03c9ea9d 172
garphil 0:a7fb03c9ea9d 173 void Creabot::flushFifo()
garphil 0:a7fb03c9ea9d 174 {
garphil 0:a7fb03c9ea9d 175 fifoBot.empty();
garphil 0:a7fb03c9ea9d 176 if(executingFifo) botTicker.detach();
garphil 0:a7fb03c9ea9d 177 executingFifo=false;
garphil 0:a7fb03c9ea9d 178 }
sepp_nepp 5:efe80c5db389 179
garphil 0:a7fb03c9ea9d 180 int Creabot::moveInFifo()
garphil 0:a7fb03c9ea9d 181 {
sepp_nepp 5:efe80c5db389 182 return fifoBot.getCount();
sepp_nepp 5:efe80c5db389 183 }
sepp_nepp 5:efe80c5db389 184
sepp_nepp 5:efe80c5db389 185 void Creabot::spirale(float b, float turns) {
sepp_nepp 6:4d8938b686a6 186 float r=0.0f;
sepp_nepp 6:4d8938b686a6 187 float angle_deg = 0.0f;
sepp_nepp 6:4d8938b686a6 188 while(angle_deg < float( 360.0f * turns) ) {
sepp_nepp 5:efe80c5db389 189 this->moveAndWait(RIGHT, 10, r);
sepp_nepp 5:efe80c5db389 190 r=r+b;
sepp_nepp 5:efe80c5db389 191 }
sepp_nepp 5:efe80c5db389 192 }
sepp_nepp 5:efe80c5db389 193
sepp_nepp 5:efe80c5db389 194 //******************************************
sepp_nepp 6:4d8938b686a6 195 // Using the FIFO to queue up Motor Commands
sepp_nepp 5:efe80c5db389 196 //******************************************
sepp_nepp 5:efe80c5db389 197
sepp_nepp 5:efe80c5db389 198 // This is the own callback function of the ticker, that works throught the FIFO stack
sepp_nepp 5:efe80c5db389 199 void Creabot::executeFifo()
sepp_nepp 5:efe80c5db389 200 { if (fifoBot.isEmpty()) flushFifo();
sepp_nepp 5:efe80c5db389 201 else {
sepp_nepp 5:efe80c5db389 202 if(endMove) {
sepp_nepp 6:4d8938b686a6 203 BotCommand *cmd = fifoBot.get();
sepp_nepp 5:efe80c5db389 204 executeCommand(cmd);
sepp_nepp 5:efe80c5db389 205 if (!executingFifo) botTicker.attach_us( callback( this, &Creabot::executeFifo), 3150);
sepp_nepp 5:efe80c5db389 206 executingFifo=true;
sepp_nepp 5:efe80c5db389 207 }
sepp_nepp 5:efe80c5db389 208 }
sepp_nepp 5:efe80c5db389 209 }
sepp_nepp 5:efe80c5db389 210
sepp_nepp 6:4d8938b686a6 211 void Creabot::move(BotCmdVerb moveType, float angle_or_cm)
sepp_nepp 5:efe80c5db389 212 {
sepp_nepp 6:4d8938b686a6 213 current_cmd.set(moveType, angle_or_cm, 0);
sepp_nepp 5:efe80c5db389 214 executeCommand(&current_cmd);
sepp_nepp 5:efe80c5db389 215 }
sepp_nepp 5:efe80c5db389 216
sepp_nepp 6:4d8938b686a6 217 void Creabot::move(BotCmdVerb moveType, float angle_deg, float dist_cm)
sepp_nepp 5:efe80c5db389 218 {
sepp_nepp 6:4d8938b686a6 219 current_cmd.set(moveType, angle_deg, dist_cm);
sepp_nepp 5:efe80c5db389 220 executeCommand(&current_cmd);
sepp_nepp 5:efe80c5db389 221 }
sepp_nepp 5:efe80c5db389 222
sepp_nepp 5:efe80c5db389 223 // Direct Command Execution, called by FIFO-Handler.
sepp_nepp 6:4d8938b686a6 224 void Creabot::executeCommand(BotCommand *cmd) {
garphil 0:a7fb03c9ea9d 225 switch (cmd->command) {
garphil 0:a7fb03c9ea9d 226 case FORWARD:
sepp_nepp 6:4d8938b686a6 227 moveForward(cmd->dist_cm);
garphil 0:a7fb03c9ea9d 228 break;
garphil 0:a7fb03c9ea9d 229 case BACKWARD:
sepp_nepp 6:4d8938b686a6 230 moveBackward(cmd->dist_cm);
garphil 0:a7fb03c9ea9d 231 break;
garphil 0:a7fb03c9ea9d 232 case ROTATE:
sepp_nepp 6:4d8938b686a6 233 rotate(cmd->angle_deg);
garphil 0:a7fb03c9ea9d 234 break;
garphil 0:a7fb03c9ea9d 235 case LEFT:
sepp_nepp 6:4d8938b686a6 236 moveLeft(cmd->angle_deg, cmd->dist_cm);
garphil 0:a7fb03c9ea9d 237 break;
garphil 0:a7fb03c9ea9d 238 case RIGHT:
sepp_nepp 6:4d8938b686a6 239 moveRight(cmd->angle_deg, cmd->dist_cm);
garphil 0:a7fb03c9ea9d 240 break;
garphil 0:a7fb03c9ea9d 241 case IDLE:
garphil 1:974d020bb582 242 // wait(10); // Not to be done in an interrupt. could start a timer with a call back with 'end move'? Would be stopped by stopMove...
garphil 0:a7fb03c9ea9d 243 break;
garphil 0:a7fb03c9ea9d 244 default:
garphil 0:a7fb03c9ea9d 245 break;
garphil 0:a7fb03c9ea9d 246 };
sepp_nepp 5:efe80c5db389 247 }; // executeCommand
sepp_nepp 5:efe80c5db389 248
sepp_nepp 6:4d8938b686a6 249 void Creabot::stopAll()
sepp_nepp 6:4d8938b686a6 250 {
sepp_nepp 6:4d8938b686a6 251 flushFifo();
sepp_nepp 6:4d8938b686a6 252 stopMotors();
sepp_nepp 6:4d8938b686a6 253 }
sepp_nepp 6:4d8938b686a6 254
garphil 0:a7fb03c9ea9d 255
sepp_nepp 5:efe80c5db389 256 //***********************************************
sepp_nepp 5:efe80c5db389 257 // Execute high level motor functions directly, no FIFO
sepp_nepp 5:efe80c5db389 258 //***********************************************
garphil 0:a7fb03c9ea9d 259
sepp_nepp 6:4d8938b686a6 260 void Creabot::moveRight(float angle_deg)
sepp_nepp 5:efe80c5db389 261 {
sepp_nepp 6:4d8938b686a6 262 moveRight(angle_deg,distance_wheel);
garphil 0:a7fb03c9ea9d 263 }
garphil 0:a7fb03c9ea9d 264
sepp_nepp 6:4d8938b686a6 265 void Creabot::moveRight(float angle_deg, float center_cm)
sepp_nepp 5:efe80c5db389 266 {
sepp_nepp 5:efe80c5db389 267 if(center_cm<0) center_cm=0; /* TO remove? */
sepp_nepp 6:4d8938b686a6 268 float perimeter_outer = 2.0f*angle_deg*(center_cm+distance_wheel)*PI/360.0f;
sepp_nepp 6:4d8938b686a6 269 float perimeter_inner = 2.0f*angle_deg*(center_cm)*PI/360.0f;
sepp_nepp 6:4d8938b686a6 270 float angleLeft = perimeter_outer*degree_wheel_per_cm;
sepp_nepp 6:4d8938b686a6 271 float angleRight = perimeter_inner*degree_wheel_per_cm;
sepp_nepp 5:efe80c5db389 272 float ratio = angleRight/angleLeft;
sepp_nepp 5:efe80c5db389 273 float speedRight = last_speed * ratio;
sepp_nepp 5:efe80c5db389 274 setSpeedMot(motor_left,last_speed);
sepp_nepp 5:efe80c5db389 275 setSpeedMot(motor_right,speedRight);
sepp_nepp 5:efe80c5db389 276 moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
sepp_nepp 5:efe80c5db389 277 moveMotorRight(CLOCKWISE, angleRight);
sepp_nepp 5:efe80c5db389 278 }
sepp_nepp 5:efe80c5db389 279
sepp_nepp 6:4d8938b686a6 280 void Creabot::moveLeft(float angle_deg)
sepp_nepp 5:efe80c5db389 281 {
sepp_nepp 6:4d8938b686a6 282 moveLeft(angle_deg,0);
garphil 4:531b1120d3ec 283 }
garphil 4:531b1120d3ec 284
sepp_nepp 6:4d8938b686a6 285 void Creabot::moveLeft(float angle_deg, float center_cm)
garphil 0:a7fb03c9ea9d 286 {
sepp_nepp 5:efe80c5db389 287 if(center_cm<0) center_cm=0; /* TO remove? */
sepp_nepp 6:4d8938b686a6 288 float perimeter_outer = 2.0f*angle_deg*(center_cm+distance_wheel)*PI/360.0f;
sepp_nepp 6:4d8938b686a6 289 float perimeter_inner = 2.0f*angle_deg*(center_cm)*PI/360.0f;
sepp_nepp 5:efe80c5db389 290 float angleLeft = perimeter_inner*degree_wheel_per_cm;
sepp_nepp 5:efe80c5db389 291 float angleRight = perimeter_outer*degree_wheel_per_cm;
sepp_nepp 5:efe80c5db389 292 float ratio = angleLeft/angleRight;
sepp_nepp 5:efe80c5db389 293 float speedRLeft = last_speed * ratio;
sepp_nepp 5:efe80c5db389 294 setSpeedMot(motor_left,speedRLeft);
sepp_nepp 5:efe80c5db389 295 setSpeedMot(motor_right,last_speed);
sepp_nepp 5:efe80c5db389 296 moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
sepp_nepp 5:efe80c5db389 297 moveMotorRight(CLOCKWISE, angleRight);
garphil 0:a7fb03c9ea9d 298 }
garphil 0:a7fb03c9ea9d 299
sepp_nepp 5:efe80c5db389 300 //******************************************************************************
sepp_nepp 6:4d8938b686a6 301 // low level motor functions, filters for angle_deg>0, remembers that a motor moves
sepp_nepp 5:efe80c5db389 302 //******************************************************************************
garphil 0:a7fb03c9ea9d 303
sepp_nepp 6:4d8938b686a6 304 void Creabot::moveMotorLeft(motorDir dir, float angle_deg)
garphil 0:a7fb03c9ea9d 305 {
sepp_nepp 6:4d8938b686a6 306 if(angle_deg>0) {
sepp_nepp 6:4d8938b686a6 307 motor_left->RunDegrees(dir, angle_deg);
sepp_nepp 6:4d8938b686a6 308 if (MotState==RMOT_RUNS) MotState=MOTORS_RUN;
sepp_nepp 6:4d8938b686a6 309 else MotState=LMOT_RUNS;
garphil 0:a7fb03c9ea9d 310 }
garphil 0:a7fb03c9ea9d 311 }
sepp_nepp 5:efe80c5db389 312
sepp_nepp 6:4d8938b686a6 313 void Creabot::moveMotorRight(motorDir dir, float angle_deg)
sepp_nepp 6:4d8938b686a6 314 {
sepp_nepp 6:4d8938b686a6 315 if(angle_deg>0) {
sepp_nepp 6:4d8938b686a6 316 motor_right->RunDegrees(dir, angle_deg);
sepp_nepp 6:4d8938b686a6 317 if (MotState==LMOT_RUNS) MotState=MOTORS_RUN;
sepp_nepp 6:4d8938b686a6 318 else MotState=RMOT_RUNS;
sepp_nepp 6:4d8938b686a6 319 }
sepp_nepp 5:efe80c5db389 320 }
sepp_nepp 5:efe80c5db389 321
sepp_nepp 6:4d8938b686a6 322 void Creabot::cbLeftMotStopped()
sepp_nepp 5:efe80c5db389 323 {
sepp_nepp 6:4d8938b686a6 324 if (MotState==MOTORS_RUN) MotState = RMOT_RUNS;
sepp_nepp 6:4d8938b686a6 325 else AllMotorsStopped();
sepp_nepp 5:efe80c5db389 326 }
sepp_nepp 5:efe80c5db389 327
sepp_nepp 6:4d8938b686a6 328 void Creabot::cbRightMotStopped()
sepp_nepp 6:4d8938b686a6 329 {
sepp_nepp 6:4d8938b686a6 330 if (MotState==MOTORS_RUN) MotState = LMOT_RUNS;
sepp_nepp 6:4d8938b686a6 331 else AllMotorsStopped();
sepp_nepp 6:4d8938b686a6 332 }
sepp_nepp 6:4d8938b686a6 333
sepp_nepp 6:4d8938b686a6 334 void Creabot::AllMotorsStopped()
sepp_nepp 6:4d8938b686a6 335 {
sepp_nepp 6:4d8938b686a6 336 if (MotState!=ALLSTOP)
sepp_nepp 6:4d8938b686a6 337 { // Only do all this here if motor state is not already in ALLSTOP!
sepp_nepp 6:4d8938b686a6 338 MotState = AlLSTOP;
sepp_nepp 6:4d8938b686a6 339 if(extCallBack!=NULL) extCallBack( MotState );
sepp_nepp 5:efe80c5db389 340 }
sepp_nepp 5:efe80c5db389 341 }
sepp_nepp 5:efe80c5db389 342
sepp_nepp 6:4d8938b686a6 343
sepp_nepp 6:4d8938b686a6 344 void Creabot::stopMotors()
sepp_nepp 6:4d8938b686a6 345 {
sepp_nepp 6:4d8938b686a6 346 motor_left->MotorOFF();
sepp_nepp 6:4d8938b686a6 347 motor_right->MotorOFF();
sepp_nepp 6:4d8938b686a6 348 AllMotorsStopped()
sepp_nepp 6:4d8938b686a6 349 }
sepp_nepp 6:4d8938b686a6 350
sepp_nepp 6:4d8938b686a6 351 void Creabot::setSpeed(float speed_cm_sec)
sepp_nepp 6:4d8938b686a6 352 {
sepp_nepp 6:4d8938b686a6 353 if (MAX_SPEED_CM_P_SEC)
sepp_nepp 6:4d8938b686a6 354 setSpeedMot(motor_left,speed_cm_sec);
sepp_nepp 6:4d8938b686a6 355 setSpeedMot(motor_right,speed_cm_sec);
sepp_nepp 6:4d8938b686a6 356 last_speed = speed_cm_sec;
sepp_nepp 6:4d8938b686a6 357 }
sepp_nepp 6:4d8938b686a6 358
sepp_nepp 6:4d8938b686a6 359 void Creabot::setSpeedMot(Motor *motor, float speed_cm_sec)
sepp_nepp 6:4d8938b686a6 360 { if (speed_cm_sec <0.0001f)
sepp_nepp 6:4d8938b686a6 361 motor->setStepTime_us( 1000000 );
sepp_nepp 6:4d8938b686a6 362 else { uint32_t tick = motor->getStepsFullTurn();
sepp_nepp 6:4d8938b686a6 363 motor->setStepTime_us( perimeter_wheel * 1000000 / speed_cm_sec / tick ) ;
sepp_nepp 6:4d8938b686a6 364 }
sepp_nepp 6:4d8938b686a6 365 }
sepp_nepp 6:4d8938b686a6 366