Abhinaba Bhattacharjee / Mbed 2 deprecated AutoMode_LaneKeep

For All

Dependencies:   LidarLitev2 Servo mbed

CollisionAviod_LaneKeep.cpp@0:8999d566f525, 2017-05-05 (annotated)

Committer:
AbhiBjee
Date:
Fri May 05 19:03:31 2017 +0000
Revision:
0:8999d566f525
Lane Keeping with Collision avoidance with FRDM K64F

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AbhiBjee 0:8999d566f525 1 #include "mbed.h"
AbhiBjee 0:8999d566f525 2 //#include "Map.hpp"
AbhiBjee 0:8999d566f525 3 #include "LidarLitev2.h"
AbhiBjee 0:8999d566f525 4
AbhiBjee 0:8999d566f525 5 #include "Servo.h"// The desired Servo sweep range is 60 degrees
AbhiBjee 0:8999d566f525 6 // But due to some mechanical Load errors it has been calibrated to 0 to 70 degrees
AbhiBjee 0:8999d566f525 7 // The front facing position varies from 27 degress to 38 degrees due to the servo motion slip.
AbhiBjee 0:8999d566f525 8
AbhiBjee 0:8999d566f525 9 // Variable declaration
AbhiBjee 0:8999d566f525 10 Servo myservo(D3);// Object to declare the servo functionalities
AbhiBjee 0:8999d566f525 11 Servo LidarServo(D7);
AbhiBjee 0:8999d566f525 12 int L_Dist;
AbhiBjee 0:8999d566f525 13 float LSrvoPos;
AbhiBjee 0:8999d566f525 14
AbhiBjee 0:8999d566f525 15 int pos = 33;// Default avg front facing posiion of the Servo
AbhiBjee 0:8999d566f525 16 int i;
AbhiBjee 0:8999d566f525 17 int j;
AbhiBjee 0:8999d566f525 18 int i1;
AbhiBjee 0:8999d566f525 19 int j1;
AbhiBjee 0:8999d566f525 20 int BT_Val;
AbhiBjee 0:8999d566f525 21 int Cnt=0;
AbhiBjee 0:8999d566f525 22 float Dist_Val;
AbhiBjee 0:8999d566f525 23 int LsrvMin = 10;
AbhiBjee 0:8999d566f525 24 int LsrvMax =90;
AbhiBjee 0:8999d566f525 25 float Val;
AbhiBjee 0:8999d566f525 26
AbhiBjee 0:8999d566f525 27 //DigitalIn IR1(A0);
AbhiBjee 0:8999d566f525 28 //DigitalIn IR2(A1);
AbhiBjee 0:8999d566f525 29
AbhiBjee 0:8999d566f525 30 AnalogIn IR1(A0);
AbhiBjee 0:8999d566f525 31 AnalogIn IR2(A1);
AbhiBjee 0:8999d566f525 32
AbhiBjee 0:8999d566f525 33 DigitalIn OA(D4);
AbhiBjee 0:8999d566f525 34 DigitalIn OA2(D2);
AbhiBjee 0:8999d566f525 35
AbhiBjee 0:8999d566f525 36 int Flg=0; // Flag to enable motions (Forward, Backward, Stop state)
AbhiBjee 0:8999d566f525 37 int Flg2 = 0;
AbhiBjee 0:8999d566f525 38 int FlgSt = 0;// Unused Variable
AbhiBjee 0:8999d566f525 39 float Level = 0.30;// Variable to store the PWM Levels activated by the gear Button in the App.
AbhiBjee 0:8999d566f525 40 float pwm_Level;// Unused Variable
AbhiBjee 0:8999d566f525 41 float LeftPos;
AbhiBjee 0:8999d566f525 42 float RightPos;
AbhiBjee 0:8999d566f525 43 // Function Initialisation
AbhiBjee 0:8999d566f525 44 void handleInput(int in);
AbhiBjee 0:8999d566f525 45 void drive_forward(float level);
AbhiBjee 0:8999d566f525 46 void drive_backward(float level);
AbhiBjee 0:8999d566f525 47 void Robot_Stop();
AbhiBjee 0:8999d566f525 48 float LidarSense(int minpos, int maxpos);
AbhiBjee 0:8999d566f525 49
AbhiBjee 0:8999d566f525 50 LidarLitev2 Lidar(PTC11, PTC10);// Tx Rx for I2C Communication in Lidar
AbhiBjee 0:8999d566f525 51 Serial pc(USBTX,USBRX);//Tx Rx for Serial Communication (UART)(PC to Processor)
AbhiBjee 0:8999d566f525 52 Serial BT(PTC15,PTC14);// Tx Rx for Bluetooth Communication ()
AbhiBjee 0:8999d566f525 53
AbhiBjee 0:8999d566f525 54 PwmOut ENApwm(D5); // Motor driver Enable A
AbhiBjee 0:8999d566f525 55 PwmOut ENBpwm(D6); // Motor driver Enable B
AbhiBjee 0:8999d566f525 56
AbhiBjee 0:8999d566f525 57 // Setting coresponding pins to output mode for Motor movements
AbhiBjee 0:8999d566f525 58 DigitalOut LeftMotor_P1(D8);
AbhiBjee 0:8999d566f525 59 DigitalOut LeftMotor_P2(D9);
AbhiBjee 0:8999d566f525 60 DigitalOut RightMotor_P1(D11);
AbhiBjee 0:8999d566f525 61 DigitalOut RightMotor_P2(D10);
AbhiBjee 0:8999d566f525 62
AbhiBjee 0:8999d566f525 63 Timer dt;
AbhiBjee 0:8999d566f525 64
AbhiBjee 0:8999d566f525 65
AbhiBjee 0:8999d566f525 66 // The main Function
AbhiBjee 0:8999d566f525 67 int main() {
AbhiBjee 0:8999d566f525 68 pc.baud(9600);// UART communication in 9600 bits per seconds
AbhiBjee 0:8999d566f525 69 BT.baud(9600);// Bluetooth communication in 9600 bits per seconds
AbhiBjee 0:8999d566f525 70 // Printing some execution statements to ensure the controller is communicating properly.
AbhiBjee 0:8999d566f525 71 pc.printf(" Hello User!\r\n");
AbhiBjee 0:8999d566f525 72 printf(" Are You Ready \r\n");
AbhiBjee 0:8999d566f525 73 // At the start set servo to its default front facing position
AbhiBjee 0:8999d566f525 74 if(pos>33){
AbhiBjee 0:8999d566f525 75 myservo = (30/100.0);
AbhiBjee 0:8999d566f525 76 }
AbhiBjee 0:8999d566f525 77 else{
AbhiBjee 0:8999d566f525 78 myservo = (39/100.0);
AbhiBjee 0:8999d566f525 79 }
AbhiBjee 0:8999d566f525 80
AbhiBjee 0:8999d566f525 81
AbhiBjee 0:8999d566f525 82 while(1){ // run an infinite loop to read blutooth data & call corresponding navigation function
AbhiBjee 0:8999d566f525 83 int Tsop2_Val = OA2.read();
AbhiBjee 0:8999d566f525 84 int Tsop1_Val = OA.read();
AbhiBjee 0:8999d566f525 85 float value1 = IR1.read();
AbhiBjee 0:8999d566f525 86 float value2 = IR2.read();
AbhiBjee 0:8999d566f525 87 // pos = ((myservo.read())*100.0);
AbhiBjee 0:8999d566f525 88 if (BT.readable()){ // Read incoming Bluetooth Data
AbhiBjee 0:8999d566f525 89 BT_Val = (int)(pc.putc(BT.getc()));// get the Ascii value of the incomming 1 byte data
AbhiBjee 0:8999d566f525 90 //int i = atoi((int)BT_Val); // Convert it to Interger.
AbhiBjee 0:8999d566f525 91 //BT_Val -='0';
AbhiBjee 0:8999d566f525 92
AbhiBjee 0:8999d566f525 93 //pc.printf("%d \r\n",BT_Val); // print the Bluetooth data in serial Monitor for cross validation
AbhiBjee 0:8999d566f525 94 // Bluetooth data below decimal 25 contains certain values for the button operations
AbhiBjee 0:8999d566f525 95 // Bluetooth data above 25 triggers the PWM pins of EnA and EnB for motor speed control
AbhiBjee 0:8999d566f525 96 if (BT_Val <= 10){
AbhiBjee 0:8999d566f525 97 handleInput(BT_Val);
AbhiBjee 0:8999d566f525 98 }
AbhiBjee 0:8999d566f525 99 else if (BT_Val > 41){
AbhiBjee 0:8999d566f525 100 float Newpos = (((float)BT_Val - 70.0)*(65.0 - 5.0)/(210.0 - 70.0) + 0.0 );
AbhiBjee 0:8999d566f525 101 /*if (Newpos<=5.0){Newpos=5.0;}
AbhiBjee 0:8999d566f525 102 else if (Newpos>=65.0){Newpos=65.0;}
AbhiBjee 0:8999d566f525 103 else {Newpos = Newpos;}*/
AbhiBjee 0:8999d566f525 104 myservo = (Newpos/100.0);
AbhiBjee 0:8999d566f525 105 LidarServo=(45/100.0);
AbhiBjee 0:8999d566f525 106 /*BT_Val_diff = BT_Val-LstBT_Val;
AbhiBjee 0:8999d566f525 107 pos = pos+ (BT_Val_diff*0.5);
AbhiBjee 0:8999d566f525 108 myservo = (pos/100.0);*/
AbhiBjee 0:8999d566f525 109 }
AbhiBjee 0:8999d566f525 110
AbhiBjee 0:8999d566f525 111
AbhiBjee 0:8999d566f525 112
AbhiBjee 0:8999d566f525 113 //Level=Level;
AbhiBjee 0:8999d566f525 114 //if BT_Val == 'B'
AbhiBjee 0:8999d566f525 115 }
AbhiBjee 0:8999d566f525 116 //LstBT_Val=BT_Val;
AbhiBjee 0:8999d566f525 117 pc.printf("%d %f %d %d \r\n",BT_Val, Level, Flg2, Flg);
AbhiBjee 0:8999d566f525 118
AbhiBjee 0:8999d566f525 119 if (BT_Val >= 30 && BT_Val <=40){
AbhiBjee 0:8999d566f525 120 Val = (((float)BT_Val - 30.0)*(100.0 - 0.0)/(40.0 - 30.0) + 0.0 );
AbhiBjee 0:8999d566f525 121 Level = ((float)Val/100);
AbhiBjee 0:8999d566f525 122 pc.printf("%f \r\n",Level);
AbhiBjee 0:8999d566f525 123 //float pwm_Level = Level;
AbhiBjee 0:8999d566f525 124 }
AbhiBjee 0:8999d566f525 125 else if (BT_Val > 10 && BT_Val <=20){
AbhiBjee 0:8999d566f525 126 handleInput(BT_Val);
AbhiBjee 0:8999d566f525 127 }
AbhiBjee 0:8999d566f525 128
AbhiBjee 0:8999d566f525 129
AbhiBjee 0:8999d566f525 130
AbhiBjee 0:8999d566f525 131 if (Flg2 == 1){
AbhiBjee 0:8999d566f525 132 Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax);
AbhiBjee 0:8999d566f525 133 if (Dist_Val< 150.0){
AbhiBjee 0:8999d566f525 134 LsrvMin = 10;
AbhiBjee 0:8999d566f525 135 LsrvMax = 90;
AbhiBjee 0:8999d566f525 136 Val=0.0;
AbhiBjee 0:8999d566f525 137 Level = ((float)Val/100);
AbhiBjee 0:8999d566f525 138 Cnt = Cnt+1;
AbhiBjee 0:8999d566f525 139 }
AbhiBjee 0:8999d566f525 140 else if ((Dist_Val> 150.0)&&(Dist_Val<= 300.0)){
AbhiBjee 0:8999d566f525 141 //LsrvMin=
AbhiBjee 0:8999d566f525 142 LsrvMin = (((float)Dist_Val - 150.0)*(40.0 - 10.0)/(300.0 - 150.0) + 10.0 );
AbhiBjee 0:8999d566f525 143 LsrvMax = (((float)Dist_Val - 150.0)*(60.0 - 90.0)/(300.0 - 150.0) + 90.0 );
AbhiBjee 0:8999d566f525 144 Val = (((float)Dist_Val - 150.0)*(75.0 - 0.0)/(300.0 - 150.0) + 0.0 );
AbhiBjee 0:8999d566f525 145 Level = ((float)Val/100);
AbhiBjee 0:8999d566f525 146 Cnt=0;
AbhiBjee 0:8999d566f525 147 if (BT_Val==13){Flg=0;}
AbhiBjee 0:8999d566f525 148 }
AbhiBjee 0:8999d566f525 149 else if (Dist_Val> 300.0){
AbhiBjee 0:8999d566f525 150 LsrvMin = 40;
AbhiBjee 0:8999d566f525 151 LsrvMax = 70;
AbhiBjee 0:8999d566f525 152 Val=75.0;
AbhiBjee 0:8999d566f525 153 Level = ((float)Val/100);
AbhiBjee 0:8999d566f525 154 Cnt=0;
AbhiBjee 0:8999d566f525 155 if (BT_Val==13){Flg=0;}
AbhiBjee 0:8999d566f525 156 }
AbhiBjee 0:8999d566f525 157
AbhiBjee 0:8999d566f525 158 if (Cnt == 10){
AbhiBjee 0:8999d566f525 159 Flg = 2;
AbhiBjee 0:8999d566f525 160 Flg2=2;
AbhiBjee 0:8999d566f525 161 //Flg = 0;
AbhiBjee 0:8999d566f525 162 Cnt=0;
AbhiBjee 0:8999d566f525 163 Val=20.0;
AbhiBjee 0:8999d566f525 164 Level = ((float)Val/100);
AbhiBjee 0:8999d566f525 165 //pc.printf(" Moving Back \r\n");
AbhiBjee 0:8999d566f525 166 LidarServo=(45/100.0);
AbhiBjee 0:8999d566f525 167 //wait(2);
AbhiBjee 0:8999d566f525 168 //Flg=0;
AbhiBjee 0:8999d566f525 169 //Flg=0;
AbhiBjee 0:8999d566f525 170 }
AbhiBjee 0:8999d566f525 171 }
AbhiBjee 0:8999d566f525 172 else if(Flg2 == 2){
AbhiBjee 0:8999d566f525 173 Flg = 2;
AbhiBjee 0:8999d566f525 174 //pc.printf(" Moving Back \r\n");
AbhiBjee 0:8999d566f525 175 while(Cnt<100){
AbhiBjee 0:8999d566f525 176 if (Tsop1_Val==1){drive_backward((float)Level);}
AbhiBjee 0:8999d566f525 177 else{Robot_Stop();Flg = 0;break;}
AbhiBjee 0:8999d566f525 178 pc.printf(" Moving Back \r\n");
AbhiBjee 0:8999d566f525 179 Cnt+=1;
AbhiBjee 0:8999d566f525 180 wait(0.1);
AbhiBjee 0:8999d566f525 181 }
AbhiBjee 0:8999d566f525 182 //wait(2);
AbhiBjee 0:8999d566f525 183 Flg2 =0;
AbhiBjee 0:8999d566f525 184 Flg=0;
AbhiBjee 0:8999d566f525 185 Cnt=0;
AbhiBjee 0:8999d566f525 186 }
AbhiBjee 0:8999d566f525 187 else if (Flg2 == 6){
AbhiBjee 0:8999d566f525 188 Level = (27.0/100.0);
AbhiBjee 0:8999d566f525 189 Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax);
AbhiBjee 0:8999d566f525 190 if (Dist_Val< 150.0){
AbhiBjee 0:8999d566f525 191 LsrvMin = 45;
AbhiBjee 0:8999d566f525 192 LsrvMax = 90;
AbhiBjee 0:8999d566f525 193 Val=0.0;
AbhiBjee 0:8999d566f525 194 Level = ((float)Val/100);
AbhiBjee 0:8999d566f525 195 Flg2=0;Flg=0;
AbhiBjee 0:8999d566f525 196 // Cnt = Cnt+1;
AbhiBjee 0:8999d566f525 197 }
AbhiBjee 0:8999d566f525 198 else if ((Dist_Val> 150.0)&&(Dist_Val<= 200.0)){
AbhiBjee 0:8999d566f525 199 //LsrvMin=
AbhiBjee 0:8999d566f525 200 LsrvMin = (((float)Dist_Val - 150.0)*(45.0 - 30.0)/(200.0 - 150.0) + 30.0 );
AbhiBjee 0:8999d566f525 201 LsrvMax = (((float)Dist_Val - 150.0)*(60.0 - 90.0)/(200.0 - 150.0) + 90.0 );
AbhiBjee 0:8999d566f525 202 Val = (((float)Dist_Val - 150.0)*(27.0 - 0.0)/(200.0 - 150.0) + 0.0 );
AbhiBjee 0:8999d566f525 203 Level = ((float)Val/100);
AbhiBjee 0:8999d566f525 204 /*pos = (int)((myservo.read())*100.0);
AbhiBjee 0:8999d566f525 205 LeftPos = pos-10.0;
AbhiBjee 0:8999d566f525 206 RightPos = pos+20.0;
AbhiBjee 0:8999d566f525 207 myservo.write(RightPos/100.0);
AbhiBjee 0:8999d566f525 208 wait(3);
AbhiBjee 0:8999d566f525 209 myservo.write(LeftPos/100.0);
AbhiBjee 0:8999d566f525 210 wait(0.01);
AbhiBjee 0:8999d566f525 211 pos = (int)((myservo.read())*100.0);
AbhiBjee 0:8999d566f525 212 if(pos>37.5){
AbhiBjee 0:8999d566f525 213 myservo = (40.0/100.0);
AbhiBjee 0:8999d566f525 214 }
AbhiBjee 0:8999d566f525 215 else{
AbhiBjee 0:8999d566f525 216 myservo = (35.0/100.0);
AbhiBjee 0:8999d566f525 217 }*/
AbhiBjee 0:8999d566f525 218 Cnt=0;
AbhiBjee 0:8999d566f525 219 if (BT_Val==15){Flg=0;}
AbhiBjee 0:8999d566f525 220 }
AbhiBjee 0:8999d566f525 221 else if (Dist_Val> 200.0){
AbhiBjee 0:8999d566f525 222 LsrvMin = 49;
AbhiBjee 0:8999d566f525 223 LsrvMax = 51;
AbhiBjee 0:8999d566f525 224 //Val=25.0;
AbhiBjee 0:8999d566f525 225 Level = (27.0/100);
AbhiBjee 0:8999d566f525 226 if ((value1>=0.99)&& (value2<0.8)){
AbhiBjee 0:8999d566f525 227 pos = (int)((myservo.read())*100.0);
AbhiBjee 0:8999d566f525 228 LeftPos = pos-10.0;
AbhiBjee 0:8999d566f525 229 RightPos = pos+5.0;
AbhiBjee 0:8999d566f525 230 myservo.write(LeftPos/100.0);
AbhiBjee 0:8999d566f525 231 wait(1);
AbhiBjee 0:8999d566f525 232 myservo.write(RightPos/100.0);
AbhiBjee 0:8999d566f525 233 wait(0.1);
AbhiBjee 0:8999d566f525 234 pos = (int)((myservo.read())*100.0);
AbhiBjee 0:8999d566f525 235 if(pos>33.5){
AbhiBjee 0:8999d566f525 236 myservo = (40.0/100.0);
AbhiBjee 0:8999d566f525 237 }
AbhiBjee 0:8999d566f525 238 else{
AbhiBjee 0:8999d566f525 239 myservo = (39.0/100.0);
AbhiBjee 0:8999d566f525 240 }
AbhiBjee 0:8999d566f525 241 }
AbhiBjee 0:8999d566f525 242 else if ((value2>=0.99)&& (value1<0.8)){
AbhiBjee 0:8999d566f525 243 pos = ((myservo.read())*100.0);
AbhiBjee 0:8999d566f525 244 LeftPos = pos-5.0;
AbhiBjee 0:8999d566f525 245 RightPos = pos+10.0;
AbhiBjee 0:8999d566f525 246 myservo.write(RightPos/100.0);
AbhiBjee 0:8999d566f525 247 wait(1);
AbhiBjee 0:8999d566f525 248 myservo.write(LeftPos/100.0);
AbhiBjee 0:8999d566f525 249 wait(0.01);
AbhiBjee 0:8999d566f525 250 pos = (int)((myservo.read())*100.0);
AbhiBjee 0:8999d566f525 251 if(pos>37.5){
AbhiBjee 0:8999d566f525 252 myservo = (40.0/100.0);
AbhiBjee 0:8999d566f525 253 }
AbhiBjee 0:8999d566f525 254 else{
AbhiBjee 0:8999d566f525 255 myservo = (35.0/100.0);
AbhiBjee 0:8999d566f525 256 }
AbhiBjee 0:8999d566f525 257
AbhiBjee 0:8999d566f525 258 }
AbhiBjee 0:8999d566f525 259
AbhiBjee 0:8999d566f525 260 Cnt=0;
AbhiBjee 0:8999d566f525 261
AbhiBjee 0:8999d566f525 262 }
AbhiBjee 0:8999d566f525 263 }
AbhiBjee 0:8999d566f525 264 // Flags for RoboMotion
AbhiBjee 0:8999d566f525 265 if (Flg == 1){
AbhiBjee 0:8999d566f525 266 if (Tsop2_Val==1){drive_forward((float)Level);}
AbhiBjee 0:8999d566f525 267 else{Robot_Stop();Flg = 0;}
AbhiBjee 0:8999d566f525 268 //drive_forward((float)Level);
AbhiBjee 0:8999d566f525 269 }
AbhiBjee 0:8999d566f525 270 else if (Flg == 2){
AbhiBjee 0:8999d566f525 271 if (Tsop1_Val==1){drive_backward((float)Level);}
AbhiBjee 0:8999d566f525 272 else{Robot_Stop();Flg = 0;}
AbhiBjee 0:8999d566f525 273 //drive_backward((float)Level);
AbhiBjee 0:8999d566f525 274 }
AbhiBjee 0:8999d566f525 275 else if (Flg == 0){
AbhiBjee 0:8999d566f525 276 Robot_Stop();
AbhiBjee 0:8999d566f525 277 }
AbhiBjee 0:8999d566f525 278
AbhiBjee 0:8999d566f525 279 // forward and backward collision avoidance by TSOP IR
AbhiBjee 0:8999d566f525 280
AbhiBjee 0:8999d566f525 281 /*if ((Flg==2)&& (Tsop1_Val==1)){
AbhiBjee 0:8999d566f525 282 drive_backward(0.3f);
AbhiBjee 0:8999d566f525 283 }
AbhiBjee 0:8999d566f525 284 else if ((Flg==2)&& (Tsop1_Val==0)){
AbhiBjee 0:8999d566f525 285 Robot_Stop();
AbhiBjee 0:8999d566f525 286 Flg=0;
AbhiBjee 0:8999d566f525 287 }
AbhiBjee 0:8999d566f525 288 if ((Flg==1)&& (Tsop2_Val==1)){
AbhiBjee 0:8999d566f525 289 drive_forward((float)Level);
AbhiBjee 0:8999d566f525 290 }
AbhiBjee 0:8999d566f525 291 else if ((Flg==1)&& (Tsop2_Val==0)){
AbhiBjee 0:8999d566f525 292 Robot_Stop();
AbhiBjee 0:8999d566f525 293 Flg=0;
AbhiBjee 0:8999d566f525 294 }*/
AbhiBjee 0:8999d566f525 295
AbhiBjee 0:8999d566f525 296 }
AbhiBjee 0:8999d566f525 297 }
AbhiBjee 0:8999d566f525 298
AbhiBjee 0:8999d566f525 299 // Function which calls specific execution for corresponding incomming Serial inputs from Bluetooth
AbhiBjee 0:8999d566f525 300
AbhiBjee 0:8999d566f525 301 void handleInput(int in) {
AbhiBjee 0:8999d566f525 302 switch(in) {
AbhiBjee 0:8999d566f525 303 //Motor Rotation Controls
AbhiBjee 0:8999d566f525 304 case 6 :// Forward Motion
AbhiBjee 0:8999d566f525 305 Flg = 1;
AbhiBjee 0:8999d566f525 306 break;
AbhiBjee 0:8999d566f525 307 case 7 :
AbhiBjee 0:8999d566f525 308 Flg = 2;// Backward Motion
AbhiBjee 0:8999d566f525 309 break;
AbhiBjee 0:8999d566f525 310 case 0 : // Robot Stop
AbhiBjee 0:8999d566f525 311 Flg = 0;
AbhiBjee 0:8999d566f525 312 Flg2 = 0;
AbhiBjee 0:8999d566f525 313 break;
AbhiBjee 0:8999d566f525 314
AbhiBjee 0:8999d566f525 315 case 13 : // Activate Lidar
AbhiBjee 0:8999d566f525 316 Flg2 = 1;
AbhiBjee 0:8999d566f525 317 break;
AbhiBjee 0:8999d566f525 318 case 15 : // Activate Lane Keeping.
AbhiBjee 0:8999d566f525 319 pos = (int)((myservo.read())*100);
AbhiBjee 0:8999d566f525 320 if(pos>33){
AbhiBjee 0:8999d566f525 321 myservo = (27/100.0);
AbhiBjee 0:8999d566f525 322 }
AbhiBjee 0:8999d566f525 323 else{
AbhiBjee 0:8999d566f525 324 myservo = (35/100.0);
AbhiBjee 0:8999d566f525 325 }
AbhiBjee 0:8999d566f525 326 Flg2 = 4;
AbhiBjee 0:8999d566f525 327 break;
AbhiBjee 0:8999d566f525 328 case 14 : // Activate Lane Keeping.
AbhiBjee 0:8999d566f525 329 pos = (int)((myservo.read())*100);
AbhiBjee 0:8999d566f525 330 if(pos>33){
AbhiBjee 0:8999d566f525 331 myservo = (27/100.0);
AbhiBjee 0:8999d566f525 332 }
AbhiBjee 0:8999d566f525 333 else{
AbhiBjee 0:8999d566f525 334 myservo = (35/100.0);
AbhiBjee 0:8999d566f525 335 }
AbhiBjee 0:8999d566f525 336 Flg2 = 6;
AbhiBjee 0:8999d566f525 337 break;
AbhiBjee 0:8999d566f525 338
AbhiBjee 0:8999d566f525 339 //Servo Rotation Controls
AbhiBjee 0:8999d566f525 340 case 1:// Turn 30 degree Left and back to front facing
AbhiBjee 0:8999d566f525 341 if (pos >5){
AbhiBjee 0:8999d566f525 342 myservo = (pos/100.0);
AbhiBjee 0:8999d566f525 343 for( j1=pos; j1>=5; j1--) {
AbhiBjee 0:8999d566f525 344 myservo = (j1/100.0);
AbhiBjee 0:8999d566f525 345 //printf("%f\r\n",myservo.read());
AbhiBjee 0:8999d566f525 346 wait(0.01);
AbhiBjee 0:8999d566f525 347 }
AbhiBjee 0:8999d566f525 348 pos=j1;
AbhiBjee 0:8999d566f525 349 for(i1=pos; i1<=39; i1++) {
AbhiBjee 0:8999d566f525 350 myservo = (i1/100.0);
AbhiBjee 0:8999d566f525 351 //printf("%f\r\n",myservo.read());
AbhiBjee 0:8999d566f525 352 wait(0.01);
AbhiBjee 0:8999d566f525 353 }
AbhiBjee 0:8999d566f525 354 pos=i1;
AbhiBjee 0:8999d566f525 355 }
AbhiBjee 0:8999d566f525 356 else{
AbhiBjee 0:8999d566f525 357 myservo = (10/100.0);
AbhiBjee 0:8999d566f525 358 pos = 5;
AbhiBjee 0:8999d566f525 359 for(i1=pos; i1<=39; i1++) {
AbhiBjee 0:8999d566f525 360 myservo = (i1/100.0);
AbhiBjee 0:8999d566f525 361 //printf("%f\r\n",myservo.read());
AbhiBjee 0:8999d566f525 362 wait(0.01);
AbhiBjee 0:8999d566f525 363 }
AbhiBjee 0:8999d566f525 364 pos=i1;
AbhiBjee 0:8999d566f525 365 }
AbhiBjee 0:8999d566f525 366 break;
AbhiBjee 0:8999d566f525 367
AbhiBjee 0:8999d566f525 368 case 2: //Turn 30 degree Left
AbhiBjee 0:8999d566f525 369 if (pos >5){
AbhiBjee 0:8999d566f525 370 //myservo = (pos/100.0);
AbhiBjee 0:8999d566f525 371 for( j1=pos; j1>=5; j1--) {
AbhiBjee 0:8999d566f525 372 myservo = (j1/100.0);
AbhiBjee 0:8999d566f525 373 // printf("%f\r\n",myservo.read());
AbhiBjee 0:8999d566f525 374 wait(0.01);
AbhiBjee 0:8999d566f525 375 }
AbhiBjee 0:8999d566f525 376 pos=j1;
AbhiBjee 0:8999d566f525 377 }
AbhiBjee 0:8999d566f525 378 else{
AbhiBjee 0:8999d566f525 379 myservo = (5/100.0);
AbhiBjee 0:8999d566f525 380 }
AbhiBjee 0:8999d566f525 381 break;
AbhiBjee 0:8999d566f525 382
AbhiBjee 0:8999d566f525 383 case 3: // Sweep Servo to forward position irrespective of its current position
AbhiBjee 0:8999d566f525 384 if(pos >40){
AbhiBjee 0:8999d566f525 385 //myservo = (pos/100.0);
AbhiBjee 0:8999d566f525 386 for( j=pos; j>=30; j--) {
AbhiBjee 0:8999d566f525 387 myservo = (j/100.0);
AbhiBjee 0:8999d566f525 388 //printf("%f\r\n",myservo.read());
AbhiBjee 0:8999d566f525 389 wait(0.01);
AbhiBjee 0:8999d566f525 390 }
AbhiBjee 0:8999d566f525 391 pos=j;
AbhiBjee 0:8999d566f525 392
AbhiBjee 0:8999d566f525 393 }
AbhiBjee 0:8999d566f525 394 else if (pos < 25){
AbhiBjee 0:8999d566f525 395 //myservo = (pos/100.0);
AbhiBjee 0:8999d566f525 396 for(i1=pos; i1<=39; i1++) {
AbhiBjee 0:8999d566f525 397 myservo = (i1/100.0);
AbhiBjee 0:8999d566f525 398 //printf("%f\r\n",myservo.read());
AbhiBjee 0:8999d566f525 399 wait(0.01);
AbhiBjee 0:8999d566f525 400 }
AbhiBjee 0:8999d566f525 401 pos=i1;
AbhiBjee 0:8999d566f525 402
AbhiBjee 0:8999d566f525 403 }
AbhiBjee 0:8999d566f525 404 else{
AbhiBjee 0:8999d566f525 405 myservo = (pos/100.0);
AbhiBjee 0:8999d566f525 406 }
AbhiBjee 0:8999d566f525 407 break;
AbhiBjee 0:8999d566f525 408
AbhiBjee 0:8999d566f525 409 case 4://Turn 30 degree Right
AbhiBjee 0:8999d566f525 410 if (pos <66){
AbhiBjee 0:8999d566f525 411 // myservo = (pos/100.0);
AbhiBjee 0:8999d566f525 412 for(i=pos; i<=65; i++) {
AbhiBjee 0:8999d566f525 413 myservo = (i/100.0);
AbhiBjee 0:8999d566f525 414 //printf("%f\r\n",myservo.read());
AbhiBjee 0:8999d566f525 415 wait(0.01);
AbhiBjee 0:8999d566f525 416 }
AbhiBjee 0:8999d566f525 417 pos=i;
AbhiBjee 0:8999d566f525 418 }
AbhiBjee 0:8999d566f525 419 else{
AbhiBjee 0:8999d566f525 420 myservo = (65/100.0);
AbhiBjee 0:8999d566f525 421 }
AbhiBjee 0:8999d566f525 422 break;
AbhiBjee 0:8999d566f525 423
AbhiBjee 0:8999d566f525 424 case 5:// Turn 30 degree Right and back to front facing
AbhiBjee 0:8999d566f525 425 if (pos <66){
AbhiBjee 0:8999d566f525 426 //myservo = (pos/100.0);
AbhiBjee 0:8999d566f525 427 for(i=pos; i<=65; i++) {
AbhiBjee 0:8999d566f525 428 myservo = (i/100.0);
AbhiBjee 0:8999d566f525 429 //printf("%f\r\n",myservo.read());
AbhiBjee 0:8999d566f525 430 wait(0.01);
AbhiBjee 0:8999d566f525 431 }
AbhiBjee 0:8999d566f525 432 pos=i;
AbhiBjee 0:8999d566f525 433 for( j=pos; j>=30; j--) {
AbhiBjee 0:8999d566f525 434 myservo = (j/100.0);
AbhiBjee 0:8999d566f525 435 //printf("%f\r\n",myservo.read());
AbhiBjee 0:8999d566f525 436 wait(0.01);
AbhiBjee 0:8999d566f525 437 }
AbhiBjee 0:8999d566f525 438 pos=j;
AbhiBjee 0:8999d566f525 439 }
AbhiBjee 0:8999d566f525 440 else{
AbhiBjee 0:8999d566f525 441 myservo = (65/100.0);
AbhiBjee 0:8999d566f525 442 pos = 65;
AbhiBjee 0:8999d566f525 443 for( j=pos; j>=30; j--) {
AbhiBjee 0:8999d566f525 444 myservo = (j/100.0);
AbhiBjee 0:8999d566f525 445 //printf("%f\r\n",myservo.read());
AbhiBjee 0:8999d566f525 446 wait(0.01);
AbhiBjee 0:8999d566f525 447 }
AbhiBjee 0:8999d566f525 448 pos=j;
AbhiBjee 0:8999d566f525 449 }
AbhiBjee 0:8999d566f525 450 //break;
AbhiBjee 0:8999d566f525 451 break;
AbhiBjee 0:8999d566f525 452
AbhiBjee 0:8999d566f525 453 case 8:// Turn Servo Left by 5 degrees irrespective of its current position
AbhiBjee 0:8999d566f525 454 pos = pos-5;
AbhiBjee 0:8999d566f525 455 myservo = (pos/100.0);
AbhiBjee 0:8999d566f525 456 if (pos < 5){
AbhiBjee 0:8999d566f525 457 pos = 5;
AbhiBjee 0:8999d566f525 458 }
AbhiBjee 0:8999d566f525 459 break;
AbhiBjee 0:8999d566f525 460
AbhiBjee 0:8999d566f525 461 case 9: // Turn Servo right by 5 degrees irrespective of its current position
AbhiBjee 0:8999d566f525 462 pos = pos+5;
AbhiBjee 0:8999d566f525 463 myservo = (pos/100.0);
AbhiBjee 0:8999d566f525 464 if (pos > 65){
AbhiBjee 0:8999d566f525 465 pos = 65;
AbhiBjee 0:8999d566f525 466 }
AbhiBjee 0:8999d566f525 467 break;
AbhiBjee 0:8999d566f525 468
AbhiBjee 0:8999d566f525 469 default:
AbhiBjee 0:8999d566f525 470 pc.printf("I don't know: \"%d\"\n", in);
AbhiBjee 0:8999d566f525 471 break;
AbhiBjee 0:8999d566f525 472 }
AbhiBjee 0:8999d566f525 473 }
AbhiBjee 0:8999d566f525 474
AbhiBjee 0:8999d566f525 475 // Function to move rear wheel DC motors forward
AbhiBjee 0:8999d566f525 476
AbhiBjee 0:8999d566f525 477 void drive_forward(float level){
AbhiBjee 0:8999d566f525 478 ENApwm.write(level);
AbhiBjee 0:8999d566f525 479 ENBpwm.write(level);
AbhiBjee 0:8999d566f525 480 LeftMotor_P1=1;
AbhiBjee 0:8999d566f525 481 LeftMotor_P2=0;
AbhiBjee 0:8999d566f525 482 RightMotor_P1=1;
AbhiBjee 0:8999d566f525 483 RightMotor_P2=0;
AbhiBjee 0:8999d566f525 484 }
AbhiBjee 0:8999d566f525 485
AbhiBjee 0:8999d566f525 486 // Function to move rear wheel DC motors backward
AbhiBjee 0:8999d566f525 487 void drive_backward(float level){
AbhiBjee 0:8999d566f525 488 ENApwm.write(level);
AbhiBjee 0:8999d566f525 489 ENBpwm.write(level);
AbhiBjee 0:8999d566f525 490 LeftMotor_P1=0;
AbhiBjee 0:8999d566f525 491 LeftMotor_P2=1;
AbhiBjee 0:8999d566f525 492 RightMotor_P1=0;
AbhiBjee 0:8999d566f525 493 RightMotor_P2=1;
AbhiBjee 0:8999d566f525 494 }
AbhiBjee 0:8999d566f525 495
AbhiBjee 0:8999d566f525 496
AbhiBjee 0:8999d566f525 497 // Function to stop rear wheel motion
AbhiBjee 0:8999d566f525 498 void Robot_Stop() {
AbhiBjee 0:8999d566f525 499 //ENApwm.write(level);
AbhiBjee 0:8999d566f525 500 //ENBpwm.write(level);
AbhiBjee 0:8999d566f525 501 LeftMotor_P1=1;
AbhiBjee 0:8999d566f525 502 LeftMotor_P2=1;
AbhiBjee 0:8999d566f525 503 RightMotor_P1=1;
AbhiBjee 0:8999d566f525 504 RightMotor_P2=1;
AbhiBjee 0:8999d566f525 505 }
AbhiBjee 0:8999d566f525 506
AbhiBjee 0:8999d566f525 507 float LidarSense(int minpos, int maxpos){
AbhiBjee 0:8999d566f525 508 float sum=0.0;
AbhiBjee 0:8999d566f525 509 int cnt =0;
AbhiBjee 0:8999d566f525 510 float Avg = 0.0;
AbhiBjee 0:8999d566f525 511 // if (Flg2 == 1) {
AbhiBjee 0:8999d566f525 512 for(int i=minpos; i<=maxpos; i++) {
AbhiBjee 0:8999d566f525 513 LidarServo = (i/100.0);
AbhiBjee 0:8999d566f525 514 L_Dist=Lidar.distance();
AbhiBjee 0:8999d566f525 515 LSrvoPos = LidarServo.read();
AbhiBjee 0:8999d566f525 516 //pc.printf("D %d P %f\n\r",L_Dist ,LSrvoPos);
AbhiBjee 0:8999d566f525 517 dt.reset();
AbhiBjee 0:8999d566f525 518 sum = sum+L_Dist;
AbhiBjee 0:8999d566f525 519 cnt = cnt+1;
AbhiBjee 0:8999d566f525 520
AbhiBjee 0:8999d566f525 521 /*if (Flg == 1){
AbhiBjee 0:8999d566f525 522 drive_forward((float)Level);
AbhiBjee 0:8999d566f525 523 }
AbhiBjee 0:8999d566f525 524 else if (Flg == 2){
AbhiBjee 0:8999d566f525 525 drive_backward((float)Level);
AbhiBjee 0:8999d566f525 526 }
AbhiBjee 0:8999d566f525 527 else if (Flg == 0){
AbhiBjee 0:8999d566f525 528 Robot_Stop();
AbhiBjee 0:8999d566f525 529 } */
AbhiBjee 0:8999d566f525 530 //printf("%f\r\n",myservo.read());
AbhiBjee 0:8999d566f525 531 wait(0.008);
AbhiBjee 0:8999d566f525 532 }
AbhiBjee 0:8999d566f525 533 for(int i=maxpos; i>=minpos; i--) {
AbhiBjee 0:8999d566f525 534 LidarServo = (i/100.0);
AbhiBjee 0:8999d566f525 535 L_Dist=Lidar.distance();
AbhiBjee 0:8999d566f525 536 LSrvoPos = LidarServo.read();
AbhiBjee 0:8999d566f525 537 //pc.printf("D %d P %f\n\r",L_Dist,LSrvoPos);
AbhiBjee 0:8999d566f525 538 //pc.printf("distance = %d cm frequency = %.2f Hz ServoPos = %f degrees\n\r", Lidar.distance(), 1/dt.read(),LidarServo.read());
AbhiBjee 0:8999d566f525 539 dt.reset();
AbhiBjee 0:8999d566f525 540 sum = sum+L_Dist;
AbhiBjee 0:8999d566f525 541 cnt = cnt+1;
AbhiBjee 0:8999d566f525 542 //printf("%f\r\n",myservo.read());
AbhiBjee 0:8999d566f525 543 /*if (Flg == 1){
AbhiBjee 0:8999d566f525 544 drive_forward((float)Level);
AbhiBjee 0:8999d566f525 545 }
AbhiBjee 0:8999d566f525 546 else if (Flg == 2){
AbhiBjee 0:8999d566f525 547 drive_backward((float)Level);
AbhiBjee 0:8999d566f525 548 }
AbhiBjee 0:8999d566f525 549 else if (Flg == 0){
AbhiBjee 0:8999d566f525 550 Robot_Stop();
AbhiBjee 0:8999d566f525 551 } */
AbhiBjee 0:8999d566f525 552 wait(0.008);
AbhiBjee 0:8999d566f525 553 }
AbhiBjee 0:8999d566f525 554 Avg = sum/cnt;
AbhiBjee 0:8999d566f525 555 pc.printf("flg2 = %d \r\n",Flg2);
AbhiBjee 0:8999d566f525 556 pc.printf("Avg = %f \r\n",Avg);
AbhiBjee 0:8999d566f525 557 return Avg;
AbhiBjee 0:8999d566f525 558 //LidarSense((int)LsrvMin,(int)LsrvMax);
AbhiBjee 0:8999d566f525 559 // }
AbhiBjee 0:8999d566f525 560 //else{
AbhiBjee 0:8999d566f525 561 // Flg2=0;
AbhiBjee 0:8999d566f525 562 // }
AbhiBjee 0:8999d566f525 563 }