Tarek Lule / CreaBotLib

My Version of CreaBotLib

Fork of CreaBotLib by CreaLab

CreaBot.cpp@5:efe80c5db389, 2018-10-31 (annotated)

Committer:
sepp_nepp
Date:
Wed Oct 31 14:36:07 2018 +0000
Revision:
5:efe80c5db389
Parent:
4:531b1120d3ec
Child:
6:4d8938b686a6
First publish of my CreaBotLib Version

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 5:efe80c5db389 4 // Motor Command and Command-FIFO classes
sepp_nepp 5:efe80c5db389 5 // *****************************************************************
sepp_nepp 5:efe80c5db389 6
sepp_nepp 5:efe80c5db389 7 void MotCommand::assign(cmdbot_t cmd, float p1, float p2)
sepp_nepp 5:efe80c5db389 8 { command = cmd;
sepp_nepp 5:efe80c5db389 9 switch (command) {
sepp_nepp 5:efe80c5db389 10 case FORWARD : ;
sepp_nepp 5:efe80c5db389 11 case BACKWARD: cm=p1; angle=0; break;
sepp_nepp 5:efe80c5db389 12 case ROTATE : angle = p1; break;
sepp_nepp 5:efe80c5db389 13 case LEFT :
sepp_nepp 5:efe80c5db389 14 case RIGHT : angle = p1; cm=p2; break;
sepp_nepp 5:efe80c5db389 15 case IDLE : break;
sepp_nepp 5:efe80c5db389 16 default : break;
sepp_nepp 5:efe80c5db389 17 };
sepp_nepp 5:efe80c5db389 18 };
sepp_nepp 5:efe80c5db389 19
sepp_nepp 5:efe80c5db389 20 MotCommand *CommandFIFO::put() {
sepp_nepp 5:efe80c5db389 21 if(count<DEPTH_FIFO) {
sepp_nepp 5:efe80c5db389 22 int old_writeIdx = writeIdx;
sepp_nepp 5:efe80c5db389 23 writeIdx++;
sepp_nepp 5:efe80c5db389 24 if (writeIdx==DEPTH_FIFO) writeIdx=0;
sepp_nepp 5:efe80c5db389 25 count++;
sepp_nepp 5:efe80c5db389 26 return &cmd[old_writeIdx];
sepp_nepp 5:efe80c5db389 27 }
sepp_nepp 5:efe80c5db389 28 else
sepp_nepp 5:efe80c5db389 29 return NULL;
sepp_nepp 5:efe80c5db389 30 }; // put()
sepp_nepp 5:efe80c5db389 31
sepp_nepp 5:efe80c5db389 32 MotCommand *CommandFIFO::get() {
sepp_nepp 5:efe80c5db389 33 if ( !isEmpty() ) {
sepp_nepp 5:efe80c5db389 34 int old_readIdx = readIdx;
sepp_nepp 5:efe80c5db389 35 readIdx++;
sepp_nepp 5:efe80c5db389 36 if (readIdx==DEPTH_FIFO) readIdx=0;
sepp_nepp 5:efe80c5db389 37 count--;
sepp_nepp 5:efe80c5db389 38 return &cmd[old_readIdx];
sepp_nepp 5:efe80c5db389 39 }
sepp_nepp 5:efe80c5db389 40 else
sepp_nepp 5:efe80c5db389 41 return NULL;
sepp_nepp 5:efe80c5db389 42 }; // get()
sepp_nepp 5:efe80c5db389 43
sepp_nepp 5:efe80c5db389 44 // *****************************************************************
sepp_nepp 5:efe80c5db389 45 // Creabot Class
sepp_nepp 5:efe80c5db389 46 // *****************************************************************
sepp_nepp 5:efe80c5db389 47
sepp_nepp 5:efe80c5db389 48 Creabot::Creabot(Motor *left, Motor *right, float diameter_wheel_cm, float distance_wheel_cm):
sepp_nepp 5:efe80c5db389 49 motor_left(left), motor_right(right), diameter_wheel(diameter_wheel_cm), distance_wheel(distance_wheel_cm)
garphil 0:a7fb03c9ea9d 50 {
garphil 0:a7fb03c9ea9d 51 perimeter_wheel = PI*diameter_wheel;
garphil 0:a7fb03c9ea9d 52 perimeter_bot = PI*distance_wheel;
garphil 0:a7fb03c9ea9d 53 degree_wheel_per_cm=360.0f/perimeter_wheel;
garphil 0:a7fb03c9ea9d 54 degree_bot_per_cm=360.0f/perimeter_bot;;
garphil 3:891b6a138383 55 ratio_wheel_bot=perimeter_bot/perimeter_wheel;
garphil 0:a7fb03c9ea9d 56 status=0;
garphil 0:a7fb03c9ea9d 57
sepp_nepp 5:efe80c5db389 58 motor_left->setMotorCallback(this, &Creabot::callBackLeft);
sepp_nepp 5:efe80c5db389 59 motor_right->setMotorCallback(this, &Creabot::callBackRight);
garphil 0:a7fb03c9ea9d 60 extCallBack = NULL;
garphil 0:a7fb03c9ea9d 61 setSpeed(DEFAULT_SPEED);
sepp_nepp 5:efe80c5db389 62 endMoveLeft = true;
sepp_nepp 5:efe80c5db389 63 endMoveRight = true;
garphil 0:a7fb03c9ea9d 64 endMove=true;
garphil 0:a7fb03c9ea9d 65 executingFifo=false;
garphil 0:a7fb03c9ea9d 66 }
garphil 0:a7fb03c9ea9d 67
garphil 0:a7fb03c9ea9d 68 void Creabot::moveForward(float cm)
garphil 0:a7fb03c9ea9d 69 {
sepp_nepp 5:efe80c5db389 70 if (cm<0) moveBackward(cm);
sepp_nepp 5:efe80c5db389 71 else {
sepp_nepp 5:efe80c5db389 72 float angle = cm*degree_wheel_per_cm;
sepp_nepp 5:efe80c5db389 73 setSpeed(last_speed);
sepp_nepp 5:efe80c5db389 74 moveMotorLeft(COUNTERCLOCKWISE, angle);
sepp_nepp 5:efe80c5db389 75 moveMotorRight(CLOCKWISE, angle);
sepp_nepp 5:efe80c5db389 76 }
garphil 0:a7fb03c9ea9d 77 }
garphil 0:a7fb03c9ea9d 78
garphil 0:a7fb03c9ea9d 79 void Creabot::moveBackward(float cm)
garphil 0:a7fb03c9ea9d 80 {
garphil 0:a7fb03c9ea9d 81 if(cm<0) moveForward(cm);
sepp_nepp 5:efe80c5db389 82 else {
sepp_nepp 5:efe80c5db389 83 float angle = cm*degree_wheel_per_cm;
sepp_nepp 5:efe80c5db389 84 setSpeed(last_speed);
sepp_nepp 5:efe80c5db389 85 moveMotorLeft(CLOCKWISE, angle);
sepp_nepp 5:efe80c5db389 86 moveMotorRight(COUNTERCLOCKWISE, angle);
sepp_nepp 5:efe80c5db389 87 }
garphil 0:a7fb03c9ea9d 88 }
garphil 0:a7fb03c9ea9d 89 void Creabot::rotate(float angleBot)
garphil 0:a7fb03c9ea9d 90 {
sepp_nepp 5:efe80c5db389 91 float angleWheel = angleBot*ratio_wheel_bot;
sepp_nepp 5:efe80c5db389 92 setSpeed(last_speed);
garphil 2:3d2d6d655d01 93 if(angleBot<0) {
garphil 2:3d2d6d655d01 94 moveMotorLeft(CLOCKWISE, -angleWheel);
garphil 2:3d2d6d655d01 95 moveMotorRight(CLOCKWISE, -angleWheel);
garphil 0:a7fb03c9ea9d 96 } else {
garphil 2:3d2d6d655d01 97 moveMotorLeft(COUNTERCLOCKWISE, angleWheel);
garphil 2:3d2d6d655d01 98 moveMotorRight(COUNTERCLOCKWISE, angleWheel);
garphil 0:a7fb03c9ea9d 99 }
garphil 0:a7fb03c9ea9d 100 }
garphil 0:a7fb03c9ea9d 101
garphil 0:a7fb03c9ea9d 102 void Creabot::stopMove()
garphil 0:a7fb03c9ea9d 103 {
sepp_nepp 5:efe80c5db389 104 motor_left->MotorOFF();
sepp_nepp 5:efe80c5db389 105 motor_right->MotorOFF();
sepp_nepp 5:efe80c5db389 106 endMoveLeft = true;
sepp_nepp 5:efe80c5db389 107 endMoveRight = true;
garphil 0:a7fb03c9ea9d 108 endMove = true;
garphil 0:a7fb03c9ea9d 109 flushFifo();
garphil 0:a7fb03c9ea9d 110 // callBack(); // ? To be called or set as parameter?
garphil 0:a7fb03c9ea9d 111 }
garphil 0:a7fb03c9ea9d 112
garphil 0:a7fb03c9ea9d 113 void Creabot::waitEndMove()
garphil 0:a7fb03c9ea9d 114 {
garphil 0:a7fb03c9ea9d 115 while(!endMove || executingFifo) {
garphil 0:a7fb03c9ea9d 116 wait(0.1);
garphil 0:a7fb03c9ea9d 117 }
garphil 1:974d020bb582 118
garphil 0:a7fb03c9ea9d 119 }
garphil 0:a7fb03c9ea9d 120 void Creabot::waitEndMove(uint32_t delay_us)
garphil 0:a7fb03c9ea9d 121 {
garphil 0:a7fb03c9ea9d 122 uint32_t v=0;
garphil 0:a7fb03c9ea9d 123 while(!endMove || executingFifo) {
garphil 0:a7fb03c9ea9d 124 wait(0.1);
garphil 0:a7fb03c9ea9d 125 v++;
garphil 0:a7fb03c9ea9d 126 if((v*10000)>delay_us) endMove = true;
garphil 0:a7fb03c9ea9d 127 }
garphil 0:a7fb03c9ea9d 128 } // watchdog
garphil 0:a7fb03c9ea9d 129
garphil 0:a7fb03c9ea9d 130 void Creabot::setCallBack(void (*callback)(int status))
garphil 0:a7fb03c9ea9d 131 {
garphil 0:a7fb03c9ea9d 132 extCallBack = callback;
garphil 0:a7fb03c9ea9d 133 }
garphil 0:a7fb03c9ea9d 134
garphil 0:a7fb03c9ea9d 135 void Creabot::fifo(cmdbot_t moveType, float angle_or_cm)
garphil 0:a7fb03c9ea9d 136 {
sepp_nepp 5:efe80c5db389 137 MotCommand *cmd = fifoBot.put();
sepp_nepp 5:efe80c5db389 138 if(cmd != NULL) {
sepp_nepp 5:efe80c5db389 139 cmd->assign(moveType, angle_or_cm, 0);
sepp_nepp 5:efe80c5db389 140 }
garphil 0:a7fb03c9ea9d 141 executeFifo();
garphil 0:a7fb03c9ea9d 142 }
garphil 0:a7fb03c9ea9d 143
sepp_nepp 5:efe80c5db389 144 void Creabot::fifo(cmdbot_t moveType, float angle, float cm)
sepp_nepp 5:efe80c5db389 145 {
sepp_nepp 5:efe80c5db389 146 MotCommand *cmd = fifoBot.put();
sepp_nepp 5:efe80c5db389 147 if(cmd!=NULL) {
sepp_nepp 5:efe80c5db389 148 cmd->assign(moveType, angle, cm);
sepp_nepp 5:efe80c5db389 149 }
sepp_nepp 5:efe80c5db389 150 executeFifo();
sepp_nepp 5:efe80c5db389 151 }
garphil 0:a7fb03c9ea9d 152
garphil 0:a7fb03c9ea9d 153 void Creabot::moveAndWait(cmdbot_t moveType, float angle_or_cm)
garphil 0:a7fb03c9ea9d 154 {
sepp_nepp 5:efe80c5db389 155 move(moveType,angle_or_cm);
sepp_nepp 5:efe80c5db389 156 waitEndMove();
garphil 0:a7fb03c9ea9d 157 }
garphil 0:a7fb03c9ea9d 158
sepp_nepp 5:efe80c5db389 159 void Creabot::moveAndWait(cmdbot_t moveType, float angle, float cm)
garphil 0:a7fb03c9ea9d 160 {
sepp_nepp 5:efe80c5db389 161 move(moveType,angle,cm);
sepp_nepp 5:efe80c5db389 162 waitEndMove();
garphil 0:a7fb03c9ea9d 163 }
garphil 0:a7fb03c9ea9d 164
garphil 0:a7fb03c9ea9d 165 void Creabot::flushFifo()
garphil 0:a7fb03c9ea9d 166 {
garphil 0:a7fb03c9ea9d 167 fifoBot.empty();
garphil 0:a7fb03c9ea9d 168 if(executingFifo) botTicker.detach();
garphil 0:a7fb03c9ea9d 169 executingFifo=false;
garphil 0:a7fb03c9ea9d 170 }
sepp_nepp 5:efe80c5db389 171
garphil 0:a7fb03c9ea9d 172 int Creabot::moveInFifo()
garphil 0:a7fb03c9ea9d 173 {
sepp_nepp 5:efe80c5db389 174 return fifoBot.getCount();
sepp_nepp 5:efe80c5db389 175 }
sepp_nepp 5:efe80c5db389 176
sepp_nepp 5:efe80c5db389 177 void Creabot::spirale(float b, float turns) {
sepp_nepp 5:efe80c5db389 178 float r=0.0;
sepp_nepp 5:efe80c5db389 179 float angle = 0.0f;
sepp_nepp 5:efe80c5db389 180 while(angle < float( 360.0 * turns) ) {
sepp_nepp 5:efe80c5db389 181 this->moveAndWait(RIGHT, 10, r);
sepp_nepp 5:efe80c5db389 182 r=r+b;
sepp_nepp 5:efe80c5db389 183 }
sepp_nepp 5:efe80c5db389 184 }
sepp_nepp 5:efe80c5db389 185
sepp_nepp 5:efe80c5db389 186 void Creabot::callBackLeft()
sepp_nepp 5:efe80c5db389 187 {
sepp_nepp 5:efe80c5db389 188 endMoveLeft=true;
sepp_nepp 5:efe80c5db389 189 if(endMoveRight)callBack();
sepp_nepp 5:efe80c5db389 190 }
sepp_nepp 5:efe80c5db389 191
sepp_nepp 5:efe80c5db389 192 void Creabot::callBackRight()
sepp_nepp 5:efe80c5db389 193 {
sepp_nepp 5:efe80c5db389 194 endMoveRight=true;
sepp_nepp 5:efe80c5db389 195 if(endMoveLeft)callBack();
sepp_nepp 5:efe80c5db389 196 }
sepp_nepp 5:efe80c5db389 197
sepp_nepp 5:efe80c5db389 198 void Creabot::callBack()
sepp_nepp 5:efe80c5db389 199 {
sepp_nepp 5:efe80c5db389 200 endMove=true;
sepp_nepp 5:efe80c5db389 201 if(extCallBack!=NULL) extCallBack(status);
garphil 0:a7fb03c9ea9d 202 }
sepp_nepp 5:efe80c5db389 203
sepp_nepp 5:efe80c5db389 204
sepp_nepp 5:efe80c5db389 205 //******************************************
sepp_nepp 5:efe80c5db389 206 // Executing the FIFO of Motor Commands
sepp_nepp 5:efe80c5db389 207 //******************************************
sepp_nepp 5:efe80c5db389 208
sepp_nepp 5:efe80c5db389 209 // This is the own callback function of the ticker, that works throught the FIFO stack
sepp_nepp 5:efe80c5db389 210 void Creabot::executeFifo()
sepp_nepp 5:efe80c5db389 211 { if (fifoBot.isEmpty()) flushFifo();
sepp_nepp 5:efe80c5db389 212 else {
sepp_nepp 5:efe80c5db389 213 if(endMove) {
sepp_nepp 5:efe80c5db389 214 MotCommand *cmd = fifoBot.get();
sepp_nepp 5:efe80c5db389 215 executeCommand(cmd);
sepp_nepp 5:efe80c5db389 216 if (!executingFifo) botTicker.attach_us( callback( this, &Creabot::executeFifo), 3150);
sepp_nepp 5:efe80c5db389 217 executingFifo=true;
sepp_nepp 5:efe80c5db389 218 }
sepp_nepp 5:efe80c5db389 219 }
sepp_nepp 5:efe80c5db389 220 }
sepp_nepp 5:efe80c5db389 221
sepp_nepp 5:efe80c5db389 222 void Creabot::move(cmdbot_t moveType, float angle_or_cm)
sepp_nepp 5:efe80c5db389 223 {
sepp_nepp 5:efe80c5db389 224 current_cmd.assign(moveType, angle_or_cm, 0);
sepp_nepp 5:efe80c5db389 225 executeCommand(&current_cmd);
sepp_nepp 5:efe80c5db389 226 }
sepp_nepp 5:efe80c5db389 227
sepp_nepp 5:efe80c5db389 228 void Creabot::move(cmdbot_t moveType, float angle, float cm)
sepp_nepp 5:efe80c5db389 229 {
sepp_nepp 5:efe80c5db389 230 current_cmd.assign(moveType, angle, cm);
sepp_nepp 5:efe80c5db389 231 executeCommand(&current_cmd);
sepp_nepp 5:efe80c5db389 232 }
sepp_nepp 5:efe80c5db389 233
sepp_nepp 5:efe80c5db389 234 // Direct Command Execution, called by FIFO-Handler.
sepp_nepp 5:efe80c5db389 235 void Creabot::executeCommand(MotCommand *cmd) {
garphil 0:a7fb03c9ea9d 236 switch (cmd->command) {
garphil 0:a7fb03c9ea9d 237 case FORWARD:
garphil 0:a7fb03c9ea9d 238 moveForward(cmd->cm);
garphil 0:a7fb03c9ea9d 239 break;
garphil 0:a7fb03c9ea9d 240 case BACKWARD:
garphil 0:a7fb03c9ea9d 241 moveBackward(cmd->cm);
garphil 0:a7fb03c9ea9d 242 break;
garphil 0:a7fb03c9ea9d 243 case ROTATE:
garphil 0:a7fb03c9ea9d 244 rotate(cmd->angle);
garphil 0:a7fb03c9ea9d 245 break;
garphil 0:a7fb03c9ea9d 246 case LEFT:
garphil 0:a7fb03c9ea9d 247 moveLeft(cmd->angle, cmd->cm);
garphil 0:a7fb03c9ea9d 248 break;
garphil 0:a7fb03c9ea9d 249 case RIGHT:
garphil 0:a7fb03c9ea9d 250 moveRight(cmd->angle, cmd->cm);
garphil 0:a7fb03c9ea9d 251 break;
garphil 0:a7fb03c9ea9d 252 case IDLE:
garphil 1:974d020bb582 253 // 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 254 break;
garphil 0:a7fb03c9ea9d 255 default:
garphil 0:a7fb03c9ea9d 256 break;
garphil 0:a7fb03c9ea9d 257 };
sepp_nepp 5:efe80c5db389 258 }; // executeCommand
sepp_nepp 5:efe80c5db389 259
garphil 0:a7fb03c9ea9d 260
sepp_nepp 5:efe80c5db389 261 //***********************************************
sepp_nepp 5:efe80c5db389 262 // Execute high level motor functions directly, no FIFO
sepp_nepp 5:efe80c5db389 263 //***********************************************
garphil 0:a7fb03c9ea9d 264
sepp_nepp 5:efe80c5db389 265 void Creabot::moveRight(float angle)
sepp_nepp 5:efe80c5db389 266 {
sepp_nepp 5:efe80c5db389 267 moveRight(angle,distance_wheel);
garphil 0:a7fb03c9ea9d 268 }
garphil 0:a7fb03c9ea9d 269
sepp_nepp 5:efe80c5db389 270 void Creabot::moveRight(float angle, float center_cm)
sepp_nepp 5:efe80c5db389 271 {
sepp_nepp 5:efe80c5db389 272 if(center_cm<0) center_cm=0; /* TO remove? */
sepp_nepp 5:efe80c5db389 273 float perimeter_outer = 2.0f*angle*(center_cm+distance_wheel)*PI/360.0f;
sepp_nepp 5:efe80c5db389 274 float perimeter_inner = 2.0f*angle*(center_cm)*PI/360.0f;
sepp_nepp 5:efe80c5db389 275 float angleLeft = perimeter_outer*degree_wheel_per_cm;
sepp_nepp 5:efe80c5db389 276 float angleRight = perimeter_inner*degree_wheel_per_cm;
sepp_nepp 5:efe80c5db389 277 float ratio = angleRight/angleLeft;
sepp_nepp 5:efe80c5db389 278 float speedRight = last_speed * ratio;
sepp_nepp 5:efe80c5db389 279 // pc_uart.printf("Set Perimeter angle =%f center_cm=%f outer = %f inner = %f angle_left %f angle right %f ratio %f cs %f\n\r",angle, center_cm, perimeter_outer,perimeter_inner,angleLeft,angleRight,ratio, last_speed);
sepp_nepp 5:efe80c5db389 280 setSpeedMot(motor_left,last_speed);
sepp_nepp 5:efe80c5db389 281 setSpeedMot(motor_right,speedRight);
sepp_nepp 5:efe80c5db389 282 moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
sepp_nepp 5:efe80c5db389 283 moveMotorRight(CLOCKWISE, angleRight);
sepp_nepp 5:efe80c5db389 284 }
sepp_nepp 5:efe80c5db389 285
sepp_nepp 5:efe80c5db389 286 void Creabot::moveLeft(float angle)
sepp_nepp 5:efe80c5db389 287 {
sepp_nepp 5:efe80c5db389 288 moveLeft(angle,0);
garphil 4:531b1120d3ec 289 }
garphil 4:531b1120d3ec 290
sepp_nepp 5:efe80c5db389 291 void Creabot::moveLeft(float angle, float center_cm)
garphil 0:a7fb03c9ea9d 292 {
sepp_nepp 5:efe80c5db389 293 if(center_cm<0) center_cm=0; /* TO remove? */
sepp_nepp 5:efe80c5db389 294 float perimeter_outer = 2.0f*angle*(center_cm+distance_wheel)*PI/360.0f;
sepp_nepp 5:efe80c5db389 295 float perimeter_inner = 2.0f*angle*(center_cm)*PI/360.0f;
sepp_nepp 5:efe80c5db389 296 float angleLeft = perimeter_inner*degree_wheel_per_cm;
sepp_nepp 5:efe80c5db389 297 float angleRight = perimeter_outer*degree_wheel_per_cm;
sepp_nepp 5:efe80c5db389 298 float ratio = angleLeft/angleRight;
sepp_nepp 5:efe80c5db389 299 float speedRLeft = last_speed * ratio;
sepp_nepp 5:efe80c5db389 300 setSpeedMot(motor_left,speedRLeft);
sepp_nepp 5:efe80c5db389 301 setSpeedMot(motor_right,last_speed);
sepp_nepp 5:efe80c5db389 302 moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
sepp_nepp 5:efe80c5db389 303 moveMotorRight(CLOCKWISE, angleRight);
garphil 0:a7fb03c9ea9d 304 }
garphil 0:a7fb03c9ea9d 305
sepp_nepp 5:efe80c5db389 306 //******************************************************************************
sepp_nepp 5:efe80c5db389 307 // low level motor functions, filters for angle>0, remembers that a motor moves
sepp_nepp 5:efe80c5db389 308 //******************************************************************************
garphil 0:a7fb03c9ea9d 309
sepp_nepp 5:efe80c5db389 310 void Creabot::moveMotorLeft(motorDir dir, float angle)
garphil 0:a7fb03c9ea9d 311 {
garphil 0:a7fb03c9ea9d 312 if(angle>0) {
garphil 0:a7fb03c9ea9d 313 motor_left->RunDegrees(dir, angle);
sepp_nepp 5:efe80c5db389 314 endMove = endMoveLeft = false;
garphil 0:a7fb03c9ea9d 315 }
garphil 0:a7fb03c9ea9d 316 }
sepp_nepp 5:efe80c5db389 317
sepp_nepp 5:efe80c5db389 318 void Creabot::moveMotorRight(motorDir dir, float angle)
sepp_nepp 5:efe80c5db389 319 { if(angle>0) {
garphil 0:a7fb03c9ea9d 320 motor_right->RunDegrees(dir, angle);
sepp_nepp 5:efe80c5db389 321 endMove = endMoveRight = false;}
sepp_nepp 5:efe80c5db389 322 }
sepp_nepp 5:efe80c5db389 323
sepp_nepp 5:efe80c5db389 324 int Creabot::getStatus()
sepp_nepp 5:efe80c5db389 325 { if(endMove ) return 0; // No Movement
sepp_nepp 5:efe80c5db389 326 if(endMoveRight) return 1; // Returns 1, assuming that only Left Motor is still running
sepp_nepp 5:efe80c5db389 327 if(endMoveLeft ) return 3; // Returns 2, Right Motor is running aloone
sepp_nepp 5:efe80c5db389 328 return 2; // Both Motors are still running!!!
sepp_nepp 5:efe80c5db389 329 }
sepp_nepp 5:efe80c5db389 330
sepp_nepp 5:efe80c5db389 331 void Creabot::setSpeed(float cm_per_second)
sepp_nepp 5:efe80c5db389 332 {
sepp_nepp 5:efe80c5db389 333 setSpeedMot(motor_left,cm_per_second);
sepp_nepp 5:efe80c5db389 334 setSpeedMot(motor_right,cm_per_second);
sepp_nepp 5:efe80c5db389 335 last_speed = cm_per_second;
sepp_nepp 5:efe80c5db389 336 }
sepp_nepp 5:efe80c5db389 337
sepp_nepp 5:efe80c5db389 338 void Creabot::setSpeedMot(Motor *motor, float cm_per_second)
sepp_nepp 5:efe80c5db389 339 { if (cm_per_second <0.0001f)
sepp_nepp 5:efe80c5db389 340 motor->setStepTime_us( 1000000 );
sepp_nepp 5:efe80c5db389 341 else { uint32_t tick = motor->getStepsFullTurn();
sepp_nepp 5:efe80c5db389 342 motor->setStepTime_us( perimeter_wheel * 1000000 / cm_per_second / tick ) ;
sepp_nepp 5:efe80c5db389 343 }
sepp_nepp 5:efe80c5db389 344 }
sepp_nepp 5:efe80c5db389 345