CollisionAviod_LaneKeep.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 Servo LidarServo(D7);
00012 int L_Dist;
00013 float LSrvoPos;
00014 
00015 int pos = 33;// Default avg front facing posiion of the Servo
00016 int i;
00017 int j;
00018 int i1;
00019 int j1;
00020 int BT_Val;
00021 int Cnt=0;
00022 float Dist_Val; 
00023 int LsrvMin = 10;
00024 int LsrvMax =90; 
00025 float Val;
00026 
00027 //DigitalIn IR1(A0);
00028 //DigitalIn IR2(A1);
00029 
00030 AnalogIn IR1(A0);
00031 AnalogIn IR2(A1);
00032 
00033 DigitalIn OA(D4);
00034 DigitalIn OA2(D2);
00035 
00036 int Flg=0; // Flag to enable motions (Forward, Backward, Stop state)
00037 int Flg2 = 0;
00038 int FlgSt = 0;// Unused Variable
00039 float Level = 0.30;// Variable to store the PWM Levels activated by the gear Button in the App.
00040 float pwm_Level;// Unused Variable
00041 float LeftPos;
00042 float RightPos;
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 Timer dt;
00064 
00065 
00066 // The main Function 
00067 int main() {
00068     pc.baud(9600);// UART communication in 9600 bits per seconds
00069     BT.baud(9600);// Bluetooth communication in 9600 bits per seconds
00070     // Printing some execution statements to ensure the controller is communicating properly.
00071     pc.printf(" Hello User!\r\n");
00072     printf(" Are You Ready \r\n");    
00073    // At the start set servo to its default front facing  position 
00074     if(pos>33){
00075         myservo = (30/100.0);
00076     }
00077     else{
00078          myservo = (39/100.0);        
00079     }
00080         
00081    
00082     while(1){ // run an infinite loop to read blutooth data & call corresponding navigation function
00083         int Tsop2_Val = OA2.read();
00084         int Tsop1_Val = OA.read();
00085         float value1 = IR1.read();
00086         float value2 = IR2.read();
00087        // pos = ((myservo.read())*100.0); 
00088         if (BT.readable()){ // Read incoming Bluetooth Data
00089            BT_Val = (int)(pc.putc(BT.getc()));// get the Ascii value of the incomming 1 byte data
00090            //int i = atoi((int)BT_Val);        // Convert it to Interger.
00091            //BT_Val -='0';
00092            
00093            //pc.printf("%d \r\n",BT_Val); // print the Bluetooth data in serial Monitor for cross validation
00094            // Bluetooth data below decimal 25 contains certain values for the button operations 
00095            // Bluetooth data above 25 triggers the PWM pins of EnA and EnB for motor speed control
00096            if (BT_Val <= 10){
00097                 handleInput(BT_Val);
00098                }
00099             else if (BT_Val > 41){
00100                float Newpos = (((float)BT_Val - 70.0)*(65.0 - 5.0)/(210.0 - 70.0) + 0.0 );
00101                /*if (Newpos<=5.0){Newpos=5.0;}
00102                else if (Newpos>=65.0){Newpos=65.0;}
00103                else {Newpos = Newpos;}*/
00104                myservo = (Newpos/100.0);
00105                LidarServo=(45/100.0);
00106                /*BT_Val_diff = BT_Val-LstBT_Val;
00107                pos = pos+ (BT_Val_diff*0.5);
00108                myservo = (pos/100.0);*/
00109             }
00110            
00111            
00112            
00113             //Level=Level;                      
00114         //if BT_Val == 'B'               
00115         }
00116         //LstBT_Val=BT_Val;
00117        pc.printf("%d %f %d %d \r\n",BT_Val, Level, Flg2, Flg);
00118         
00119            if (BT_Val >= 30 && BT_Val <=40){
00120                Val = (((float)BT_Val - 30.0)*(100.0 - 0.0)/(40.0 - 30.0) + 0.0 );
00121                Level = ((float)Val/100);               
00122                pc.printf("%f \r\n",Level);
00123                //float pwm_Level = Level;
00124                }
00125             else if (BT_Val > 10 && BT_Val <=20){
00126                 handleInput(BT_Val);                
00127             }
00128             
00129            
00130                    
00131         if (Flg2 == 1){
00132             Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax); 
00133                     if (Dist_Val< 150.0){
00134                         LsrvMin = 10;
00135                         LsrvMax = 90;
00136                         Val=0.0;
00137                         Level = ((float)Val/100);
00138                         Cnt = Cnt+1;                        
00139                         }
00140                     else if ((Dist_Val> 150.0)&&(Dist_Val<= 300.0)){
00141                         //LsrvMin= 
00142                         LsrvMin = (((float)Dist_Val - 150.0)*(40.0 - 10.0)/(300.0 - 150.0) + 10.0 );
00143                         LsrvMax = (((float)Dist_Val - 150.0)*(60.0 - 90.0)/(300.0 - 150.0) + 90.0 );
00144                         Val = (((float)Dist_Val - 150.0)*(75.0 - 0.0)/(300.0 - 150.0) + 0.0 );
00145                         Level = ((float)Val/100);
00146                         Cnt=0;
00147                         if (BT_Val==13){Flg=0;}
00148                         }
00149                     else if (Dist_Val> 300.0){
00150                         LsrvMin = 40;
00151                         LsrvMax = 70;
00152                         Val=75.0;
00153                         Level = ((float)Val/100);
00154                         Cnt=0;
00155                         if (BT_Val==13){Flg=0;}
00156                         }
00157                         
00158                     if (Cnt == 10){
00159                         Flg = 2; 
00160                         Flg2=2;
00161                         //Flg = 0; 
00162                         Cnt=0;
00163                         Val=20.0;                        
00164                         Level = ((float)Val/100);
00165                         //pc.printf(" Moving Back \r\n");
00166                         LidarServo=(45/100.0);
00167                         //wait(2);
00168                         //Flg=0;
00169                         //Flg=0;                        
00170                         }
00171         }
00172         else if(Flg2 == 2){
00173              Flg = 2;
00174              //pc.printf(" Moving Back \r\n");
00175              while(Cnt<100){
00176                  if (Tsop1_Val==1){drive_backward((float)Level);}
00177                  else{Robot_Stop();Flg = 0;break;}
00178                  pc.printf(" Moving Back \r\n");
00179                  Cnt+=1;  
00180                  wait(0.1);              
00181              }
00182              //wait(2);
00183              Flg2 =0;
00184              Flg=0;
00185              Cnt=0;            
00186         } 
00187         else if (Flg2 == 6){
00188             Level = (27.0/100.0); 
00189             Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax); 
00190                    if (Dist_Val< 150.0){
00191                         LsrvMin = 45;
00192                         LsrvMax = 90;
00193                         Val=0.0;
00194                         Level = ((float)Val/100);
00195                         Flg2=0;Flg=0;
00196                        // Cnt = Cnt+1;                        
00197                         }
00198                     else if ((Dist_Val> 150.0)&&(Dist_Val<= 200.0)){
00199                         //LsrvMin= 
00200                         LsrvMin = (((float)Dist_Val - 150.0)*(45.0 - 30.0)/(200.0 - 150.0) + 30.0 );
00201                         LsrvMax = (((float)Dist_Val - 150.0)*(60.0 - 90.0)/(200.0 - 150.0) + 90.0 );
00202                         Val = (((float)Dist_Val - 150.0)*(27.0 - 0.0)/(200.0 - 150.0) + 0.0 );
00203                         Level = ((float)Val/100);
00204                         /*pos = (int)((myservo.read())*100.0);
00205                               LeftPos = pos-10.0;
00206                               RightPos = pos+20.0;
00207                               myservo.write(RightPos/100.0);
00208                               wait(3);
00209                               myservo.write(LeftPos/100.0);
00210                               wait(0.01);
00211                               pos = (int)((myservo.read())*100.0);
00212                               if(pos>37.5){
00213                                  myservo = (40.0/100.0);
00214                               }
00215                               else{
00216                                  myservo = (35.0/100.0);        
00217                               }*/
00218                         Cnt=0;
00219                         if (BT_Val==15){Flg=0;}
00220                         }
00221                     else if (Dist_Val> 200.0){
00222                         LsrvMin = 49;
00223                         LsrvMax = 51;
00224                         //Val=25.0;
00225                         Level = (27.0/100);
00226                         if ((value1>=0.99)&& (value2<0.8)){
00227                            pos = (int)((myservo.read())*100.0);
00228                            LeftPos = pos-10.0;
00229                            RightPos = pos+5.0;
00230                            myservo.write(LeftPos/100.0);
00231                            wait(1);
00232                            myservo.write(RightPos/100.0);
00233                            wait(0.1);
00234                            pos = (int)((myservo.read())*100.0);
00235                            if(pos>33.5){
00236                               myservo = (40.0/100.0);
00237                            }
00238                            else{
00239                               myservo = (39.0/100.0);        
00240                            }
00241                         }
00242                         else if ((value2>=0.99)&& (value1<0.8)){
00243                               pos = ((myservo.read())*100.0);
00244                               LeftPos = pos-5.0;
00245                               RightPos = pos+10.0;
00246                               myservo.write(RightPos/100.0);
00247                               wait(1);
00248                               myservo.write(LeftPos/100.0);
00249                               wait(0.01);
00250                               pos = (int)((myservo.read())*100.0);
00251                               if(pos>37.5){
00252                                  myservo = (40.0/100.0);
00253                               }
00254                               else{
00255                                  myservo = (35.0/100.0);        
00256                               }
00257              
00258                         }
00259          
00260                         Cnt=0;            
00261             
00262                  }   
00263         }
00264         // Flags for RoboMotion
00265         if (Flg == 1){
00266             if (Tsop2_Val==1){drive_forward((float)Level);}
00267             else{Robot_Stop();Flg = 0;}
00268             //drive_forward((float)Level);                
00269             }
00270         else if (Flg == 2){
00271             if (Tsop1_Val==1){drive_backward((float)Level);}
00272             else{Robot_Stop();Flg = 0;}
00273             //drive_backward((float)Level);
00274             } 
00275         else if (Flg == 0){
00276             Robot_Stop();
00277             }
00278             
00279         // forward and backward collision avoidance by TSOP IR
00280             
00281         /*if ((Flg==2)&& (Tsop1_Val==1)){
00282              drive_backward(0.3f);
00283             }
00284         else if ((Flg==2)&& (Tsop1_Val==0)){
00285              Robot_Stop();
00286              Flg=0;             
00287             }
00288         if ((Flg==1)&& (Tsop2_Val==1)){
00289              drive_forward((float)Level);
00290             }
00291         else if ((Flg==1)&& (Tsop2_Val==0)){
00292              Robot_Stop();
00293              Flg=0;             
00294             }*/
00295             
00296     }
00297 }  
00298     
00299 // Function which calls specific execution for corresponding incomming Serial inputs from Bluetooth
00300 
00301 void handleInput(int in) {
00302     switch(in) { 
00303     //Motor Rotation Controls       
00304         case 6 :// Forward Motion
00305             Flg = 1;
00306             break;        
00307         case 7 :
00308             Flg = 2;// Backward Motion
00309             break;
00310         case 0 :  // Robot Stop      
00311             Flg = 0;
00312             Flg2 = 0;
00313             break;
00314             
00315         case 13 :  // Activate Lidar     
00316             Flg2 = 1;
00317             break;
00318         case 15 :  // Activate Lane Keeping.
00319              pos = (int)((myservo.read())*100); 
00320             if(pos>33){
00321                myservo = (27/100.0);
00322             }
00323             else{
00324                myservo = (35/100.0);        
00325             }     
00326             Flg2 = 4;
00327             break;
00328         case 14 :  // Activate Lane Keeping.
00329             pos = (int)((myservo.read())*100); 
00330             if(pos>33){
00331                myservo = (27/100.0);
00332             }
00333             else{
00334                myservo = (35/100.0);        
00335             }     
00336             Flg2 = 6;
00337             break;
00338             
00339     //Servo Rotation Controls
00340         case 1:// Turn 30 degree Left and back to front facing
00341             if (pos >5){
00342                 myservo = (pos/100.0);
00343                 for( j1=pos; j1>=5; j1--) {
00344                     myservo = (j1/100.0);
00345                     //printf("%f\r\n",myservo.read());
00346                     wait(0.01);        
00347                 }
00348                 pos=j1;
00349                 for(i1=pos; i1<=39; i1++) {
00350                     myservo = (i1/100.0);
00351                     //printf("%f\r\n",myservo.read());
00352                     wait(0.01);
00353                 }
00354                 pos=i1;         
00355             }
00356             else{
00357                  myservo = (10/100.0); 
00358                  pos = 5;
00359                  for(i1=pos; i1<=39; i1++) {
00360                     myservo = (i1/100.0);
00361                     //printf("%f\r\n",myservo.read());
00362                     wait(0.01);
00363                 }
00364                 pos=i1;                
00365             }                
00366             break;
00367             
00368         case 2: //Turn 30 degree Left 
00369             if (pos >5){
00370                 //myservo = (pos/100.0);
00371                 for( j1=pos; j1>=5; j1--) {
00372                     myservo = (j1/100.0);
00373                    // printf("%f\r\n",myservo.read());
00374                     wait(0.01);        
00375                 }
00376                 pos=j1;         
00377             }
00378             else{
00379                  myservo = (5/100.0);                
00380             }      
00381             break;
00382             
00383         case 3: // Sweep Servo to forward position irrespective of its current position
00384             if(pos >40){
00385                 //myservo = (pos/100.0);
00386                 for( j=pos; j>=30; j--) {
00387                     myservo = (j/100.0);
00388                     //printf("%f\r\n",myservo.read());
00389                     wait(0.01);        
00390                 }
00391                 pos=j;
00392                 
00393             }
00394             else if (pos < 25){
00395                 //myservo = (pos/100.0);
00396                 for(i1=pos; i1<=39; i1++) {
00397                     myservo = (i1/100.0);
00398                     //printf("%f\r\n",myservo.read());
00399                     wait(0.01);
00400                 }
00401                 pos=i1;   
00402                 
00403             }
00404             else{
00405                 myservo = (pos/100.0);                
00406             }
00407             break;
00408             
00409         case 4://Turn 30 degree Right
00410             if (pos <66){
00411                // myservo = (pos/100.0);
00412                 for(i=pos; i<=65; i++) {
00413                     myservo = (i/100.0);
00414                     //printf("%f\r\n",myservo.read());
00415                     wait(0.01);
00416                 }
00417                 pos=i;                
00418             }
00419             else{
00420                 myservo = (65/100.0);                 
00421             }
00422             break;
00423             
00424         case 5:// Turn 30 degree Right and back to front facing
00425             if (pos <66){
00426                 //myservo = (pos/100.0);
00427                 for(i=pos; i<=65; i++) {
00428                     myservo = (i/100.0);
00429                     //printf("%f\r\n",myservo.read());
00430                     wait(0.01);
00431                 }
00432                 pos=i; 
00433                 for( j=pos; j>=30; j--) {
00434                     myservo = (j/100.0);
00435                     //printf("%f\r\n",myservo.read());
00436                     wait(0.01);        
00437                 }
00438                 pos=j;               
00439             }
00440             else{
00441                 myservo = (65/100.0);
00442                 pos = 65;
00443                 for( j=pos; j>=30; j--) {
00444                     myservo = (j/100.0);
00445                     //printf("%f\r\n",myservo.read());
00446                     wait(0.01);        
00447                 }
00448                 pos=j;                 
00449             }
00450             //break;
00451             break;
00452             
00453         case 8:// Turn Servo Left by 5 degrees irrespective of its current position
00454             pos = pos-5;
00455             myservo = (pos/100.0);
00456             if (pos < 5){
00457                 pos = 5;                
00458             }   
00459             break;
00460             
00461         case 9: // Turn Servo right by 5 degrees irrespective of its current position
00462             pos = pos+5;
00463             myservo = (pos/100.0); 
00464             if (pos > 65){
00465                 pos = 65;                
00466             }            
00467             break; 
00468         
00469         default:
00470             pc.printf("I don't know: \"%d\"\n", in);
00471             break;
00472     }
00473 }
00474 
00475 // Function to move rear wheel DC motors forward
00476 
00477 void drive_forward(float level){
00478      ENApwm.write(level);
00479      ENBpwm.write(level); 
00480      LeftMotor_P1=1; 
00481      LeftMotor_P2=0;
00482      RightMotor_P1=1;
00483      RightMotor_P2=0; 
00484 } 
00485 
00486 // Function to move rear wheel DC motors backward 
00487 void drive_backward(float level){
00488      ENApwm.write(level);
00489      ENBpwm.write(level); 
00490      LeftMotor_P1=0; 
00491      LeftMotor_P2=1;
00492      RightMotor_P1=0;
00493      RightMotor_P2=1; 
00494  } 
00495  
00496 
00497 // Function to stop rear wheel motion
00498 void Robot_Stop() {
00499      //ENApwm.write(level);
00500      //ENBpwm.write(level); 
00501      LeftMotor_P1=1; 
00502      LeftMotor_P2=1;
00503      RightMotor_P1=1;
00504      RightMotor_P2=1; 
00505 }
00506 
00507 float LidarSense(int minpos, int maxpos){
00508     float sum=0.0;
00509     int cnt =0;
00510     float Avg = 0.0;
00511    // if (Flg2 == 1)  {
00512             for(int i=minpos; i<=maxpos; i++) {
00513                   LidarServo = (i/100.0);
00514                   L_Dist=Lidar.distance();
00515                   LSrvoPos = LidarServo.read(); 
00516                   //pc.printf("D %d P %f\n\r",L_Dist ,LSrvoPos);
00517                   dt.reset();
00518                   sum = sum+L_Dist;
00519                   cnt = cnt+1;
00520                   
00521                   /*if (Flg == 1){
00522                     drive_forward((float)Level);  
00523                     }
00524                 else if (Flg == 2){
00525                     drive_backward((float)Level);
00526                     } 
00527                 else if (Flg == 0){
00528                     Robot_Stop();
00529                     } */
00530                   //printf("%f\r\n",myservo.read());
00531                   wait(0.008);
00532                  }
00533                 for(int i=maxpos; i>=minpos; i--) {
00534                   LidarServo = (i/100.0);
00535                   L_Dist=Lidar.distance();
00536                   LSrvoPos = LidarServo.read();
00537                   //pc.printf("D %d P %f\n\r",L_Dist,LSrvoPos);
00538                   //pc.printf("distance = %d cm frequency = %.2f Hz ServoPos = %f degrees\n\r", Lidar.distance(), 1/dt.read(),LidarServo.read());
00539                   dt.reset();
00540                   sum = sum+L_Dist; 
00541                   cnt = cnt+1;
00542                   //printf("%f\r\n",myservo.read());
00543                   /*if (Flg == 1){
00544                     drive_forward((float)Level);  
00545                     }
00546                 else if (Flg == 2){
00547                     drive_backward((float)Level);
00548                     } 
00549                 else if (Flg == 0){
00550                     Robot_Stop();
00551                     } */
00552                   wait(0.008);
00553                 }
00554     Avg = sum/cnt;
00555     pc.printf("flg2 = %d \r\n",Flg2);
00556     pc.printf("Avg = %f \r\n",Avg);
00557     return Avg;
00558     //LidarSense((int)LsrvMin,(int)LsrvMax);
00559       //  }
00560     //else{ 
00561     //     Flg2=0;
00562     //    }    
00563 }