monika kakani / Mbed 2 deprecated Obstacle_avoider

this robot uses ultrasonic sensors to detect the obstacles and the servo motor acts as the neck for the robot in checking the obstacles in right and left directions

Dependencies:   HCSR04 Servo mbed

Fork of Obstacle_avoidance by ece nith

main.cpp@1:43568a9d4323, 2017-03-24 (annotated)

Committer:
monikakakani
Date:
Fri Mar 24 01:02:04 2017 +0000
Revision:
1:43568a9d4323
Parent:
0:a87600bebf7b 
obstacle avoiding and detecting robot using ultrasonic and servo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kit3 0:a87600bebf7b 1 #include "mbed.h"
kit3 0:a87600bebf7b 2 #include "hcsr04.h"
kit3 0:a87600bebf7b 3 #include "Servo.h"
monikakakani 1:43568a9d4323 4 #include "string.h"
monikakakani 1:43568a9d4323 5 #define HIGH 1
monikakakani 1:43568a9d4323 6 #define LOW 0
monikakakani 1:43568a9d4323 7
monikakakani 1:43568a9d4323 8 void forward();
monikakakani 1:43568a9d4323 9 void backward();
monikakakani 1:43568a9d4323 10 void right();
monikakakani 1:43568a9d4323 11 void left();
monikakakani 1:43568a9d4323 12 void stop();
monikakakani 1:43568a9d4323 13 void loop();
monikakakani 1:43568a9d4323 14
monikakakani 1:43568a9d4323 15 void blueInterrupt();
monikakakani 1:43568a9d4323 16 void timerInterrupt();
monikakakani 1:43568a9d4323 17 void bluecheck();
monikakakani 1:43568a9d4323 18
monikakakani 1:43568a9d4323 19 int distance; //Variable to store distance from an object
monikakakani 1:43568a9d4323 20 int checkRight;
monikakakani 1:43568a9d4323 21 int checkLeft;
monikakakani 1:43568a9d4323 22 int function=1; //Variable to store function of robot: '1' running or '0' stoped. By
monikakakani 1:43568a9d4323 23 int pos=90;
monikakakani 1:43568a9d4323 24 int flagBlue=0;
monikakakani 1:43568a9d4323 25 int flagTimer=0;
monikakakani 1:43568a9d4323 26
monikakakani 1:43568a9d4323 27 Ticker timer;
monikakakani 1:43568a9d4323 28
monikakakani 1:43568a9d4323 29
monikakakani 1:43568a9d4323 30 int c;
monikakakani 1:43568a9d4323 31
monikakakani 1:43568a9d4323 32
monikakakani 1:43568a9d4323 33 Serial pc(USBTX,USBRX);
monikakakani 1:43568a9d4323 34 Serial blue(PTC15,PTC14);
monikakakani 1:43568a9d4323 35 PwmOut servo(D6);
monikakakani 1:43568a9d4323 36
monikakakani 1:43568a9d4323 37 char send[512],rcv[1000];//Variables
monikakakani 1:43568a9d4323 38
monikakakani 1:43568a9d4323 39 int main()
monikakakani 1:43568a9d4323 40 {
monikakakani 1:43568a9d4323 41
monikakakani 1:43568a9d4323 42 blue.baud(9600);
monikakakani 1:43568a9d4323 43
monikakakani 1:43568a9d4323 44 while(1)
monikakakani 1:43568a9d4323 45 {
monikakakani 1:43568a9d4323 46 if(!blue.readable())
monikakakani 1:43568a9d4323 47 {
monikakakani 1:43568a9d4323 48 !blue.gets(rcv,1000);
monikakakani 1:43568a9d4323 49 if(strstr(rcv,"forward")!=NULL)
monikakakani 1:43568a9d4323 50 {
monikakakani 1:43568a9d4323 51 forward();
monikakakani 1:43568a9d4323 52 }
monikakakani 1:43568a9d4323 53 else if(strstr(rcv,"backward")!=NULL)
monikakakani 1:43568a9d4323 54 {
monikakakani 1:43568a9d4323 55 backward();
monikakakani 1:43568a9d4323 56 }
monikakakani 1:43568a9d4323 57 else if(strstr(rcv,"right")!=NULL)
monikakakani 1:43568a9d4323 58 {
monikakakani 1:43568a9d4323 59 right();
monikakakani 1:43568a9d4323 60 }
monikakakani 1:43568a9d4323 61 else if(strstr(rcv,"left")!=NULL)
monikakakani 1:43568a9d4323 62 {
monikakakani 1:43568a9d4323 63 left();
monikakakani 1:43568a9d4323 64 }
monikakakani 1:43568a9d4323 65 }
monikakakani 1:43568a9d4323 66 else
monikakakani 1:43568a9d4323 67 {
monikakakani 1:43568a9d4323 68 stop();
monikakakani 1:43568a9d4323 69 }
monikakakani 1:43568a9d4323 70 }
monikakakani 1:43568a9d4323 71 }
monikakakani 1:43568a9d4323 72
monikakakani 1:43568a9d4323 73 void forward(){
monikakakani 1:43568a9d4323 74
monikakakani 1:43568a9d4323 75
monikakakani 1:43568a9d4323 76
monikakakani 1:43568a9d4323 77 pc.printf("fwd");
monikakakani 1:43568a9d4323 78
monikakakani 1:43568a9d4323 79 DigitalOut(D3,1);
monikakakani 1:43568a9d4323 80
monikakakani 1:43568a9d4323 81 DigitalOut(D5, 0);
monikakakani 1:43568a9d4323 82
monikakakani 1:43568a9d4323 83 DigitalOut(D10, 1);
monikakakani 1:43568a9d4323 84
monikakakani 1:43568a9d4323 85 DigitalOut(D11, 0);
monikakakani 1:43568a9d4323 86
monikakakani 1:43568a9d4323 87 }
monikakakani 1:43568a9d4323 88
monikakakani 1:43568a9d4323 89 void backward(){
monikakakani 1:43568a9d4323 90
monikakakani 1:43568a9d4323 91 DigitalOut(D3,0);
monikakakani 1:43568a9d4323 92
monikakakani 1:43568a9d4323 93 DigitalOut(D5, 1);
monikakakani 1:43568a9d4323 94
monikakakani 1:43568a9d4323 95 DigitalOut(D10, 0);
monikakakani 1:43568a9d4323 96
monikakakani 1:43568a9d4323 97 DigitalOut(D11, 1);
monikakakani 1:43568a9d4323 98
monikakakani 1:43568a9d4323 99 }
monikakakani 1:43568a9d4323 100
monikakakani 1:43568a9d4323 101
monikakakani 1:43568a9d4323 102 void right()
monikakakani 1:43568a9d4323 103
monikakakani 1:43568a9d4323 104
monikakakani 1:43568a9d4323 105 {
monikakakani 1:43568a9d4323 106
monikakakani 1:43568a9d4323 107
monikakakani 1:43568a9d4323 108 DigitalOut(D3,0);
monikakakani 1:43568a9d4323 109
monikakakani 1:43568a9d4323 110 DigitalOut(D5, 1);
monikakakani 1:43568a9d4323 111
monikakakani 1:43568a9d4323 112 DigitalOut(D10, 1);
monikakakani 1:43568a9d4323 113
monikakakani 1:43568a9d4323 114 DigitalOut(D11, 0);
monikakakani 1:43568a9d4323 115
monikakakani 1:43568a9d4323 116
monikakakani 1:43568a9d4323 117 }
monikakakani 1:43568a9d4323 118
monikakakani 1:43568a9d4323 119
monikakakani 1:43568a9d4323 120 void left()
monikakakani 1:43568a9d4323 121
monikakakani 1:43568a9d4323 122
monikakakani 1:43568a9d4323 123 {
monikakakani 1:43568a9d4323 124
monikakakani 1:43568a9d4323 125 DigitalOut(D3,1);
monikakakani 1:43568a9d4323 126
monikakakani 1:43568a9d4323 127 DigitalOut(D5, 0);
monikakakani 1:43568a9d4323 128
monikakakani 1:43568a9d4323 129 DigitalOut(D10, 0);
monikakakani 1:43568a9d4323 130
monikakakani 1:43568a9d4323 131 DigitalOut(D11, 1);
monikakakani 1:43568a9d4323 132
monikakakani 1:43568a9d4323 133
monikakakani 1:43568a9d4323 134 }
monikakakani 1:43568a9d4323 135
monikakakani 1:43568a9d4323 136
monikakakani 1:43568a9d4323 137
monikakakani 1:43568a9d4323 138 void stop()
monikakakani 1:43568a9d4323 139
monikakakani 1:43568a9d4323 140
monikakakani 1:43568a9d4323 141 {
kit3 0:a87600bebf7b 142
kit3 0:a87600bebf7b 143
monikakakani 1:43568a9d4323 144 DigitalOut(D3,0);
kit3 0:a87600bebf7b 145
monikakakani 1:43568a9d4323 146 DigitalOut(D5, 0);
kit3 0:a87600bebf7b 147
monikakakani 1:43568a9d4323 148 DigitalOut(D10,0);
monikakakani 1:43568a9d4323 149
monikakakani 1:43568a9d4323 150 DigitalOut(D11, 0);
monikakakani 1:43568a9d4323 151
monikakakani 1:43568a9d4323 152
kit3 0:a87600bebf7b 153 }
monikakakani 1:43568a9d4323 154
monikakakani 1:43568a9d4323 155 /*
monikakakani 1:43568a9d4323 156 void loop()
monikakakani 1:43568a9d4323 157
monikakakani 1:43568a9d4323 158 {
kit3 0:a87600bebf7b 159
monikakakani 1:43568a9d4323 160 servo.period(0.020);
monikakakani 1:43568a9d4323 161 usensor.start();
monikakakani 1:43568a9d4323 162 wait_ms(500);
monikakakani 1:43568a9d4323 163 distance = usensor.get_dist_cm(); //Tip: Use 'CM' for centimeters or 'INC' for inches
monikakakani 1:43568a9d4323 164 pc.printf("Dist: %d ",distance);
kit3 0:a87600bebf7b 165
monikakakani 1:43568a9d4323 166 //Check for objects...
monikakakani 1:43568a9d4323 167 if (distance > 50){
monikakakani 1:43568a9d4323 168 pc.printf(" No Object \n");
monikakakani 1:43568a9d4323 169 forward(); //All clear, move forward!
monikakakani 1:43568a9d4323 170 //noTone(buzzer);
monikakakani 1:43568a9d4323 171 // digitalOut LED(0);
monikakakani 1:43568a9d4323 172 }
monikakakani 1:43568a9d4323 173 else if (distance <=50){
monikakakani 1:43568a9d4323 174 pc.printf("Halt - Object on the Way \n");
monikakakani 1:43568a9d4323 175 stop();
monikakakani 1:43568a9d4323 176
monikakakani 1:43568a9d4323 177
monikakakani 1:43568a9d4323 178 for(float offset=0.0016; offset<=0.0020; offset+=0.0001) // turning RIGHT
monikakakani 1:43568a9d4323 179
monikakakani 1:43568a9d4323 180 {
monikakakani 1:43568a9d4323 181
monikakakani 1:43568a9d4323 182 pc.printf("Turning Right");
monikakakani 1:43568a9d4323 183
monikakakani 1:43568a9d4323 184 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
monikakakani 1:43568a9d4323 185
monikakakani 1:43568a9d4323 186 wait_ms(0.10);
monikakakani 1:43568a9d4323 187
monikakakani 1:43568a9d4323 188 }
monikakakani 1:43568a9d4323 189
monikakakani 1:43568a9d4323 190 wait_ms(200); // stopping at RIGHTMOST position for 2 seconds
monikakakani 1:43568a9d4323 191
monikakakani 1:43568a9d4323 192
monikakakani 1:43568a9d4323 193 usensor.start();
monikakakani 1:43568a9d4323 194
monikakakani 1:43568a9d4323 195 wait_ms(500);
monikakakani 1:43568a9d4323 196
monikakakani 1:43568a9d4323 197 checkRight = usensor.get_dist_cm(); //Take distance from the left side
monikakakani 1:43568a9d4323 198
monikakakani 1:43568a9d4323 199 pc.printf("DistL: %d ",checkRight);
monikakakani 1:43568a9d4323 200
monikakakani 1:43568a9d4323 201
monikakakani 1:43568a9d4323 202 for (float offset=0.0020; offset>=0.0016;) // turning back to theCENTER position
monikakakani 1:43568a9d4323 203 {
monikakakani 1:43568a9d4323 204 pc.printf("Turning Center");
monikakakani 1:43568a9d4323 205
monikakakani 1:43568a9d4323 206 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
monikakakani 1:43568a9d4323 207 offset=offset-0.0001;
monikakakani 1:43568a9d4323 208
monikakakani 1:43568a9d4323 209 wait_ms(0.10);
monikakakani 1:43568a9d4323 210
monikakakani 1:43568a9d4323 211 }
monikakakani 1:43568a9d4323 212
monikakakani 1:43568a9d4323 213 wait_ms(200);
monikakakani 1:43568a9d4323 214
monikakakani 1:43568a9d4323 215
monikakakani 1:43568a9d4323 216 for (float offset=0.0016; offset>=0.0012; offset=offset-0.0001) // Turning towards LEFT
monikakakani 1:43568a9d4323 217
monikakakani 1:43568a9d4323 218 {
monikakakani 1:43568a9d4323 219
monikakakani 1:43568a9d4323 220 pc.printf("Turning Left");
monikakakani 1:43568a9d4323 221 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
monikakakani 1:43568a9d4323 222
monikakakani 1:43568a9d4323 223 wait_ms(0.10);
monikakakani 1:43568a9d4323 224
monikakakani 1:43568a9d4323 225 }
monikakakani 1:43568a9d4323 226
monikakakani 1:43568a9d4323 227 wait_ms(200); // stopping at LEFTMOST position for 2
monikakakani 1:43568a9d4323 228
monikakakani 1:43568a9d4323 229
monikakakani 1:43568a9d4323 230 usensor.start();
monikakakani 1:43568a9d4323 231
monikakakani 1:43568a9d4323 232 wait_ms(500);
monikakakani 1:43568a9d4323 233
monikakakani 1:43568a9d4323 234 checkLeft= usensor.get_dist_cm();
monikakakani 1:43568a9d4323 235
monikakakani 1:43568a9d4323 236 pc.printf("DistR: %d ",checkLeft);
monikakakani 1:43568a9d4323 237
monikakakani 1:43568a9d4323 238
monikakakani 1:43568a9d4323 239 for(float offset=0.0012; offset<=0.0016; offset=offset+0.0001)
monikakakani 1:43568a9d4323 240
monikakakani 1:43568a9d4323 241 {
monikakakani 1:43568a9d4323 242 pc.printf("Turning Center");
monikakakani 1:43568a9d4323 243
monikakakani 1:43568a9d4323 244 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
monikakakani 1:43568a9d4323 245
monikakakani 1:43568a9d4323 246 wait_ms(0.10);
monikakakani 1:43568a9d4323 247
monikakakani 1:43568a9d4323 248 }
monikakakani 1:43568a9d4323 249
monikakakani 1:43568a9d4323 250
monikakakani 1:43568a9d4323 251 //Finally, take the right decision, turn left or right?
monikakakani 1:43568a9d4323 252
monikakakani 1:43568a9d4323 253 if (checkLeft < checkRight){
monikakakani 1:43568a9d4323 254
monikakakani 1:43568a9d4323 255 backward();
monikakakani 1:43568a9d4323 256 wait_ms(1000);
monikakakani 1:43568a9d4323 257
monikakakani 1:43568a9d4323 258
monikakakani 1:43568a9d4323 259 left();
monikakakani 1:43568a9d4323 260 pc.printf("goingleft");
monikakakani 1:43568a9d4323 261 wait_ms(400);
monikakakani 1:43568a9d4323 262
monikakakani 1:43568a9d4323 263 // wait_ms, change value if necessary to make robot turn.
monikakakani 1:43568a9d4323 264 forward();
monikakakani 1:43568a9d4323 265 pc.printf("now going straight");
monikakakani 1:43568a9d4323 266 }
monikakakani 1:43568a9d4323 267
monikakakani 1:43568a9d4323 268 else if (checkLeft > checkRight){
monikakakani 1:43568a9d4323 269
monikakakani 1:43568a9d4323 270 backward();
monikakakani 1:43568a9d4323 271 wait_ms(1000);
monikakakani 1:43568a9d4323 272
monikakakani 1:43568a9d4323 273 right();
kit3 0:a87600bebf7b 274
kit3 0:a87600bebf7b 275
monikakakani 1:43568a9d4323 276
monikakakani 1:43568a9d4323 277 wait_ms(400);
monikakakani 1:43568a9d4323 278 forward();
monikakakani 1:43568a9d4323 279 // wait_ms, change value if necessary to make robot turn.
monikakakani 1:43568a9d4323 280
kit3 0:a87600bebf7b 281 }
kit3 0:a87600bebf7b 282
monikakakani 1:43568a9d4323 283 else if (checkLeft <=10 && checkRight <=10){
kit3 0:a87600bebf7b 284
monikakakani 1:43568a9d4323 285 backward();
monikakakani 1:43568a9d4323 286 wait_ms(1000); //The road is closed... go back and then left ;)
kit3 0:a87600bebf7b 287
monikakakani 1:43568a9d4323 288 left();
monikakakani 1:43568a9d4323 289 wait_ms(400);
monikakakani 1:43568a9d4323 290 forward();
monikakakani 1:43568a9d4323 291
monikakakani 1:43568a9d4323 292 }
monikakakani 1:43568a9d4323 293
monikakakani 1:43568a9d4323 294 }
monikakakani 1:43568a9d4323 295 */
monikakakani 1:43568a9d4323 296