Tanu Tanu / Mbed 2 deprecated Obstacle_Avoiding_Robot

Obstacle detection and avoidance Robot

Dependencies:   HCSR-04 Servo mbed

Fork of Obstacle_avoidance by ece nith

main.cpp@1:26d645dc47f8, 2016-03-27 (annotated)

Committer:
goeltanu
Date:
Sun Mar 27 16:25:03 2016 +0000
Revision:
1:26d645dc47f8
Parent:
0:a87600bebf7b 
Obstacle detection and Avoidance Robot

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"
goeltanu 1:26d645dc47f8 4 #define HIGH 1
goeltanu 1:26d645dc47f8 5 #define LOW 0
goeltanu 1:26d645dc47f8 6 void forward();
goeltanu 1:26d645dc47f8 7 void backward();
goeltanu 1:26d645dc47f8 8 void right();
goeltanu 1:26d645dc47f8 9 void left();
goeltanu 1:26d645dc47f8 10 void stop();
goeltanu 1:26d645dc47f8 11 void loop();
goeltanu 1:26d645dc47f8 12
goeltanu 1:26d645dc47f8 13 int distance; //Variable to store distance from an object
goeltanu 1:26d645dc47f8 14 int checkRight;
goeltanu 1:26d645dc47f8 15 int checkLeft;
goeltanu 1:26d645dc47f8 16 int function=1; //Variable to store function of robot: '1' running or '0' stoped. By
goeltanu 1:26d645dc47f8 17 int pos=90;
goeltanu 1:26d645dc47f8 18
goeltanu 1:26d645dc47f8 19
goeltanu 1:26d645dc47f8 20 Serial pc(USBTX,USBRX);
goeltanu 1:26d645dc47f8 21 PwmOut servo(D6);
goeltanu 1:26d645dc47f8 22
goeltanu 1:26d645dc47f8 23
goeltanu 1:26d645dc47f8 24
goeltanu 1:26d645dc47f8 25 HCSR04 usensor(A4,A5);
goeltanu 1:26d645dc47f8 26
goeltanu 1:26d645dc47f8 27 //Variables
goeltanu 1:26d645dc47f8 28
goeltanu 1:26d645dc47f8 29 int main()
goeltanu 1:26d645dc47f8 30 {
goeltanu 1:26d645dc47f8 31
goeltanu 1:26d645dc47f8 32 while(1)
goeltanu 1:26d645dc47f8 33 {
goeltanu 1:26d645dc47f8 34 loop();
goeltanu 1:26d645dc47f8 35 }
goeltanu 1:26d645dc47f8 36
goeltanu 1:26d645dc47f8 37 }
goeltanu 1:26d645dc47f8 38
goeltanu 1:26d645dc47f8 39
goeltanu 1:26d645dc47f8 40
goeltanu 1:26d645dc47f8 41 void forward(){
goeltanu 1:26d645dc47f8 42
goeltanu 1:26d645dc47f8 43 pc.printf("fwd");
goeltanu 1:26d645dc47f8 44 DigitalOut(D13,0);
goeltanu 1:26d645dc47f8 45 DigitalOut(D12, 1);
goeltanu 1:26d645dc47f8 46 DigitalOut(D9, 1);
goeltanu 1:26d645dc47f8 47 DigitalOut(D8, 0);
goeltanu 1:26d645dc47f8 48 }
goeltanu 1:26d645dc47f8 49
goeltanu 1:26d645dc47f8 50
goeltanu 1:26d645dc47f8 51
goeltanu 1:26d645dc47f8 52 void backward(){
goeltanu 1:26d645dc47f8 53 DigitalOut(D13,1);
goeltanu 1:26d645dc47f8 54 DigitalOut(D12, 0);
goeltanu 1:26d645dc47f8 55 DigitalOut(D9, 0);
goeltanu 1:26d645dc47f8 56 DigitalOut(D8, 1);
goeltanu 1:26d645dc47f8 57 }
goeltanu 1:26d645dc47f8 58
goeltanu 1:26d645dc47f8 59 void right()
goeltanu 1:26d645dc47f8 60
goeltanu 1:26d645dc47f8 61 {
goeltanu 1:26d645dc47f8 62
goeltanu 1:26d645dc47f8 63 DigitalOut(D13,0);
goeltanu 1:26d645dc47f8 64 DigitalOut(D12, 1);
goeltanu 1:26d645dc47f8 65 DigitalOut(D9, 0);
goeltanu 1:26d645dc47f8 66 DigitalOut(D8, 1);
goeltanu 1:26d645dc47f8 67
goeltanu 1:26d645dc47f8 68
goeltanu 1:26d645dc47f8 69 }
goeltanu 1:26d645dc47f8 70
goeltanu 1:26d645dc47f8 71
goeltanu 1:26d645dc47f8 72
goeltanu 1:26d645dc47f8 73 void left()
goeltanu 1:26d645dc47f8 74
goeltanu 1:26d645dc47f8 75 {
goeltanu 1:26d645dc47f8 76
goeltanu 1:26d645dc47f8 77 DigitalOut(D13,1);
goeltanu 1:26d645dc47f8 78 DigitalOut(D12, 0);
goeltanu 1:26d645dc47f8 79 DigitalOut(D9, 1);
goeltanu 1:26d645dc47f8 80 DigitalOut(D8, 0);
goeltanu 1:26d645dc47f8 81
goeltanu 1:26d645dc47f8 82
goeltanu 1:26d645dc47f8 83 }
kit3 0:a87600bebf7b 84
kit3 0:a87600bebf7b 85
goeltanu 1:26d645dc47f8 86 void stop()
kit3 0:a87600bebf7b 87
goeltanu 1:26d645dc47f8 88 {
goeltanu 1:26d645dc47f8 89
goeltanu 1:26d645dc47f8 90 DigitalOut(D13,0);
goeltanu 1:26d645dc47f8 91 DigitalOut(D12, 0);
goeltanu 1:26d645dc47f8 92 DigitalOut(D9, 0);
goeltanu 1:26d645dc47f8 93 DigitalOut(D8, 0);
goeltanu 1:26d645dc47f8 94
goeltanu 1:26d645dc47f8 95
goeltanu 1:26d645dc47f8 96 }
goeltanu 1:26d645dc47f8 97
goeltanu 1:26d645dc47f8 98
goeltanu 1:26d645dc47f8 99 void loop()
goeltanu 1:26d645dc47f8 100
goeltanu 1:26d645dc47f8 101 {
goeltanu 1:26d645dc47f8 102
goeltanu 1:26d645dc47f8 103 servo.period(0.020);
goeltanu 1:26d645dc47f8 104 usensor.start();
goeltanu 1:26d645dc47f8 105 wait_ms(500);
goeltanu 1:26d645dc47f8 106 distance = usensor.get_dist_cm(); //Tip: Use 'CM' for centimeters or 'INC' for inches
goeltanu 1:26d645dc47f8 107 pc.printf("Dist: %d ",distance);
goeltanu 1:26d645dc47f8 108 //Check for objects...
goeltanu 1:26d645dc47f8 109 if (distance > 10){
goeltanu 1:26d645dc47f8 110 pc.printf(" No Object \n");
goeltanu 1:26d645dc47f8 111 forward(); //All clear, move forward!
goeltanu 1:26d645dc47f8 112 //noTone(buzzer);
goeltanu 1:26d645dc47f8 113 // digitalOut LED(0);
goeltanu 1:26d645dc47f8 114 }
goeltanu 1:26d645dc47f8 115 else if (distance <=10){
goeltanu 1:26d645dc47f8 116 pc.printf("Halt - Object on the Way \n");
goeltanu 1:26d645dc47f8 117 stop();
goeltanu 1:26d645dc47f8 118
goeltanu 1:26d645dc47f8 119
goeltanu 1:26d645dc47f8 120 for(float offset=0.0016; offset<=0.0027; offset+=0.0001) // turning RIGHT
goeltanu 1:26d645dc47f8 121
goeltanu 1:26d645dc47f8 122 {
kit3 0:a87600bebf7b 123
goeltanu 1:26d645dc47f8 124 pc.printf("Turning Right");
goeltanu 1:26d645dc47f8 125
goeltanu 1:26d645dc47f8 126 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
goeltanu 1:26d645dc47f8 127
goeltanu 1:26d645dc47f8 128 wait(0.10);
goeltanu 1:26d645dc47f8 129
goeltanu 1:26d645dc47f8 130 }
goeltanu 1:26d645dc47f8 131
goeltanu 1:26d645dc47f8 132 wait (2); // stopping at RIGHTMOST position for 2 seconds
goeltanu 1:26d645dc47f8 133
goeltanu 1:26d645dc47f8 134
goeltanu 1:26d645dc47f8 135
goeltanu 1:26d645dc47f8 136
goeltanu 1:26d645dc47f8 137 usensor.start();
goeltanu 1:26d645dc47f8 138
goeltanu 1:26d645dc47f8 139 wait_ms(500);
goeltanu 1:26d645dc47f8 140
goeltanu 1:26d645dc47f8 141 checkRight = usensor.get_dist_cm(); //Take distance from the left side
goeltanu 1:26d645dc47f8 142
goeltanu 1:26d645dc47f8 143 pc.printf("DistL: %d ",checkLeft);
goeltanu 1:26d645dc47f8 144
goeltanu 1:26d645dc47f8 145
goeltanu 1:26d645dc47f8 146 for (float offset=0.0027; offset>=0.0016;) // turning back to theCENTER position
goeltanu 1:26d645dc47f8 147 {
goeltanu 1:26d645dc47f8 148 pc.printf("Turning Center");
goeltanu 1:26d645dc47f8 149
goeltanu 1:26d645dc47f8 150 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
goeltanu 1:26d645dc47f8 151
goeltanu 1:26d645dc47f8 152 wait(0.10);
goeltanu 1:26d645dc47f8 153 offset=offset-0.0001;
goeltanu 1:26d645dc47f8 154 }
goeltanu 1:26d645dc47f8 155
goeltanu 1:26d645dc47f8 156 wait(2);
goeltanu 1:26d645dc47f8 157
goeltanu 1:26d645dc47f8 158
goeltanu 1:26d645dc47f8 159 for (float offset=0.0016; offset>=0.0009; offset=offset-0.0001) // Turning towards LEFT
goeltanu 1:26d645dc47f8 160
goeltanu 1:26d645dc47f8 161 {
goeltanu 1:26d645dc47f8 162
goeltanu 1:26d645dc47f8 163 pc.printf("Turning Left");
goeltanu 1:26d645dc47f8 164 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
goeltanu 1:26d645dc47f8 165
goeltanu 1:26d645dc47f8 166 wait(0.10);
goeltanu 1:26d645dc47f8 167
goeltanu 1:26d645dc47f8 168 }
goeltanu 1:26d645dc47f8 169
goeltanu 1:26d645dc47f8 170 wait(2); // stopping at LEFTMOST position for 2
goeltanu 1:26d645dc47f8 171
goeltanu 1:26d645dc47f8 172
goeltanu 1:26d645dc47f8 173 usensor.start();
goeltanu 1:26d645dc47f8 174
goeltanu 1:26d645dc47f8 175 wait_ms(500);
goeltanu 1:26d645dc47f8 176
goeltanu 1:26d645dc47f8 177 checkLeft= usensor.get_dist_cm();
goeltanu 1:26d645dc47f8 178
goeltanu 1:26d645dc47f8 179 pc.printf("DistR: %d ",checkRight);
goeltanu 1:26d645dc47f8 180
goeltanu 1:26d645dc47f8 181
goeltanu 1:26d645dc47f8 182 for(float offset=0.0009; offset<=0.0016; offset=offset+0.0001)
goeltanu 1:26d645dc47f8 183
goeltanu 1:26d645dc47f8 184 {
goeltanu 1:26d645dc47f8 185 pc.printf("Turning Center");
goeltanu 1:26d645dc47f8 186
goeltanu 1:26d645dc47f8 187 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
goeltanu 1:26d645dc47f8 188
goeltanu 1:26d645dc47f8 189 wait(0.10);
goeltanu 1:26d645dc47f8 190
goeltanu 1:26d645dc47f8 191 }
goeltanu 1:26d645dc47f8 192
goeltanu 1:26d645dc47f8 193
goeltanu 1:26d645dc47f8 194 //Finally, take the right decision, turn left or right?
goeltanu 1:26d645dc47f8 195
goeltanu 1:26d645dc47f8 196 if (checkLeft < checkRight){
goeltanu 1:26d645dc47f8 197
goeltanu 1:26d645dc47f8 198 left();
goeltanu 1:26d645dc47f8 199
goeltanu 1:26d645dc47f8 200 wait_ms(400);
goeltanu 1:26d645dc47f8 201
goeltanu 1:26d645dc47f8 202 // wait_ms, change value if necessary to make robot turn.
goeltanu 1:26d645dc47f8 203 forward();
goeltanu 1:26d645dc47f8 204 }
goeltanu 1:26d645dc47f8 205
goeltanu 1:26d645dc47f8 206 else if (checkLeft > checkRight){
goeltanu 1:26d645dc47f8 207
goeltanu 1:26d645dc47f8 208 right();
goeltanu 1:26d645dc47f8 209
goeltanu 1:26d645dc47f8 210 wait_ms(400);
goeltanu 1:26d645dc47f8 211 forward();
goeltanu 1:26d645dc47f8 212 // wait_ms, change value if necessary to make robot turn.
goeltanu 1:26d645dc47f8 213
goeltanu 1:26d645dc47f8 214 }
goeltanu 1:26d645dc47f8 215
goeltanu 1:26d645dc47f8 216 else if (checkLeft <=10 && checkRight <=10){
goeltanu 1:26d645dc47f8 217
goeltanu 1:26d645dc47f8 218 backward(); //The road is closed... go back and then left ;)
goeltanu 1:26d645dc47f8 219
goeltanu 1:26d645dc47f8 220 left();
goeltanu 1:26d645dc47f8 221 forward();
goeltanu 1:26d645dc47f8 222
goeltanu 1:26d645dc47f8 223 }
goeltanu 1:26d645dc47f8 224
goeltanu 1:26d645dc47f8 225 }
goeltanu 1:26d645dc47f8 226
kit3 0:a87600bebf7b 227
goeltanu 1:26d645dc47f8 228
goeltanu 1:26d645dc47f8 229 }
goeltanu 1:26d645dc47f8 230
goeltanu 1:26d645dc47f8 231
goeltanu 1:26d645dc47f8 232
kit3 0:a87600bebf7b 233
kit3 0:a87600bebf7b 234
goeltanu 1:26d645dc47f8 235
goeltanu 1:26d645dc47f8 236
goeltanu 1:26d645dc47f8 237
goeltanu 1:26d645dc47f8 238
goeltanu 1:26d645dc47f8 239
goeltanu 1:26d645dc47f8 240
kit3 0:a87600bebf7b 241
goeltanu 1:26d645dc47f8 242
goeltanu 1:26d645dc47f8 243