For All
Dependencies: LidarLitev2 Servo mbed
CollisionAviod_LaneKeep.cpp
- Committer:
- AbhiBjee
- Date:
- 2017-05-05
- Revision:
- 0:8999d566f525
File content as of revision 0:8999d566f525:
#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 Servo LidarServo(D7); int L_Dist; float LSrvoPos; int pos = 33;// Default avg front facing posiion of the Servo int i; int j; int i1; int j1; int BT_Val; int Cnt=0; float Dist_Val; int LsrvMin = 10; int LsrvMax =90; float Val; //DigitalIn IR1(A0); //DigitalIn IR2(A1); AnalogIn IR1(A0); AnalogIn IR2(A1); DigitalIn OA(D4); DigitalIn OA2(D2); int Flg=0; // Flag to enable motions (Forward, Backward, Stop state) int Flg2 = 0; int FlgSt = 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 float LeftPos; float RightPos; // 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); Timer dt; // 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 = (30/100.0); } else{ myservo = (39/100.0); } while(1){ // run an infinite loop to read blutooth data & call corresponding navigation function int Tsop2_Val = OA2.read(); int Tsop1_Val = OA.read(); float value1 = IR1.read(); float value2 = IR2.read(); // pos = ((myservo.read())*100.0); if (BT.readable()){ // Read incoming Bluetooth Data 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 <= 10){ handleInput(BT_Val); } else if (BT_Val > 41){ float Newpos = (((float)BT_Val - 70.0)*(65.0 - 5.0)/(210.0 - 70.0) + 0.0 ); /*if (Newpos<=5.0){Newpos=5.0;} else if (Newpos>=65.0){Newpos=65.0;} else {Newpos = Newpos;}*/ myservo = (Newpos/100.0); LidarServo=(45/100.0); /*BT_Val_diff = BT_Val-LstBT_Val; pos = pos+ (BT_Val_diff*0.5); myservo = (pos/100.0);*/ } //Level=Level; //if BT_Val == 'B' } //LstBT_Val=BT_Val; pc.printf("%d %f %d %d \r\n",BT_Val, Level, Flg2, Flg); if (BT_Val >= 30 && BT_Val <=40){ 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 > 10 && BT_Val <=20){ handleInput(BT_Val); } if (Flg2 == 1){ Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax); 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)*(75.0 - 0.0)/(300.0 - 150.0) + 0.0 ); Level = ((float)Val/100); Cnt=0; if (BT_Val==13){Flg=0;} } else if (Dist_Val> 300.0){ LsrvMin = 40; LsrvMax = 70; Val=75.0; Level = ((float)Val/100); Cnt=0; if (BT_Val==13){Flg=0;} } if (Cnt == 10){ Flg = 2; Flg2=2; //Flg = 0; Cnt=0; Val=20.0; Level = ((float)Val/100); //pc.printf(" Moving Back \r\n"); LidarServo=(45/100.0); //wait(2); //Flg=0; //Flg=0; } } else if(Flg2 == 2){ Flg = 2; //pc.printf(" Moving Back \r\n"); while(Cnt<100){ if (Tsop1_Val==1){drive_backward((float)Level);} else{Robot_Stop();Flg = 0;break;} pc.printf(" Moving Back \r\n"); Cnt+=1; wait(0.1); } //wait(2); Flg2 =0; Flg=0; Cnt=0; } else if (Flg2 == 6){ Level = (27.0/100.0); Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax); if (Dist_Val< 150.0){ LsrvMin = 45; LsrvMax = 90; Val=0.0; Level = ((float)Val/100); Flg2=0;Flg=0; // Cnt = Cnt+1; } else if ((Dist_Val> 150.0)&&(Dist_Val<= 200.0)){ //LsrvMin= LsrvMin = (((float)Dist_Val - 150.0)*(45.0 - 30.0)/(200.0 - 150.0) + 30.0 ); LsrvMax = (((float)Dist_Val - 150.0)*(60.0 - 90.0)/(200.0 - 150.0) + 90.0 ); Val = (((float)Dist_Val - 150.0)*(27.0 - 0.0)/(200.0 - 150.0) + 0.0 ); Level = ((float)Val/100); /*pos = (int)((myservo.read())*100.0); LeftPos = pos-10.0; RightPos = pos+20.0; myservo.write(RightPos/100.0); wait(3); myservo.write(LeftPos/100.0); wait(0.01); pos = (int)((myservo.read())*100.0); if(pos>37.5){ myservo = (40.0/100.0); } else{ myservo = (35.0/100.0); }*/ Cnt=0; if (BT_Val==15){Flg=0;} } else if (Dist_Val> 200.0){ LsrvMin = 49; LsrvMax = 51; //Val=25.0; Level = (27.0/100); if ((value1>=0.99)&& (value2<0.8)){ pos = (int)((myservo.read())*100.0); LeftPos = pos-10.0; RightPos = pos+5.0; myservo.write(LeftPos/100.0); wait(1); myservo.write(RightPos/100.0); wait(0.1); pos = (int)((myservo.read())*100.0); if(pos>33.5){ myservo = (40.0/100.0); } else{ myservo = (39.0/100.0); } } else if ((value2>=0.99)&& (value1<0.8)){ pos = ((myservo.read())*100.0); LeftPos = pos-5.0; RightPos = pos+10.0; myservo.write(RightPos/100.0); wait(1); myservo.write(LeftPos/100.0); wait(0.01); pos = (int)((myservo.read())*100.0); if(pos>37.5){ myservo = (40.0/100.0); } else{ myservo = (35.0/100.0); } } Cnt=0; } } // Flags for RoboMotion if (Flg == 1){ if (Tsop2_Val==1){drive_forward((float)Level);} else{Robot_Stop();Flg = 0;} //drive_forward((float)Level); } else if (Flg == 2){ if (Tsop1_Val==1){drive_backward((float)Level);} else{Robot_Stop();Flg = 0;} //drive_backward((float)Level); } else if (Flg == 0){ Robot_Stop(); } // forward and backward collision avoidance by TSOP IR /*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 : // Activate Lidar Flg2 = 1; break; case 15 : // Activate Lane Keeping. pos = (int)((myservo.read())*100); if(pos>33){ myservo = (27/100.0); } else{ myservo = (35/100.0); } Flg2 = 4; break; case 14 : // Activate Lane Keeping. pos = (int)((myservo.read())*100); if(pos>33){ myservo = (27/100.0); } else{ myservo = (35/100.0); } Flg2 = 6; break; //Servo Rotation Controls case 1:// Turn 30 degree Left and back to front facing if (pos >5){ myservo = (pos/100.0); for( j1=pos; j1>=5; j1--) { myservo = (j1/100.0); //printf("%f\r\n",myservo.read()); wait(0.01); } pos=j1; for(i1=pos; i1<=39; i1++) { myservo = (i1/100.0); //printf("%f\r\n",myservo.read()); wait(0.01); } pos=i1; } else{ myservo = (10/100.0); pos = 5; for(i1=pos; i1<=39; 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 >5){ //myservo = (pos/100.0); for( j1=pos; j1>=5; j1--) { myservo = (j1/100.0); // printf("%f\r\n",myservo.read()); wait(0.01); } pos=j1; } else{ myservo = (5/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>=30; 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<=39; 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 <66){ // myservo = (pos/100.0); for(i=pos; i<=65; i++) { myservo = (i/100.0); //printf("%f\r\n",myservo.read()); wait(0.01); } pos=i; } else{ myservo = (65/100.0); } break; case 5:// Turn 30 degree Right and back to front facing if (pos <66){ //myservo = (pos/100.0); for(i=pos; i<=65; i++) { myservo = (i/100.0); //printf("%f\r\n",myservo.read()); wait(0.01); } pos=i; for( j=pos; j>=30; j--) { myservo = (j/100.0); //printf("%f\r\n",myservo.read()); wait(0.01); } pos=j; } else{ myservo = (65/100.0); pos = 65; for( j=pos; j>=30; 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 < 5){ pos = 5; } break; case 9: // Turn Servo right by 5 degrees irrespective of its current position pos = pos+5; myservo = (pos/100.0); if (pos > 65){ pos = 65; } 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; // } }