main.cpp

00001 #include "mbed.h"
00002 #include "TextLCD.h"
00003 #include "QEI.h"
00004 
00005 #define ppr 1120
00006 #define diam 11.0
00007 #define circum (3.14*diam)
00008 #define width 23.0
00009 #define motRPM 150
00010 
00011 TextLCD lcd(PA_0,PA_1,PA_4,PB_0,PC_1,PC_0); // rs, e, d4-d7
00012 QEI leftEnc(PB_8,PB_9);
00013 QEI rightEnc(PA_6,PA_5);
00014 
00015 Serial serial(USBTX, USBRX);
00016 //Serial hmcRead(PC_10,PC_11);//tx rx
00017 Serial bt(PA_9,PA_10);//tx,rx
00018 Serial xbee1(PC_10,PC_11);
00019 
00020 DigitalOut dirLa(PB_4);
00021 DigitalOut dirLb(PB_10);
00022 PwmOut pwmR(PA_8);//left
00023 
00024 DigitalOut dirRa(PB_5);
00025 DigitalOut dirRb(PB_3);
00026 PwmOut pwmL(PC_7);//right
00027 Ticker TISR;
00028 Ticker Print;
00029 
00030 DigitalOut rolA(PA_13);
00031 DigitalOut rolB(PA_14);
00032 PwmOut RolPwm(PA_15);//roller
00033 
00034 void rolStop();
00035 void InitSerial();
00036 void runMotor(char mot, bool dir, float pwm);
00037 float updatePIDL();
00038 float updatePIDR();
00039 void keepEnCount(void);
00040 void printData(void);
00041 void execPIDL();
00042 void execPIDR();
00043 void hmcCorrect();
00044 float updatePID();double hoq;
00045 void allfunction();
00046 void runRol();
00047 void Brake();
00048 float err;
00049 int ch;
00050 char run_mode;
00051 bool zom = true;int jim=0,jim1=0;
00052 long Count1,Count2;
00053 long CountL,CountR,CountDiffL,CountDiffR,CountPrevL,CountPrevR;
00054 float correctPwmL,correctPwmR;
00055 double distL,distR,distAv,velL,velR;
00056 float xpwmL=0.8*1.1,xpwmR=0.8;
00057 float xpwmLc,xpwmRc,xpwmLR,outputPID;
00058 double KpL=0.1,KpR=0.125,KdL=0,KdR=0,Kp=0.3,Kd = 0;
00059 //int Mx,Mx1,err,lastError;
00060 //float outputPID,P,D;
00061 void InitSerial()
00062 {
00063     bt.baud(9600);
00064     serial.baud(9600);
00065 }
00066 void SetPwmf_kHz(float freq)
00067 {
00068 
00069     pwmL.period_ms(1/freq);
00070     pwmR.period_ms(1/freq);
00071     RolPwm.period_ms(1/freq);
00072 }
00073 /*void hmcCorrect()
00074 {
00075   err = Mx1 - Mx;
00076   P = err*KpL;
00077   D = (err - lastError)*KdL;
00078   outputPID = (P + D);                                   
00079   lastError = err;
00080   if(outputPID < 0)
00081   {runMotor('L', 1, (xpwmL + outputPID));
00082    runMotor('R', 1, (xpwmR - outputPID));}
00083   else if(outputPID > 0)   
00084    {runMotor('L', 1, (xpwmL + outputPID));
00085     runMotor('R', 1, (xpwmR - outputPID));}
00086   else 
00087    {runMotor('L', 1, (xpwmL));
00088     runMotor('R', 1, (xpwmR));}
00089     }*/    
00090 void printData()
00091 {
00092  //   lcd.cls();
00093    // lcd.locate(1,0);
00094     //lcd.printf("%f",outputPID );
00095     //lcd.locate(1,1);
00096     //lcd.printf("%f",velR);
00097 }
00098 int main()
00099 {
00100     InitSerial();
00101     SetPwmf_kHz(1);
00102     TISR.attach(&keepEnCount, 0.01);
00103     Print.attach(&printData, 0.05);
00104     lcd.cls();
00105     Brake();
00106     
00107     while(1) 
00108     {
00109         
00110       //rolA=1;rolB=0;  
00111 /*        CountL = leftEnc.read();
00112         CountR = rightEnc.read();
00113         distL = (CountL/ppr)*circum;
00114         distR = (CountR/ppr)*circum;
00115         distAv = (0.5*(distL + distR));
00116         xpwmLc = float(CountDiffL*60)/(ppr*motRPM);
00117         xpwmRc = float(CountDiffR*60)/(ppr*motRPM);
00118         xpwmLR = float((CountDiffL-CountDiffR)*60)/(ppr*motRPM);
00119         velL   = (circum*motRPM*xpwmLc)/60;
00120         velR   = (circum*motRPM*xpwmRc)/60;
00121         correctPwmL = updatePIDL();
00122         correctPwmR = updatePIDR();
00123         outputPID = updatePID();      
00124         if(outputPID < 0)
00125   {runMotor('L', 1, (xpwmL - outputPID));
00126    runMotor('R', 1, (xpwmR + outputPID));}
00127   else if(outputPID > 0)   
00128    {runMotor('L', 1, (xpwmL - outputPID));
00129     runMotor('R', 1, (xpwmR + outputPID));}
00130   else 
00131    {runMotor('L', 1, (xpwmL));
00132     runMotor('R', 1, (xpwmR));}
00133    
00134         //execPIDL();
00135         //execPIDR();
00136           
00137         
00138         */
00139         CountL = leftEnc.read();
00140         CountR = rightEnc.read();
00141         distL = (CountL/ppr)*circum;
00142         distR = (CountR/ppr)*circum;
00143         distAv = (0.5*(distL + distR));
00144         xpwmLc = float(CountDiffL*60)/(ppr*motRPM);
00145         xpwmRc = float(CountDiffR*60)/(ppr*motRPM);
00146         xpwmLR = float((CountDiffL-CountDiffR)*60)/(ppr*motRPM);
00147         velL   = (circum*motRPM*xpwmLc)/60;
00148         velR   = (circum*motRPM*xpwmRc)/60;
00149         correctPwmL = updatePIDL();
00150         correctPwmR = updatePIDR();
00151         //RolPwm = 0.5;
00152         
00153         if(xbee1.readable())
00154         {
00155             run_mode = xbee1.getc();
00156             //serial.printf("%c\n",run_mode);
00157             switch(run_mode)
00158             {
00159                 case 'd':
00160                 runMotor('L', 1, 0.7);
00161                 runMotor('R', 1, 0.65);
00162                 serial.printf("M Forward\n");    
00163                 break;//front
00164                 
00165                 case 'a':
00166                 runMotor('L', 1, 1);
00167                 runMotor('R', 0, 1);
00168                 serial.printf("M Right\n");
00169                 break;//right
00170                 
00171                 case 's':
00172                 runMotor('L', 0, 1);
00173                 runMotor('R', 0, 1);
00174                 serial.printf("M Back\n");
00175                 break;//back
00176                 
00177                 case 'w':
00178                 runMotor('L', 0, 1);
00179                 runMotor('R', 1, 1);
00180                 serial.printf("M Left\n");
00181                 break;//left
00182                 
00183                 case 'x':
00184                 runMotor('L', 0, 0);
00185                 runMotor('R', 1, 0); 
00186                 serial.printf("M Stop\n");   
00187                 break;//stop
00188                 }
00189          }       
00190         else if(bt.readable())
00191          {
00192             ch = bt.getc();
00193             serial.printf("%c\n",ch);
00194             
00195             switch(ch) 
00196             {
00197 
00198                 case 'w'://Front
00199                     runMotor('L', 1, 0.7);
00200                     runMotor('R', 1, 0.65);
00201                     //serial.printf("A Forward\n");
00202                     lcd.cls();
00203                     lcd.locate(1,0);
00204                     lcd.printf("Forward\n");
00205                     break;
00206 
00207                 case 'd'://Right
00208                     runMotor('L', 1, 1);
00209                     runMotor('R', 0, 1);
00210                     //serial.printf("A Right\n");
00211                     lcd.cls();
00212                     lcd.locate(1,0);
00213                     lcd.printf("Right\n");
00214                     break;
00215                     
00216                 case 's'://Back
00217                     runMotor('L', 0, 1);
00218                     runMotor('R', 0, 1);
00219                     //serial.printf("A Back\n");
00220                     lcd.cls();
00221                     lcd.locate(1,0);
00222                     lcd.printf("Back\n");
00223                     break;
00224 
00225                 case 'a'://Left
00226                     runMotor('L', 0, 1);
00227                     runMotor('R', 1, 1);
00228                     //serial.printf("A Left\n");
00229                     lcd.cls();
00230                     lcd.locate(1,0);
00231                     lcd.printf("Left\n");
00232                     break;
00233 
00234                 case 'A'://Anticlock
00235                     runMotor('L', 0, 0.45);
00236                     runMotor('R', 1, 0.41);
00237                     //serial.printf("A AntiClock\n");
00238                     lcd.cls();
00239                     lcd.locate(1,0);
00240                     lcd.printf("AntiClock\n");
00241                     break;
00242 
00243                 case 'C'://Clock
00244                     runMotor('L', 1, 0.45);
00245                     runMotor('R', 0, 0.41);
00246                     //serial.printf("A Clock\n");
00247                     lcd.cls();
00248                     lcd.locate(1,0);
00249                     lcd.printf("Clock\n");
00250                     break;
00251                     
00252                 case 'O'://Motor Stop
00253                     Brake();
00254                     //rolA = 0;
00255                     //rolB = 0;
00256                     //RolPwm = 0;
00257                     if(jim1==0)
00258                     {
00259                       rolStop();
00260                       jim1++;
00261                     }
00262                     jim1=0;
00263                     //serial.printf("A Motor STOP\n");
00264                     lcd.cls();
00265                     lcd.locate(1,0);
00266                     lcd.printf("STOP\n");
00267                     break;
00268 
00269                 case 'b':
00270                     if(jim==0)
00271                     {
00272                      runRol();
00273                      jim++;
00274                     }
00275                    break;
00276                    
00277                 default:
00278                     Brake();
00279                     //serial.printf("Brake");
00280                     lcd.cls();
00281                     lcd.locate(1,0);
00282                     lcd.printf("Brake");
00283                     }
00284                 jim=0;jim1=0;
00285             }
00286     }
00287 }
00288 
00289 void runRol()
00290 {
00291 rolA = 1;
00292 rolB = 0;
00293 for (hoq = 0.0;hoq < 0.3; hoq = hoq + 0.01)
00294 {
00295  RolPwm = hoq;
00296  wait(0.05);    
00297 }
00298 }
00299 
00300 void rolStop()
00301 {
00302 rolA = 1;
00303 rolB = 0;
00304 for (hoq < 0.3;hoq > 0; hoq = hoq - 0.005)
00305 {
00306  RolPwm = hoq;
00307  wait(0.05);    
00308 }
00309     
00310 }
00311 void allfunction()
00312 {
00313         outputPID = updatePID();      
00314         if(outputPID < 0)
00315   {runMotor('L', 1, (xpwmL - outputPID));
00316    runMotor('R', 1, (xpwmR + outputPID));}
00317   else if(outputPID > 0)   
00318    {runMotor('L', 1, (xpwmL - outputPID));
00319     runMotor('R', 1, (xpwmR + outputPID));}
00320   else 
00321    {runMotor('L', 1, (xpwmL));
00322     runMotor('R', 1, (xpwmR));}
00323    
00324         //execPIDL();
00325         //execPIDR();
00326           
00327         }
00328 float updatePID()
00329 {
00330   float lastError,pidTerm,P,D;
00331   err = CountDiffL - CountDiffR;
00332   P = err*Kp;
00333   D = (err - lastError)*Kd;
00334   pidTerm = (P + D);                                   
00335   lastError = err;
00336   return pidTerm;  
00337 }
00338 float updatePIDL()
00339 {
00340   float error,lastError,pidTerm,P,D;
00341   error = xpwmL - xpwmLc;
00342   P = error*KpL;
00343   D = (error - lastError)*KdL;
00344   pidTerm = (P + D);                                   
00345   lastError = error;
00346   return pidTerm;  
00347 }
00348 float updatePIDR()
00349 {
00350   float error,lastError,pidTerm,P,D;
00351   error = xpwmR - xpwmRc;
00352   P = error*KpR;
00353   D = (error - lastError)*KdR;
00354   pidTerm = (P + D);                                   
00355   lastError = error;
00356   return pidTerm;  
00357 }
00358 void keepEnCount()
00359 {
00360  Count1 = CountL;
00361  CountDiffL = Count1 - CountPrevL;
00362  CountPrevL = Count1 ;
00363  
00364  Count2 = CountR;
00365  CountDiffR = Count2 - CountPrevR;
00366  CountPrevR = Count2 ;    
00367 }
00368 void execPIDL()
00369 {
00370     if(correctPwmL < 0)
00371         {runMotor('L', 1, (xpwmL + correctPwmL));}
00372     else if(correctPwmL > 0)   
00373         {runMotor('L', 1, (xpwmL + correctPwmL));}
00374     else 
00375         {runMotor('L', 1, (xpwmL));}
00376 }
00377 void execPIDR()
00378 {                 
00379     if(correctPwmR < 0)
00380         {runMotor('R', 1, (xpwmR + correctPwmR));}
00381     else if(correctPwmR > 0)   
00382         {runMotor('R', 1, (xpwmR + correctPwmR));}
00383     else 
00384         {runMotor('R', 1, (xpwmR));}     
00385 }        
00386 void runMotor(char mot, bool dir, float pwm)
00387 {
00388     switch(mot) {
00389         case 'L':
00390             if(dir) {
00391                 dirLa = 1;
00392                 dirLb = 0;
00393             } else {
00394                 dirLa = 0;
00395                 dirLb = 1;
00396             }
00397             pwmL= pwm;
00398             //runSmoothL(pwm);
00399             break;
00400 
00401         case 'R':
00402             if(dir) {
00403                 dirRa = 1;
00404                 dirRb = 0;
00405             } else {
00406                 dirRa = 0;
00407                 dirRb = 1;
00408             }
00409             pwmR= pwm;
00410             //runSmoothR(pwm);
00411             break;
00412 
00413         default:
00414             Brake();
00415     }
00416 }
00417 void linDist(unsigned int DistanceInCM)
00418 {
00419     float ReqdShaftCount = 0;
00420     unsigned long int ReqdShaftCountInt = 0;
00421 
00422     ReqdShaftCount = DistanceInCM*31.0;
00423     ReqdShaftCountInt = (unsigned long int) ReqdShaftCount;
00424     CountL = 0;
00425     CountR = 0;
00426 
00427     while(1) {
00428         wait(0.01);
00429         if((CountL + CountR)*0.5 > ReqdShaftCountInt) {
00430             break;
00431         }
00432     }
00433     Brake();
00434 }
00435 void botRot(int Degrees)
00436 {
00437     float ReqdShaftCount = 0;
00438     unsigned long int ReqdShaftCountInt = 0;
00439 
00440     ReqdShaftCount = Degrees* 5.6;
00441     ReqdShaftCountInt = (unsigned int) ReqdShaftCount;
00442     CountL = 0;
00443     CountR = 0;
00444     while (true) {
00445         wait(0.01);
00446         if((CountL >= ReqdShaftCountInt) | (CountR >= ReqdShaftCountInt)) {
00447             break;
00448         }
00449     }
00450     Brake();
00451 }
00452 void Brake()
00453 {
00454     dirLa = 0;
00455     dirLb = 0;
00456     pwmL=0.0;
00457 
00458     dirRa = 0;
00459     dirRb = 0;
00460     pwmR=0.0;
00461 }