Revision 0:32adce4ab239, committed 2017-05-05

Comitter:

AbhiBjee

Date:

Fri May 05 19:01:21 2017 +0000

Commit message:

FRDM Smart Car code by Abhinaba

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AutoRoboMotion.cpp	Fri May 05 19:01:21 2017 +0000
@@ -0,0 +1,639 @@
+#include "mbed.h"
+//#include "Map.hpp"
+#include "LidarLitev2.h"
+
+#include "Servo.h"// The desired Servo sweep range is 60 degrees
+// But due to some mechanical Load errors it has been calibrated to 0 to 70 degrees
+// The front facing position varies from 27 degress to 38 degrees due to the servo motion slip.
+
+// Variable declaration
+Servo myservo(D3);// Object to declare the servo functionalities
+float pos = 33;// Default avg front facing posiion of the Servo
+Servo LidarServo(D7);
+int L_Dist;
+float LSrvoPos;
+
+DigitalIn IR1(A0);
+DigitalIn IR2(A1);
+
+int L2Cnt=0;
+int L1Cnt=0;
+int LstVal1 =0;
+int LstVal2 =0;
+
+DigitalIn OA(D4);
+DigitalIn OA2(D2);
+int Tsop1_Val;
+int i;
+int j;
+int i1;
+int j1;
+float Dist_Val; 
+int LsrvMin = 10;
+int LsrvMax =90; 
+float Val;
+
+Timer dt;
+int Cnt=0;
+int Flg2=0; // For Sensor activation
+int Flg=0; // Flag to enable motions (Forward, Backward, Stop state)
+int Flg2St = 0;// Unused Variable
+float Level = 0.30;// Variable to store the PWM Levels activated by the gear Button in the App.
+float pwm_Level;// Unused Variable
+// Function Initialisation
+void handleInput(int in); 
+void drive_forward(float level);
+void drive_backward(float level);
+void Robot_Stop();
+float LidarSense(int minpos, int maxpos);
+
+LidarLitev2 Lidar(PTC11, PTC10);// Tx Rx for I2C Communication in Lidar
+Serial pc(USBTX,USBRX);//Tx Rx for Serial Communication (UART)(PC to Processor)
+Serial BT(PTC15,PTC14);// Tx Rx for Bluetooth Communication () 
+
+PwmOut ENApwm(D5); // Motor driver Enable A
+PwmOut ENBpwm(D6); // Motor driver Enable B
+
+// Setting coresponding pins to output mode for Motor movements
+DigitalOut LeftMotor_P1(D8);
+DigitalOut LeftMotor_P2(D9);
+DigitalOut RightMotor_P1(D11);
+DigitalOut RightMotor_P2(D10);
+
+
+// The main Function 
+int main() {
+    pc.baud(9600);// UART communication in 9600 bits per seconds
+    BT.baud(9600);// Bluetooth communication in 9600 bits per seconds
+    // Printing some execution statements to ensure the controller is communicating properly.
+    pc.printf(" Hello User!\r\n");
+    printf(" Are You Ready \r\n");    
+   // At the start set servo to its default front facing  position 
+    if(pos>33){
+        myservo = (27/100.0);
+    }
+    else{
+         myservo = (35/100.0);        
+    }
+    Lidar.configure();
+    dt.start();        
+   
+    while(1){ // run an infinite loop to read blutooth data & call corresponding navigation function
+        if (BT.readable()){ // Read incoming Bluetooth Data
+         int BT_Val = (int)(pc.putc(BT.getc()));// get the Ascii value of the incomming 1 byte data
+           //int i = atoi((int)BT_Val);        // Convert it to Interger.
+           //BT_Val -='0';
+           pc.printf("%d \r\n",BT_Val); // print the Bluetooth data in serial Monitor for cross validation
+           // Bluetooth data below decimal 25 contains certain values for the button operations 
+           // Bluetooth data above 25 triggers the PWM pins of EnA and EnB for motor speed control
+          
+           if (BT_Val >= 30 && BT_Val <=40){
+               float Val = (((float)BT_Val - 30.0)*(100.0 - 0.0)/(40.0 - 30.0) + 0.0 );
+               Level = ((float)Val/100);               
+               pc.printf("%f \r\n",Level);
+               //float pwm_Level = Level;
+               }
+           else if (BT_Val < 25){
+                handleInput(BT_Val);
+               }
+           else if (BT_Val > 41){// accelerometer of phone for motion control
+               float Newpos = (((float)BT_Val - 70.0)*(70.0 - 0.0)/(210.0 - 70.0) + 0.0 );
+               myservo = (Newpos/100.0);
+               /*BT_Val_diff = BT_Val-LstBT_Val;
+               pos = pos+ (BT_Val_diff*0.5);
+               myservo = (pos/100.0);*/
+               }  
+               
+             while (Flg2 ==1){
+                   Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax); 
+                    if (BT.readable()){ // Read incoming Bluetooth Data
+                       int BT_Val = (int)(pc.putc(BT.getc()));// get the Ascii value of the incomming 1 byte data           
+                       pc.printf("%d \r\n",BT_Val); // print the Bluetooth data in serial Monitor for cross validation
+                       
+                      if (BT_Val < 25){
+                        handleInput(BT_Val);                
+                      }
+                      
+                    }
+                    if (Flg == 1){
+                         drive_forward((float)Level);  
+                      }
+                    else if (Flg == 2){
+                         if(Dist_Val < 150){
+                              Val=50.0f;
+                              Level = ((float)Val/100);
+                             }                         
+                         drive_backward((float)Level);
+                      } 
+                    else if (Flg == 0){
+                         Robot_Stop();
+                      }
+                   // int L_Dist=Lidar.distance();
+                   // dt.reset();
+                   // LidarServo = (45/100.0);
+                     
+                    if (Dist_Val< 150.0){
+                        LsrvMin = 10;
+                        LsrvMax = 90;
+                        Val=0.0;
+                        Level = ((float)Val/100);
+                        Cnt = Cnt+1;                        
+                        }
+                    else if ((Dist_Val> 150.0)&&(Dist_Val<= 300.0)){
+                        //LsrvMin= 
+                        LsrvMin = (((float)Dist_Val - 150.0)*(40.0 - 10.0)/(300.0 - 150.0) + 10.0 );
+                        LsrvMax = (((float)Dist_Val - 150.0)*(60.0 - 90.0)/(300.0 - 150.0) + 90.0 );
+                        Val = (((float)Dist_Val - 150.0)*(50.0 - 0.0)/(300.0 - 150.0) + 0.0 );
+                        Level = ((float)Val/100);
+                        }
+                    else if (Dist_Val> 300.0){
+                        LsrvMin = 40;
+                        LsrvMax = 70;
+                        Val=100.0;
+                        Level = ((float)Val/100);
+                        }
+                        
+                    if (Cnt == 10){
+                        Flg2= 2; 
+                        Cnt=0;
+                        break;
+                        }
+                        
+                    int Tsop2_Val = OA2.read();
+                    int Tsop1_Val = OA.read(); 
+                    if ((Flg==2)&& (Tsop1_Val==1)){
+                         drive_backward((float)Level);
+                     }
+                    else if ((Flg==2)&& (Tsop1_Val==0)){
+                         Robot_Stop();
+                         Flg=0;
+             
+                    }
+                    if ((Flg==1)&& (Tsop2_Val==1)){
+                         drive_forward((float)Level);
+                    }
+                    else if ((Flg==1)&& (Tsop2_Val==0)){
+                          Robot_Stop();
+                          Flg=0;
+             
+                    }
+                        
+                    continue; 
+
+                  // break;                  
+               }  
+               
+          /* if (Flg2 == Flg2St) && ()
+               if (Flg2 ==1){
+                   Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax);                   
+                   }
+               else if (Flg2 ==0) {
+                   Robot_Stop();
+                   }               
+               }
+            else{
+                 if (Flg2 ==1){
+                   Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax);                   
+                   }
+               else if (Flg2 ==0) {
+                   Robot_Stop();
+                   }
+                }*/
+           
+           //pc.printf("Flg2 = %d", Flg2);
+           while(Flg2 ==2){
+                int Tsop1_Val = OA.read();                
+                if(Tsop1_Val==1){
+                     Val=40.0;
+                     Level = ((float)Val/100);
+                     drive_backward((float)Level);
+                     if(Cnt == 5){
+                          Cnt=0;
+                          Robot_Stop();
+                         }
+                     //wait(2);                     
+                }
+                else if (Tsop1_Val == 0){  
+                     Robot_Stop(); 
+                     Flg=0;               
+                     break;                     
+                }                   
+                Cnt=Cnt+1;                       
+            }
+              
+           /*while(Flg2 == 4){
+                if (BT.readable()){ // Read incoming Bluetooth Data
+                       int BT_Val = (int)(pc.putc(BT.getc()));// get the Ascii value of the incomming 1 byte data           
+                       pc.printf("%d \r\n",BT_Val); // print the Bluetooth data in serial Monitor for cross validation
+                       
+                      if (BT_Val < 25){
+                        handleInput(BT_Val);                
+                      }
+                      
+                }
+                Val=30.0;
+                Level = ((float)Val/100);
+                LidarServo = (45/100.0);
+                L_Dist=Lidar.distance();
+                LSrvoPos = LidarServo.read();
+                dt.reset();
+                int value1 = IR1.read();
+                int value2 = IR2.read();
+                if ((value1==1)&&(value1>=LstVal1)){
+                    L1Cnt=L1Cnt+1;
+                }
+                else if(value1<LstVal1){
+                    L1Cnt=0;
+                }
+                    
+                if ((value2==1)&&(value2>=LstVal2)){
+                    L2Cnt=L2Cnt+1;
+                }
+                else if(value2<LstVal2){
+                    L2Cnt=0;
+                }
+                //int ServoPos = (int)((myservo.read())*100);
+                int pos = ((int)((myservo.read())*100.0f));
+                
+                if ((L1Cnt>5)&&(L2Cnt<5)){
+                     pos = pos-3;
+                     myservo = (pos/100.0);
+                     //wait(0.01);
+                     pos = pos+1;
+                     myservo = (pos/100.0);
+                     if (pos < 24){
+                        pos = 24;                
+                     } 
+                }
+                else if ((L2Cnt>5)&&(L1Cnt<5)){
+                     pos = pos+3;
+                     myservo = (pos/100.0);
+                     //wait(0.01);
+                     pos = pos-1;
+                     myservo = (pos/100.0);
+                     if (pos > 38){
+                         pos = 38;                
+                     } 
+                } 
+                else if ((L2Cnt>=5)&&(L1Cnt>=5)){
+                    continue;
+                    }     
+                if (Flg == 1){
+                    drive_forward((float)Level);  
+                    }
+                else if (Flg == 2){
+                    drive_backward((float)Level);                    
+                    } 
+                else if (Flg == 0){
+                    Robot_Stop();
+                    break;
+                    }
+                int Tsop2_Val = OA2.read();
+                int Tsop1_Val = OA.read(); 
+                if ((Flg==2)&& (Tsop1_Val==1)){
+                    drive_backward((float)Level);
+                }
+                else if ((Flg==2)&& (Tsop1_Val==0)){
+                    Robot_Stop();
+                    Flg=0;
+                    break;
+             
+                }
+                /*if ((Flg==1)&& (Tsop2_Val==1)){
+                    drive_forward((float)Level);
+                }
+                else if ((Flg==1)&& (Tsop2_Val==0)){
+                    Robot_Stop();
+                    Flg=0; 
+                    break;            
+                }  
+                if (L_Dist<50){
+                    Robot_Stop();
+                    Flg=0;
+                    break;   
+                    }  
+                
+                LstVal1=value1;
+                LstVal2=value2;
+                continue;
+                
+            }*/
+               
+           if (Flg == 1){
+                    drive_forward((float)Level);  
+                    }
+           else if (Flg == 2){
+                    drive_backward((float)Level);                    
+                    } 
+           else if (Flg == 0){
+                    Robot_Stop();
+                    }   
+               
+      // while (Flg == 2) 
+                                    
+                                    
+        //if BT_Val == 'B'               
+        }
+        //LstBT_Val=BT_Val;
+        //Flg2St = Flg2;
+        int Tsop2_Val = OA2.read();
+        int Tsop1_Val = OA.read(); 
+        if ((Flg==2)&& (Tsop1_Val==1)){
+             drive_backward(0.3f);
+            }
+        else if ((Flg==2)&& (Tsop1_Val==0)){
+             Robot_Stop();
+             Flg=0;
+             
+            }
+        if ((Flg==1)&& (Tsop2_Val==1)){
+             drive_forward((float)Level);
+            }
+        else if ((Flg==1)&& (Tsop2_Val==0)){
+             Robot_Stop();
+             Flg=0;             
+            }
+        
+    }
+ } 
+    
+// Function which calls specific execution for corresponding incomming Serial inputs from Bluetooth
+
+void handleInput(int in) {
+    switch(in) { 
+    //Motor Rotation Controls       
+        case 6 :// Forward Motion
+            Flg = 1;
+            break;        
+        case 7 :
+            Flg = 2;// Backward Motion
+            break;
+        case 0 :  // Robot Stop      
+            
+                Flg = 0;
+                Flg2 = 0;
+            break;
+        case 13 :  // Robot Stop      
+            Flg2 = 1;
+            break;
+        case 14 :  // Robot Stop 
+            int pos = (int)((myservo.read())*100); 
+            if(pos>33){
+               myservo = (27/100.0);
+            }
+            else{
+               myservo = (35/100.0);        
+            }     
+            Flg2 = 4;
+            break;
+    //Servo Rotation Controls
+        case 1:// Turn 30 degree Left and back to front facing
+            if (pos >0){
+                myservo = (pos/100.0);
+                for( j1=pos; j1>=0; j1--) {
+                    myservo = (j1/100.0);
+                    //printf("%f\r\n",myservo.read());
+                    wait(0.01);        
+                }
+                pos=j1;
+                for(i1=pos; i1<=35; i1++) {
+                    myservo = (i1/100.0);
+                    //printf("%f\r\n",myservo.read());
+                    wait(0.01);
+                }
+                pos=i1;         
+            }
+            else{
+                 myservo = (0/100.0); 
+                 pos = 0;
+                 for(i1=pos; i1<=35; i1++) {
+                    myservo = (i1/100.0);
+                    //printf("%f\r\n",myservo.read());
+                    wait(0.01);
+                }
+                pos=i1;                
+            }                
+            break;
+            
+        case 2: //Turn 30 degree Left 
+            if (pos >0){
+                //myservo = (pos/100.0);
+                for( j1=pos; j1>=0; j1--) {
+                    myservo = (j1/100.0);
+                   // printf("%f\r\n",myservo.read());
+                    wait(0.01);        
+                }
+                pos=j1;         
+            }
+            else{
+                 myservo = (0/100.0);                
+            }      
+            break;
+            
+        case 3: // Sweep Servo to forward position irrespective of its current position
+            if(pos >40){
+                //myservo = (pos/100.0);
+                for( j=pos; j>=27; j--) {
+                    myservo = (j/100.0);
+                    //printf("%f\r\n",myservo.read());
+                    wait(0.01);        
+                }
+                pos=j;
+                
+            }
+            else if (pos < 25){
+                //myservo = (pos/100.0);
+                for(i1=pos; i1<=35; i1++) {
+                    myservo = (i1/100.0);
+                    //printf("%f\r\n",myservo.read());
+                    wait(0.01);
+                }
+                pos=i1;   
+                
+            }
+            else{
+                myservo = (pos/100.0);                
+            }
+            break;
+            
+        case 4://Turn 30 degree Right
+            if (pos <61){
+               // myservo = (pos/100.0);
+                for(i=pos; i<=60; i++) {
+                    myservo = (i/100.0);
+                    //printf("%f\r\n",myservo.read());
+                    wait(0.01);
+                }
+                pos=i;                
+            }
+            else{
+                myservo = (60/100.0);                 
+            }
+            break;
+            
+        case 5:// Turn 30 degree Right and back to front facing
+            if (pos <61){
+                //myservo = (pos/100.0);
+                for(i=pos; i<=60; i++) {
+                    myservo = (i/100.0);
+                    //printf("%f\r\n",myservo.read());
+                    wait(0.01);
+                }
+                pos=i; 
+                for( j=pos; j>=27; j--) {
+                    myservo = (j/100.0);
+                    //printf("%f\r\n",myservo.read());
+                    wait(0.01);        
+                }
+                pos=j;               
+            }
+            else{
+                myservo = (60/100.0);
+                pos = 60;
+                for( j=pos; j>=27; j--) {
+                    myservo = (j/100.0);
+                    //printf("%f\r\n",myservo.read());
+                    wait(0.01);        
+                }
+                pos=j;                 
+            }
+            //break;
+            break;
+            
+        case 8:// Turn Servo Left by 5 degrees irrespective of its current position
+            pos = pos-5;
+            myservo = (pos/100.0);
+            if (pos < 0){
+                pos = 0;                
+            }   
+            break;
+            
+        case 9: // Turn Servo right by 5 degrees irrespective of its current position
+            pos = pos+5;
+            myservo = (pos/100.0); 
+            if (pos > 60){
+                pos = 60;                
+            }            
+            break; 
+        
+        default:
+            pc.printf("I don't know: \"%d\"\n", in);
+            break;
+    }
+}
+
+// Function to move rear wheel DC motors forward
+
+void drive_forward(float level){
+     ENApwm.write(level);
+     ENBpwm.write(level); 
+     LeftMotor_P1=1; 
+     LeftMotor_P2=0;
+     RightMotor_P1=1;
+     RightMotor_P2=0; 
+} 
+
+// Function to move rear wheel DC motors backward 
+void drive_backward(float level){
+     ENApwm.write(level);
+     ENBpwm.write(level); 
+     LeftMotor_P1=0; 
+     LeftMotor_P2=1;
+     RightMotor_P1=0;
+     RightMotor_P2=1; 
+ } 
+ 
+
+// Function to stop rear wheel motion
+void Robot_Stop() {
+     //ENApwm.write(level);
+     //ENBpwm.write(level); 
+     LeftMotor_P1=1; 
+     LeftMotor_P2=1;
+     RightMotor_P1=1;
+     RightMotor_P2=1; 
+} 
+
+float LidarSense(int minpos, int maxpos){
+    float sum=0.0;
+    int cnt =0;
+    float Avg = 0.0;
+   // if (Flg2 == 1)  {
+            for(int i=minpos; i<=maxpos; i++) {
+                  LidarServo = (i/100.0);
+                  L_Dist=Lidar.distance();
+                  LSrvoPos = LidarServo.read(); 
+                  //pc.printf("D %d P %f\n\r",L_Dist ,LSrvoPos);
+                  dt.reset();
+                  sum = sum+L_Dist;
+                  cnt = cnt+1;
+                  
+                  /*if (Flg == 1){
+                    drive_forward((float)Level);  
+                    }
+                else if (Flg == 2){
+                    drive_backward((float)Level);
+                    } 
+                else if (Flg == 0){
+                    Robot_Stop();
+                    } */
+                  //printf("%f\r\n",myservo.read());
+                  wait(0.008);
+                 }
+                for(int i=maxpos; i>=minpos; i--) {
+                  LidarServo = (i/100.0);
+                  L_Dist=Lidar.distance();
+                  LSrvoPos = LidarServo.read();
+                  //pc.printf("D %d P %f\n\r",L_Dist,LSrvoPos);
+                  //pc.printf("distance = %d cm frequency = %.2f Hz ServoPos = %f degrees\n\r", Lidar.distance(), 1/dt.read(),LidarServo.read());
+                  dt.reset();
+                  sum = sum+L_Dist; 
+                  cnt = cnt+1;
+                  //printf("%f\r\n",myservo.read());
+                  /*if (Flg == 1){
+                    drive_forward((float)Level);  
+                    }
+                else if (Flg == 2){
+                    drive_backward((float)Level);
+                    } 
+                else if (Flg == 0){
+                    Robot_Stop();
+                    } */
+                  wait(0.008);
+                }
+    Avg = sum/cnt;
+    pc.printf("flg2 = %d \r\n",Flg2);
+    pc.printf("Avg = %f \r\n",Avg);
+    return Avg;
+    //LidarSense((int)LsrvMin,(int)LsrvMax);
+      //  }
+    //else{ 
+    //     Flg2=0;
+    //    }    
+    }
+    
+   /* int LidarRotate(int pos){
+         int Max_L_Dist =0;
+         int Max_LsrvPos = 0;
+         
+         for(int i= pos; i<=99; i++) {
+                  LidarServo = (i/100.0);
+                  L_Dist=Lidar.distance();
+                  LSrvoPos = LidarServo.read();
+                  if (L_Dist > Max_L_Dist) {
+                        Max_L_Dist = L_Dist;
+                        Max_LsrvPos= i;
+                  }
+                  
+                  
+                 // cnt = cnt+1;
+                  
+                    //} 
+                  wait(0.008);
+                  lst_L_Dist = L_Dist;
+                }*/
+       
+        
+        
+       // }
+    
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LidarLitev2.lib	Fri May 05 19:01:21 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/sventura3/code/LidarLitev2/#7e6c07be1754

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Fri May 05 19:01:21 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri May 05 19:01:21 2017 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/mbed/builds/97feb9bacc10
\ No newline at end of file