For All
Dependencies: LidarLitev2 Servo mbed
Diff: CollisionAviod_LaneKeep.cpp
- Revision:
- 0:8999d566f525
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CollisionAviod_LaneKeep.cpp Fri May 05 19:03:31 2017 +0000 @@ -0,0 +1,563 @@ +#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; + // } +} \ No newline at end of file