AutoRoboMotion.cpp

00001 #include "mbed.h"
00002 //#include "Map.hpp"
00003 #include "LidarLitev2.h"
00004 
00005 #include "Servo.h"// The desired Servo sweep range is 60 degrees
00006 // But due to some mechanical Load errors it has been calibrated to 0 to 70 degrees
00007 // The front facing position varies from 27 degress to 38 degrees due to the servo motion slip.
00008 
00009 // Variable declaration
00010 Servo myservo(D3);// Object to declare the servo functionalities
00011 float pos = 33;// Default avg front facing posiion of the Servo
00012 Servo LidarServo(D7);
00013 int L_Dist;
00014 float LSrvoPos;
00015 
00016 DigitalIn IR1(A0);
00017 DigitalIn IR2(A1);
00018 
00019 int L2Cnt=0;
00020 int L1Cnt=0;
00021 int LstVal1 =0;
00022 int LstVal2 =0;
00023 
00024 DigitalIn OA(D4);
00025 DigitalIn OA2(D2);
00026 int Tsop1_Val;
00027 int i;
00028 int j;
00029 int i1;
00030 int j1;
00031 float Dist_Val; 
00032 int LsrvMin = 10;
00033 int LsrvMax =90; 
00034 float Val;
00035 
00036 Timer dt;
00037 int Cnt=0;
00038 int Flg2=0; // For Sensor activation
00039 int Flg=0; // Flag to enable motions (Forward, Backward, Stop state)
00040 int Flg2St = 0;// Unused Variable
00041 float Level = 0.30;// Variable to store the PWM Levels activated by the gear Button in the App.
00042 float pwm_Level;// Unused Variable
00043 // Function Initialisation
00044 void handleInput(int in); 
00045 void drive_forward(float level);
00046 void drive_backward(float level);
00047 void Robot_Stop();
00048 float LidarSense(int minpos, int maxpos);
00049 
00050 LidarLitev2 Lidar(PTC11, PTC10);// Tx Rx for I2C Communication in Lidar
00051 Serial pc(USBTX,USBRX);//Tx Rx for Serial Communication (UART)(PC to Processor)
00052 Serial BT(PTC15,PTC14);// Tx Rx for Bluetooth Communication () 
00053 
00054 PwmOut ENApwm(D5); // Motor driver Enable A
00055 PwmOut ENBpwm(D6); // Motor driver Enable B
00056 
00057 // Setting coresponding pins to output mode for Motor movements
00058 DigitalOut LeftMotor_P1(D8);
00059 DigitalOut LeftMotor_P2(D9);
00060 DigitalOut RightMotor_P1(D11);
00061 DigitalOut RightMotor_P2(D10);
00062 
00063 
00064 // The main Function 
00065 int main() {
00066     pc.baud(9600);// UART communication in 9600 bits per seconds
00067     BT.baud(9600);// Bluetooth communication in 9600 bits per seconds
00068     // Printing some execution statements to ensure the controller is communicating properly.
00069     pc.printf(" Hello User!\r\n");
00070     printf(" Are You Ready \r\n");    
00071    // At the start set servo to its default front facing  position 
00072     if(pos>33){
00073         myservo = (27/100.0);
00074     }
00075     else{
00076          myservo = (35/100.0);        
00077     }
00078     Lidar.configure();
00079     dt.start();        
00080    
00081     while(1){ // run an infinite loop to read blutooth data & call corresponding navigation function
00082         if (BT.readable()){ // Read incoming Bluetooth Data
00083          int BT_Val = (int)(pc.putc(BT.getc()));// get the Ascii value of the incomming 1 byte data
00084            //int i = atoi((int)BT_Val);        // Convert it to Interger.
00085            //BT_Val -='0';
00086            pc.printf("%d \r\n",BT_Val); // print the Bluetooth data in serial Monitor for cross validation
00087            // Bluetooth data below decimal 25 contains certain values for the button operations 
00088            // Bluetooth data above 25 triggers the PWM pins of EnA and EnB for motor speed control
00089           
00090            if (BT_Val >= 30 && BT_Val <=40){
00091                float Val = (((float)BT_Val - 30.0)*(100.0 - 0.0)/(40.0 - 30.0) + 0.0 );
00092                Level = ((float)Val/100);               
00093                pc.printf("%f \r\n",Level);
00094                //float pwm_Level = Level;
00095                }
00096            else if (BT_Val < 25){
00097                 handleInput(BT_Val);
00098                }
00099            else if (BT_Val > 41){// accelerometer of phone for motion control
00100                float Newpos = (((float)BT_Val - 70.0)*(70.0 - 0.0)/(210.0 - 70.0) + 0.0 );
00101                myservo = (Newpos/100.0);
00102                /*BT_Val_diff = BT_Val-LstBT_Val;
00103                pos = pos+ (BT_Val_diff*0.5);
00104                myservo = (pos/100.0);*/
00105                }  
00106                
00107              while (Flg2 ==1){
00108                    Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax); 
00109                     if (BT.readable()){ // Read incoming Bluetooth Data
00110                        int BT_Val = (int)(pc.putc(BT.getc()));// get the Ascii value of the incomming 1 byte data           
00111                        pc.printf("%d \r\n",BT_Val); // print the Bluetooth data in serial Monitor for cross validation
00112                        
00113                       if (BT_Val < 25){
00114                         handleInput(BT_Val);                
00115                       }
00116                       
00117                     }
00118                     if (Flg == 1){
00119                          drive_forward((float)Level);  
00120                       }
00121                     else if (Flg == 2){
00122                          if(Dist_Val < 150){
00123                               Val=50.0f;
00124                               Level = ((float)Val/100);
00125                              }                         
00126                          drive_backward((float)Level);
00127                       } 
00128                     else if (Flg == 0){
00129                          Robot_Stop();
00130                       }
00131                    // int L_Dist=Lidar.distance();
00132                    // dt.reset();
00133                    // LidarServo = (45/100.0);
00134                      
00135                     if (Dist_Val< 150.0){
00136                         LsrvMin = 10;
00137                         LsrvMax = 90;
00138                         Val=0.0;
00139                         Level = ((float)Val/100);
00140                         Cnt = Cnt+1;                        
00141                         }
00142                     else if ((Dist_Val> 150.0)&&(Dist_Val<= 300.0)){
00143                         //LsrvMin= 
00144                         LsrvMin = (((float)Dist_Val - 150.0)*(40.0 - 10.0)/(300.0 - 150.0) + 10.0 );
00145                         LsrvMax = (((float)Dist_Val - 150.0)*(60.0 - 90.0)/(300.0 - 150.0) + 90.0 );
00146                         Val = (((float)Dist_Val - 150.0)*(50.0 - 0.0)/(300.0 - 150.0) + 0.0 );
00147                         Level = ((float)Val/100);
00148                         }
00149                     else if (Dist_Val> 300.0){
00150                         LsrvMin = 40;
00151                         LsrvMax = 70;
00152                         Val=100.0;
00153                         Level = ((float)Val/100);
00154                         }
00155                         
00156                     if (Cnt == 10){
00157                         Flg2= 2; 
00158                         Cnt=0;
00159                         break;
00160                         }
00161                         
00162                     int Tsop2_Val = OA2.read();
00163                     int Tsop1_Val = OA.read(); 
00164                     if ((Flg==2)&& (Tsop1_Val==1)){
00165                          drive_backward((float)Level);
00166                      }
00167                     else if ((Flg==2)&& (Tsop1_Val==0)){
00168                          Robot_Stop();
00169                          Flg=0;
00170              
00171                     }
00172                     if ((Flg==1)&& (Tsop2_Val==1)){
00173                          drive_forward((float)Level);
00174                     }
00175                     else if ((Flg==1)&& (Tsop2_Val==0)){
00176                           Robot_Stop();
00177                           Flg=0;
00178              
00179                     }
00180                         
00181                     continue; 
00182 
00183                   // break;                  
00184                }  
00185                
00186           /* if (Flg2 == Flg2St) && ()
00187                if (Flg2 ==1){
00188                    Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax);                   
00189                    }
00190                else if (Flg2 ==0) {
00191                    Robot_Stop();
00192                    }               
00193                }
00194             else{
00195                  if (Flg2 ==1){
00196                    Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax);                   
00197                    }
00198                else if (Flg2 ==0) {
00199                    Robot_Stop();
00200                    }
00201                 }*/
00202            
00203            //pc.printf("Flg2 = %d", Flg2);
00204            while(Flg2 ==2){
00205                 int Tsop1_Val = OA.read();                
00206                 if(Tsop1_Val==1){
00207                      Val=40.0;
00208                      Level = ((float)Val/100);
00209                      drive_backward((float)Level);
00210                      if(Cnt == 5){
00211                           Cnt=0;
00212                           Robot_Stop();
00213                          }
00214                      //wait(2);                     
00215                 }
00216                 else if (Tsop1_Val == 0){  
00217                      Robot_Stop(); 
00218                      Flg=0;               
00219                      break;                     
00220                 }                   
00221                 Cnt=Cnt+1;                       
00222             }
00223               
00224            /*while(Flg2 == 4){
00225                 if (BT.readable()){ // Read incoming Bluetooth Data
00226                        int BT_Val = (int)(pc.putc(BT.getc()));// get the Ascii value of the incomming 1 byte data           
00227                        pc.printf("%d \r\n",BT_Val); // print the Bluetooth data in serial Monitor for cross validation
00228                        
00229                       if (BT_Val < 25){
00230                         handleInput(BT_Val);                
00231                       }
00232                       
00233                 }
00234                 Val=30.0;
00235                 Level = ((float)Val/100);
00236                 LidarServo = (45/100.0);
00237                 L_Dist=Lidar.distance();
00238                 LSrvoPos = LidarServo.read();
00239                 dt.reset();
00240                 int value1 = IR1.read();
00241                 int value2 = IR2.read();
00242                 if ((value1==1)&&(value1>=LstVal1)){
00243                     L1Cnt=L1Cnt+1;
00244                 }
00245                 else if(value1<LstVal1){
00246                     L1Cnt=0;
00247                 }
00248                     
00249                 if ((value2==1)&&(value2>=LstVal2)){
00250                     L2Cnt=L2Cnt+1;
00251                 }
00252                 else if(value2<LstVal2){
00253                     L2Cnt=0;
00254                 }
00255                 //int ServoPos = (int)((myservo.read())*100);
00256                 int pos = ((int)((myservo.read())*100.0f));
00257                 
00258                 if ((L1Cnt>5)&&(L2Cnt<5)){
00259                      pos = pos-3;
00260                      myservo = (pos/100.0);
00261                      //wait(0.01);
00262                      pos = pos+1;
00263                      myservo = (pos/100.0);
00264                      if (pos < 24){
00265                         pos = 24;                
00266                      } 
00267                 }
00268                 else if ((L2Cnt>5)&&(L1Cnt<5)){
00269                      pos = pos+3;
00270                      myservo = (pos/100.0);
00271                      //wait(0.01);
00272                      pos = pos-1;
00273                      myservo = (pos/100.0);
00274                      if (pos > 38){
00275                          pos = 38;                
00276                      } 
00277                 } 
00278                 else if ((L2Cnt>=5)&&(L1Cnt>=5)){
00279                     continue;
00280                     }     
00281                 if (Flg == 1){
00282                     drive_forward((float)Level);  
00283                     }
00284                 else if (Flg == 2){
00285                     drive_backward((float)Level);                    
00286                     } 
00287                 else if (Flg == 0){
00288                     Robot_Stop();
00289                     break;
00290                     }
00291                 int Tsop2_Val = OA2.read();
00292                 int Tsop1_Val = OA.read(); 
00293                 if ((Flg==2)&& (Tsop1_Val==1)){
00294                     drive_backward((float)Level);
00295                 }
00296                 else if ((Flg==2)&& (Tsop1_Val==0)){
00297                     Robot_Stop();
00298                     Flg=0;
00299                     break;
00300              
00301                 }
00302                 /*if ((Flg==1)&& (Tsop2_Val==1)){
00303                     drive_forward((float)Level);
00304                 }
00305                 else if ((Flg==1)&& (Tsop2_Val==0)){
00306                     Robot_Stop();
00307                     Flg=0; 
00308                     break;            
00309                 }  
00310                 if (L_Dist<50){
00311                     Robot_Stop();
00312                     Flg=0;
00313                     break;   
00314                     }  
00315                 
00316                 LstVal1=value1;
00317                 LstVal2=value2;
00318                 continue;
00319                 
00320             }*/
00321                
00322            if (Flg == 1){
00323                     drive_forward((float)Level);  
00324                     }
00325            else if (Flg == 2){
00326                     drive_backward((float)Level);                    
00327                     } 
00328            else if (Flg == 0){
00329                     Robot_Stop();
00330                     }   
00331                
00332       // while (Flg == 2) 
00333                                     
00334                                     
00335         //if BT_Val == 'B'               
00336         }
00337         //LstBT_Val=BT_Val;
00338         //Flg2St = Flg2;
00339         int Tsop2_Val = OA2.read();
00340         int Tsop1_Val = OA.read(); 
00341         if ((Flg==2)&& (Tsop1_Val==1)){
00342              drive_backward(0.3f);
00343             }
00344         else if ((Flg==2)&& (Tsop1_Val==0)){
00345              Robot_Stop();
00346              Flg=0;
00347              
00348             }
00349         if ((Flg==1)&& (Tsop2_Val==1)){
00350              drive_forward((float)Level);
00351             }
00352         else if ((Flg==1)&& (Tsop2_Val==0)){
00353              Robot_Stop();
00354              Flg=0;             
00355             }
00356         
00357     }
00358  } 
00359     
00360 // Function which calls specific execution for corresponding incomming Serial inputs from Bluetooth
00361 
00362 void handleInput(int in) {
00363     switch(in) { 
00364     //Motor Rotation Controls       
00365         case 6 :// Forward Motion
00366             Flg = 1;
00367             break;        
00368         case 7 :
00369             Flg = 2;// Backward Motion
00370             break;
00371         case 0 :  // Robot Stop      
00372             
00373                 Flg = 0;
00374                 Flg2 = 0;
00375             break;
00376         case 13 :  // Robot Stop      
00377             Flg2 = 1;
00378             break;
00379         case 14 :  // Robot Stop 
00380             int pos = (int)((myservo.read())*100); 
00381             if(pos>33){
00382                myservo = (27/100.0);
00383             }
00384             else{
00385                myservo = (35/100.0);        
00386             }     
00387             Flg2 = 4;
00388             break;
00389     //Servo Rotation Controls
00390         case 1:// Turn 30 degree Left and back to front facing
00391             if (pos >0){
00392                 myservo = (pos/100.0);
00393                 for( j1=pos; j1>=0; j1--) {
00394                     myservo = (j1/100.0);
00395                     //printf("%f\r\n",myservo.read());
00396                     wait(0.01);        
00397                 }
00398                 pos=j1;
00399                 for(i1=pos; i1<=35; i1++) {
00400                     myservo = (i1/100.0);
00401                     //printf("%f\r\n",myservo.read());
00402                     wait(0.01);
00403                 }
00404                 pos=i1;         
00405             }
00406             else{
00407                  myservo = (0/100.0); 
00408                  pos = 0;
00409                  for(i1=pos; i1<=35; i1++) {
00410                     myservo = (i1/100.0);
00411                     //printf("%f\r\n",myservo.read());
00412                     wait(0.01);
00413                 }
00414                 pos=i1;                
00415             }                
00416             break;
00417             
00418         case 2: //Turn 30 degree Left 
00419             if (pos >0){
00420                 //myservo = (pos/100.0);
00421                 for( j1=pos; j1>=0; j1--) {
00422                     myservo = (j1/100.0);
00423                    // printf("%f\r\n",myservo.read());
00424                     wait(0.01);        
00425                 }
00426                 pos=j1;         
00427             }
00428             else{
00429                  myservo = (0/100.0);                
00430             }      
00431             break;
00432             
00433         case 3: // Sweep Servo to forward position irrespective of its current position
00434             if(pos >40){
00435                 //myservo = (pos/100.0);
00436                 for( j=pos; j>=27; j--) {
00437                     myservo = (j/100.0);
00438                     //printf("%f\r\n",myservo.read());
00439                     wait(0.01);        
00440                 }
00441                 pos=j;
00442                 
00443             }
00444             else if (pos < 25){
00445                 //myservo = (pos/100.0);
00446                 for(i1=pos; i1<=35; i1++) {
00447                     myservo = (i1/100.0);
00448                     //printf("%f\r\n",myservo.read());
00449                     wait(0.01);
00450                 }
00451                 pos=i1;   
00452                 
00453             }
00454             else{
00455                 myservo = (pos/100.0);                
00456             }
00457             break;
00458             
00459         case 4://Turn 30 degree Right
00460             if (pos <61){
00461                // myservo = (pos/100.0);
00462                 for(i=pos; i<=60; i++) {
00463                     myservo = (i/100.0);
00464                     //printf("%f\r\n",myservo.read());
00465                     wait(0.01);
00466                 }
00467                 pos=i;                
00468             }
00469             else{
00470                 myservo = (60/100.0);                 
00471             }
00472             break;
00473             
00474         case 5:// Turn 30 degree Right and back to front facing
00475             if (pos <61){
00476                 //myservo = (pos/100.0);
00477                 for(i=pos; i<=60; i++) {
00478                     myservo = (i/100.0);
00479                     //printf("%f\r\n",myservo.read());
00480                     wait(0.01);
00481                 }
00482                 pos=i; 
00483                 for( j=pos; j>=27; j--) {
00484                     myservo = (j/100.0);
00485                     //printf("%f\r\n",myservo.read());
00486                     wait(0.01);        
00487                 }
00488                 pos=j;               
00489             }
00490             else{
00491                 myservo = (60/100.0);
00492                 pos = 60;
00493                 for( j=pos; j>=27; j--) {
00494                     myservo = (j/100.0);
00495                     //printf("%f\r\n",myservo.read());
00496                     wait(0.01);        
00497                 }
00498                 pos=j;                 
00499             }
00500             //break;
00501             break;
00502             
00503         case 8:// Turn Servo Left by 5 degrees irrespective of its current position
00504             pos = pos-5;
00505             myservo = (pos/100.0);
00506             if (pos < 0){
00507                 pos = 0;                
00508             }   
00509             break;
00510             
00511         case 9: // Turn Servo right by 5 degrees irrespective of its current position
00512             pos = pos+5;
00513             myservo = (pos/100.0); 
00514             if (pos > 60){
00515                 pos = 60;                
00516             }            
00517             break; 
00518         
00519         default:
00520             pc.printf("I don't know: \"%d\"\n", in);
00521             break;
00522     }
00523 }
00524 
00525 // Function to move rear wheel DC motors forward
00526 
00527 void drive_forward(float level){
00528      ENApwm.write(level);
00529      ENBpwm.write(level); 
00530      LeftMotor_P1=1; 
00531      LeftMotor_P2=0;
00532      RightMotor_P1=1;
00533      RightMotor_P2=0; 
00534 } 
00535 
00536 // Function to move rear wheel DC motors backward 
00537 void drive_backward(float level){
00538      ENApwm.write(level);
00539      ENBpwm.write(level); 
00540      LeftMotor_P1=0; 
00541      LeftMotor_P2=1;
00542      RightMotor_P1=0;
00543      RightMotor_P2=1; 
00544  } 
00545  
00546 
00547 // Function to stop rear wheel motion
00548 void Robot_Stop() {
00549      //ENApwm.write(level);
00550      //ENBpwm.write(level); 
00551      LeftMotor_P1=1; 
00552      LeftMotor_P2=1;
00553      RightMotor_P1=1;
00554      RightMotor_P2=1; 
00555 } 
00556 
00557 float LidarSense(int minpos, int maxpos){
00558     float sum=0.0;
00559     int cnt =0;
00560     float Avg = 0.0;
00561    // if (Flg2 == 1)  {
00562             for(int i=minpos; i<=maxpos; i++) {
00563                   LidarServo = (i/100.0);
00564                   L_Dist=Lidar.distance();
00565                   LSrvoPos = LidarServo.read(); 
00566                   //pc.printf("D %d P %f\n\r",L_Dist ,LSrvoPos);
00567                   dt.reset();
00568                   sum = sum+L_Dist;
00569                   cnt = cnt+1;
00570                   
00571                   /*if (Flg == 1){
00572                     drive_forward((float)Level);  
00573                     }
00574                 else if (Flg == 2){
00575                     drive_backward((float)Level);
00576                     } 
00577                 else if (Flg == 0){
00578                     Robot_Stop();
00579                     } */
00580                   //printf("%f\r\n",myservo.read());
00581                   wait(0.008);
00582                  }
00583                 for(int i=maxpos; i>=minpos; i--) {
00584                   LidarServo = (i/100.0);
00585                   L_Dist=Lidar.distance();
00586                   LSrvoPos = LidarServo.read();
00587                   //pc.printf("D %d P %f\n\r",L_Dist,LSrvoPos);
00588                   //pc.printf("distance = %d cm frequency = %.2f Hz ServoPos = %f degrees\n\r", Lidar.distance(), 1/dt.read(),LidarServo.read());
00589                   dt.reset();
00590                   sum = sum+L_Dist; 
00591                   cnt = cnt+1;
00592                   //printf("%f\r\n",myservo.read());
00593                   /*if (Flg == 1){
00594                     drive_forward((float)Level);  
00595                     }
00596                 else if (Flg == 2){
00597                     drive_backward((float)Level);
00598                     } 
00599                 else if (Flg == 0){
00600                     Robot_Stop();
00601                     } */
00602                   wait(0.008);
00603                 }
00604     Avg = sum/cnt;
00605     pc.printf("flg2 = %d \r\n",Flg2);
00606     pc.printf("Avg = %f \r\n",Avg);
00607     return Avg;
00608     //LidarSense((int)LsrvMin,(int)LsrvMax);
00609       //  }
00610     //else{ 
00611     //     Flg2=0;
00612     //    }    
00613     }
00614     
00615    /* int LidarRotate(int pos){
00616          int Max_L_Dist =0;
00617          int Max_LsrvPos = 0;
00618          
00619          for(int i= pos; i<=99; i++) {
00620                   LidarServo = (i/100.0);
00621                   L_Dist=Lidar.distance();
00622                   LSrvoPos = LidarServo.read();
00623                   if (L_Dist > Max_L_Dist) {
00624                         Max_L_Dist = L_Dist;
00625                         Max_LsrvPos= i;
00626                   }
00627                   
00628                   
00629                  // cnt = cnt+1;
00630                   
00631                     //} 
00632                   wait(0.008);
00633                   lst_L_Dist = L_Dist;
00634                 }*/
00635        
00636         
00637         
00638        // }
00639