D:/DRISSI/arduino-0022/arduino-0022/libraries/motorsControl/motorsControl.cpp
Go to the documentation of this file.
00001 
00005 /* Methods for motors control and get information */
00006 
00007 #include "motorsControl.h"
00008 
00009 
00011 
00012 
00013 Motors::Motors()
00014 {
00015 #ifdef __DEBUG__MOTORS__C__
00016 //      Serial.println("*** ROBOT DIWHEEL V2 ***");
00017 #endif
00018 
00019         Serial1.begin(115200);
00020         Serial2.begin(115200);
00021   
00022 //      EnableLeft();
00023 //      EnableRight();
00024         
00025 //      ResetLeft();
00026 //      ResetRight();
00027 
00028 //      EnableLeft();
00029 //      EnableRight();
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 // Methods to enable left or right
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 // Methods to disable left or right
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 // Methods to reset left or right
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         if(strcmp (Buffer,"FAULHABER MCDC3003S\n")==0)
00106                 return true;
00107         return false;   
00108         */
00109 }
00110 
00111 
00112 // Methods to wait for an answer from left or right
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 //      Serial.print(Buffer);
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 //      Serial.print("STRCMP : ");
00160 //      Serial.println (strcmp (Buffer,"OK\n"));
00161         if(strcmp (Buffer,"OK\n")==0)
00162                 return true;
00163         return false;
00164 }
00165 
00166 
00168 
00169 
00170 // Methods to set speed left or right
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 //      Serial1.print("NV");
00180 //      Serial1.println(25000); 
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 //      Serial2.print("NV");    
00192 //      Serial2.println(25000);
00193 }
00194 
00195 
00196 // Methods to set absolute position left or right
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 // Methods to set relative position left or right
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 //      Serial1.println("NP");
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 //      Serial2.println("NP");
00246         Serial2.println("M"); 
00247 }
00248 
00249 
00250 // Methods to set acceleration left or right
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 // Methods to set deceleration left or right
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 // Methods to set peak current limit left or right
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 // Methods to set continuous current limit left or right
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 // Methods to get speed left or right
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 // Methods to get current position left or right
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 // Methods to get current acceleration left or right
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 // Methods to get current deceleration left or right
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 // Methods to get current peak current limit left or right
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 // Methods to get current continuous current limit left or right
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 }