Tarek Lule / CreaBotLib

My Version of CreaBotLib

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