D:/DRISSI/arduino-0022/arduino-0022/libraries/motors/motors.cpp
Go to the documentation of this file.
00001 
00005 /* Methods for motors control and get information */
00006 
00007 #include "motors.h"
00008 
00009 bool TimeOutFlagMotors=false;
00010 
00012 
00013 
00014 // Methods to enable left or right
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 // Methods to disable left or right
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 // Methods to reset left or right
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 // Methods to wait for an answer from left or right
00063 char ReadBufferLeft (char *Buffer,int NbMax,long TimeOut)
00064 {
00065         int i=0;
00066         Timer1.initialize(TimeOut);
00067     Timer1.attachInterrupt(ISR_TimeOut_Motors);                 // Attach the interrupt method to the TimeOut
00068     TimeOutFlagMotors=false;
00069     Timer1.start();                                                                                     // Start of Timer1
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);                 // Attach the interrupt method to the TimeOut
00098     TimeOutFlagMotors=false;
00099     Timer1.start();                                                                                     // Start of Timer1
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 // Methods to check if an "OK" packet has been received
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 // Methods to check if a "reset" packet has been received
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 // Time out of serial link
00174 void ISR_TimeOut_Motors()
00175 {
00176     TimeOutFlagMotors=true;                                                                             // Set the TimeOutFlagMotors to true means that receive has failed
00177     Timer1.stop();                                                                                      // Stop Timer1
00178 
00179 #ifdef __DEBUG__MOTORS__
00180     Serial.println("TimeOut...");
00181 #endif
00182 }
00183 
00184 
00186 
00187 
00188 // Methods to set speed left or right
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 // Methods to set absolute position left or right
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 // Methods to set relative position left or right
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 // Methods to set acceleration left or right
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 // Methods to set deceleration left or right
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 // Methods to set peak current limit left or right
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 // Methods to set continuous current limit left or right
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 // Methods to notify when position is reached
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 // Methods to end notify position
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 // Methods to notify when velocity is reached
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 // Methods to end notify velocity
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 // Methods to move to last position load
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 // Methods to get speed left or right
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 // Methods to get current position left or right
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 // Methods to get current acceleration left or right
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 // Methods to get current deceleration left or right
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 // Methods to get current peak current limit left or right
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 // Methods to get current continuous current limit left or right
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 // Methods to get when position is reached
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 // Methods to get when velocity is reached
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 }