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