Abhinaba Bhattacharjee / Mbed 2 deprecated AutoRobo

For all

Dependencies:   LidarLitev2 Servo mbed

AutoRoboMotion.cpp@0:32adce4ab239, 2017-05-05 (annotated)

Committer:
AbhiBjee
Date:
Fri May 05 19:01:21 2017 +0000
Revision:
0:32adce4ab239
FRDM Smart Car code by Abhinaba

Who changed what in which revision?

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