Go to the documentation of this file.00001
00005
00006
00007 #include "motorsControl.h"
00008
00009
00011
00012
00013 Motors::Motors()
00014 {
00015 #ifdef __DEBUG__MOTORS__C__
00016
00017 #endif
00018
00019 Serial1.begin(115200);
00020 Serial2.begin(115200);
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 }
00032
00033 Motors::~Motors()
00034 {
00035 #ifdef __DEBUG__MOTORS__C__
00036 Serial.println("Communication with motors close");
00037 #endif
00038 DisableLeft();
00039 DisableRight();
00040
00041 Serial1.end();
00042 Serial2.end();
00043 }
00044
00045
00046 void Motors::EnableLeft()
00047 {
00048 #ifdef __DEBUG__MOTORS__C__
00049 Serial.println("Enabling left ... ");
00050 #endif
00051
00052 EnableLeftInstruction();
00053 WaitLeftAnswer();
00054 }
00055
00056 void Motors::EnableRight()
00057 {
00058 #ifdef __DEBUG__MOTORS__C__
00059 Serial.println("Enabling right ... ");
00060 #endif
00061
00062 Serial2.println("EN");
00063 }
00064
00065
00066 void Motors::DisableLeft()
00067 {
00068 #ifdef __DEBUG__MOTORS__C__
00069 Serial.println("Disabling left ... ");
00070 #endif
00071
00072 Serial1.println("DI");
00073 }
00074
00075 void Motors::DisableRight()
00076 {
00077 #ifdef __DEBUG__MOTORS__C__
00078 Serial.println("Disabling right... ");
00079 #endif
00080
00081 Serial2.println("DI");
00082 }
00083
00084
00085
00086 void Motors::ResetLeft()
00087 {
00088 #ifdef __DEBUG__MOTORS__C__
00089 Serial.println("Reset left ... ");
00090 #endif
00091
00092 Serial1.println("RESET");
00093 }
00094
00095 void Motors::ResetRight()
00096 {
00097 #ifdef __DEBUG__MOTORS__C__
00098
00099 Serial.println("Reset right... ");
00100 #endif
00101 Serial2.println("RESET");
00102 char Buffer[30]="";
00103 ReadBufferRight(Buffer,29,10);
00104
00105
00106
00107
00108
00109 }
00110
00111
00112
00113 bool Motors::WaitLeftAnswer()
00114 {
00115
00116 #ifdef __DEBUG__MOTORS__C__
00117 Serial.println("Waiting for left answer... ");
00118 #endif
00119 char Buffer[3]="";
00120 int i=0;
00121 do {
00122 while(!Serial1.available());
00123 Buffer[i++]=Serial1.read();
00124 }
00125 while (Buffer[i-1] != '\n' && i<4);
00126
00127 #ifdef __DEBUG__MOTORS__C__
00128 Buffer[i]=0;
00129 Serial.print(Buffer);
00130 #endif
00131
00132 if(strcmp (Buffer,"OK\n")==0);
00133 return true;
00134 return false;
00135 }
00136
00137 bool Motors::ReadBufferRight (char *Buffer,int NbMax,int TimeOut)
00138 {
00139 int i=0;
00140 do {
00141 while(!Serial2.available());
00142 Buffer[i++]=Serial2.read();
00143 }
00144 while (Buffer[i-1] != '\n' && i<NbMax);
00145 Buffer[i]=0;
00146
00147 #ifdef __DEBUG__MOTORS__C__
00148
00149 #endif
00150 }
00151
00152 bool Motors::WaitRightAnswer()
00153 {
00154 #ifdef __DEBUG__MOTORS__C__
00155 Serial.println("Waiting for right answer... ");
00156 #endif
00157 char Buffer[4]="";
00158 ReadBufferRight(Buffer,3,10);
00159
00160
00161 if(strcmp (Buffer,"OK\n")==0)
00162 return true;
00163 return false;
00164 }
00165
00166
00168
00169
00170
00171 void Motors::SetLeftSpeed(int Vit)
00172 {
00173 #ifdef __DEBUG__MOTORS__C__
00174 Serial.println("Setting left speed... ");
00175 #endif
00176
00177 Serial1.print("V");
00178 Serial1.println(Vit);
00179
00180
00181 }
00182
00183 void Motors::SetRightSpeed(int Vit)
00184 {
00185 #ifdef __DEBUG__MOTORS__C__
00186 Serial.println("Setting right speed... ");
00187 #endif
00188
00189 Serial2.print("V");
00190 Serial2.println(-Vit);
00191
00192
00193 }
00194
00195
00196
00197 void Motors::SetLeftAbsolutePos(int Pos)
00198 {
00199 #ifdef __DEBUG__MOTORS__C__
00200 Serial.println("Setting left absolute position... ");
00201 #endif
00202
00203 Serial1.print("LA");
00204 Serial1.println(Pos);
00205 Serial1.println("M");
00206 }
00207
00208 void Motors::SetRightAbsolutePos(int Pos)
00209 {
00210 #ifdef __DEBUG__MOTORS__C__
00211 Serial.println("Setting right absolute position... ");
00212 #endif
00213
00214 Serial2.print("LA");
00215 Serial2.println(Pos);
00216 Serial2.println("M");
00217 }
00218
00219
00220
00221 void Motors::SetLeftRelativePos(int Pos)
00222 {
00223 #ifdef __DEBUG__MOTORS__C__
00224 Serial.println("Setting left relative position... ");
00225 #endif
00226
00227 Serial1.print("LR");
00228 Serial1.println(Pos);
00229
00230 Serial1.println("M");
00231 }
00232
00233 void Motors::SetRightRelativePos(int Pos)
00234 {
00235 #ifdef __DEBUG__MOTORS__C__
00236 Serial.println("Setting right relative position... ");
00237 #endif
00238
00239 Serial.print("LR");
00240 Serial.println(Pos);
00241
00242 Serial2.print("LR");
00243 Serial2.println(Pos);
00244
00245
00246 Serial2.println("M");
00247 }
00248
00249
00250
00251 void Motors::SetLeftAcc(int Acc)
00252 {
00253 #ifdef __DEBUG__MOTORS__C__
00254 Serial.println("Setting left acceleration... ");
00255 #endif
00256
00257 Serial1.print("AC");
00258 Serial1.println(Acc);
00259 }
00260
00261 void Motors::SetRightAcc(int Acc)
00262 {
00263 #ifdef __DEBUG__MOTORS__C__
00264 Serial.println("Setting right acceleration... ");
00265 #endif
00266
00267 Serial2.print("AC");
00268 Serial2.println(Acc);
00269 }
00270
00271
00272 void Motors::SetLeftDec(int Dec)
00273 {
00274 #ifdef __DEBUG__MOTORS__C__
00275 Serial.println("Setting left deceleration... ");
00276 #endif
00277
00278 Serial1.print("DEC");
00279 Serial1.println(Dec);
00280 }
00281
00282 void Motors::SetRightDec(int Dec)
00283 {
00284 #ifdef __DEBUG__MOTORS__C__
00285 Serial.println("Setting right deceleration... ");
00286 #endif
00287
00288 Serial2.print("DEC");
00289 Serial2.println(Dec);
00290 }
00291
00292
00293 void Motors::SetLeftPeakCurrentLimit(int Current)
00294 {
00295 #ifdef __DEBUG__MOTORS__C__
00296 Serial.println("Setting left peak current limit... ");
00297 #endif
00298
00299 Serial1.print("LPC");
00300 Serial1.println(Current);
00301 }
00302
00303 void Motors::SetRightPeakCurrentLimit(int Current)
00304 {
00305 #ifdef __DEBUG__MOTORS__C__
00306 Serial.println("Setting right peak current limit... ");
00307 #endif
00308
00309 Serial2.print("LPC");
00310 Serial2.println(Current);
00311 }
00312
00313
00314 void Motors::SetLeftContCurrentLimit(int Current)
00315 {
00316 #ifdef __DEBUG__MOTORS__C__
00317 Serial.println("Setting left continuous current limit... ");
00318 #endif
00319
00320 Serial1.print("LCC");
00321 Serial1.println(Current);
00322 }
00323
00324 void Motors::SetRightContCurrentLimit(int Current)
00325 {
00326 #ifdef __DEBUG__MOTORS__C__
00327 Serial.println("Setting right continuous current limit... ");
00328 #endif
00329
00330 Serial2.print("LCC");
00331 Serial2.println(Current);
00332 }
00333
00334
00336
00337
00338
00339 int Motors::GetLeftSpeed()
00340 {
00341 #ifdef __DEBUG__MOTORS__C__
00342 Serial.println("Getting left speed... ");
00343 #endif
00344
00345 char Buffer[100];
00346 int i;
00347 Serial1.flush();
00348 Serial1.println("GV");
00349
00350 while (1)
00351 {
00352 if (Serial1.available()>0)
00353 {
00354 Buffer[i++] = Serial1.read();
00355 if (Buffer[i-1]==10)
00356 {
00357 long int Val;
00358 sscanf (Buffer,"%ld",&Val);
00359 return Val;
00360 }
00361 }
00362 }
00363 }
00364
00365 int Motors::GetRightSpeed()
00366 {
00367 #ifdef __DEBUG__MOTORS__C__
00368 Serial.println("Getting right speed... ");
00369 #endif
00370
00371 char Buffer[100];
00372 int i;
00373 Serial2.flush();
00374 Serial2.println("GV");
00375
00376 while (1)
00377 {
00378 if (Serial2.available()>0)
00379 {
00380 Buffer[i++] = Serial2.read();
00381 if (Buffer[i-1]==10)
00382 {
00383 long int Val;
00384 sscanf (Buffer,"%ld",&Val);
00385 return Val;
00386 }
00387 }
00388 }
00389 }
00390
00391
00392 int Motors::GetLeftPos()
00393 {
00394 #ifdef __DEBUG__MOTORS__C__
00395 Serial.println("Getting left position... ");
00396 #endif
00397
00398 char Buffer[100];
00399 int i;
00400 Serial1.flush();
00401 Serial1.println("POS");
00402 while (1)
00403 {
00404 if (Serial1.available()>0)
00405 {
00406 Buffer[i++] = Serial1.read();
00407 if (Buffer[i-1]==0x0A)
00408 {
00409 long int Val;
00410 sscanf (Buffer,"%ld",&Val);
00411 return (Val);
00412 }
00413 }
00414 }
00415 }
00416
00417 int Motors::GetRightPos()
00418 {
00419 #ifdef __DEBUG__MOTORS__C__
00420 Serial.println("Getting right position... ");
00421 #endif
00422
00423 char Buffer[100];
00424 int i;
00425 Serial2.flush();
00426 Serial2.println("POS");
00427
00428 while (1)
00429 {
00430 if (Serial2.available()>0)
00431 {
00432 Buffer[i++] = Serial2.read();
00433 if (Buffer[i-1]==10)
00434 {
00435 long int Val;
00436 sscanf (Buffer,"%ld",&Val);
00437 return (Val);
00438 }
00439 }
00440 }
00441 }
00442
00443
00444 int Motors::GetLeftAcc()
00445 {
00446 #ifdef __DEBUG__MOTORS__C__
00447 Serial.println("Getting left acceleration... ");
00448 #endif
00449
00450 char Buffer[100];
00451 int i;
00452 Serial1.flush();
00453 Serial1.println("GAC");
00454 while (1)
00455 {
00456 if (Serial1.available()>0)
00457 {
00458 Buffer[i++] = Serial1.read();
00459 if (Buffer[i-1]==10)
00460 {
00461 long int Val;
00462 sscanf (Buffer,"%ld",&Val);
00463 return (Val);
00464 }
00465 }
00466 }
00467 }
00468
00469 int Motors::GetRightAcc()
00470 {
00471 #ifdef __DEBUG__MOTORS__C__
00472 Serial.println("Getting right acceleration... ");
00473 #endif
00474
00475 char Buffer[100];
00476 int i;
00477 Serial2.flush();
00478 Serial2.println("GAC");
00479
00480 while (1)
00481 {
00482 if (Serial2.available()>0)
00483 {
00484 Buffer[i++] = Serial2.read();
00485 if (Buffer[i-1]==10)
00486 {
00487 long int Val;
00488 sscanf (Buffer,"%ld",&Val);
00489 return (Val);
00490 }
00491 }
00492 }
00493 }
00494
00495
00496 int Motors::GetLeftDec()
00497 {
00498 #ifdef __DEBUG__MOTORS__C__
00499 Serial.println("Getting left deceleration... ");
00500 #endif
00501
00502 char Buffer[100];
00503 int i;
00504 Serial1.flush();
00505 Serial1.println("GDEC");
00506 while (1)
00507 {
00508 if (Serial1.available()>0)
00509 {
00510 Buffer[i++] = Serial1.read();
00511 if (Buffer[i-1]==10)
00512 {
00513 long int Val;
00514 sscanf (Buffer,"%ld",&Val);
00515 return (Val);
00516 }
00517 }
00518 }
00519 }
00520
00521 int Motors::GetRightDec()
00522 {
00523 #ifdef __DEBUG__MOTORS__C__
00524 Serial.println("Getting right deceleration... ");
00525 #endif
00526
00527 char Buffer[100];
00528 int i;
00529 Serial2.flush();
00530 Serial2.println("GDEC");
00531
00532 while (1)
00533 {
00534 if (Serial2.available()>0)
00535 {
00536 Buffer[i++] = Serial2.read();
00537 if (Buffer[i-1]==10)
00538 {
00539 long int Val;
00540 sscanf (Buffer,"%ld",&Val);
00541 return (Val);
00542 }
00543 }
00544 }
00545 }
00546
00547
00548 int Motors::GetLeftPeakCurrentLimit()
00549 {
00550 #ifdef __DEBUG__MOTORS__C__
00551 Serial.println("Getting left peak current limit... ");
00552 #endif
00553
00554 char Buffer[100];
00555 int i;
00556 Serial1.flush();
00557 Serial1.println("GPC");
00558 while (1)
00559 {
00560 if (Serial1.available()>0)
00561 {
00562 Buffer[i++] = Serial1.read();
00563 if (Buffer[i-1]==10)
00564 {
00565 long int Val;
00566 sscanf (Buffer,"%ld",&Val);
00567 return (Val);
00568 }
00569 }
00570 }
00571 }
00572
00573 int Motors::GetRightPeakCurrentLimit()
00574 {
00575 #ifdef __DEBUG__MOTORS__C__
00576 Serial.println("Getting right peak current limit... ");
00577 #endif
00578
00579 char Buffer[100];
00580 int i;
00581 Serial2.flush();
00582 Serial2.println("GPC");
00583
00584 while (1)
00585 {
00586 if (Serial2.available()>0)
00587 {
00588 Buffer[i++] = Serial2.read();
00589 if (Buffer[i-1]==10)
00590 {
00591 long int Val;
00592 sscanf (Buffer,"%ld",&Val);
00593 return (Val);
00594 }
00595 }
00596 }
00597 }
00598
00599
00600 int Motors::GetLeftContCurrentLimit()
00601 {
00602 #ifdef __DEBUG__MOTORS__C__
00603 Serial.println("Getting left continuous current limit... ");
00604 #endif
00605
00606 char Buffer[100];
00607 int i;
00608 Serial1.flush();
00609 Serial1.println("GCC");
00610 while (1)
00611 {
00612 if (Serial1.available()>0)
00613 {
00614 Buffer[i++] = Serial1.read();
00615 if (Buffer[i-1]==10)
00616 {
00617 long int Val;
00618 sscanf (Buffer,"%ld",&Val);
00619 return (Val);
00620 }
00621 }
00622 }
00623 }
00624
00625 int Motors::GetRightContCurrentLimit()
00626 {
00627 #ifdef __DEBUG__MOTORS__C__
00628 Serial.println("Getting right continuous current limit... ");
00629 #endif
00630
00631 char Buffer[100];
00632 int i;
00633 Serial2.flush();
00634 Serial2.println("GCC");
00635
00636 while (1)
00637 {
00638 if (Serial2.available()>0)
00639 {
00640 Buffer[i++] = Serial2.read();
00641 if (Buffer[i-1]==10)
00642 {
00643 long int Val;
00644 sscanf (Buffer,"%ld",&Val);
00645 return (Val);
00646 }
00647 }
00648 }
00649 }