ece nith / Mbed 2 deprecated Obstacle_avoidance

OBSTACLE AVOIDING ROBOT USING ULTRASONIC SENSOR

Dependencies:   HCSR04 Motordriver Servo TextLCD mbed

main.cpp@0:a87600bebf7b, 2013-05-18 (annotated)

Committer:
kit3
Date:
Sat May 18 04:39:55 2013 +0000
Revision:
0:a87600bebf7b
OBSTACLE AVOIDING ROBOT USING ULTRASONIC SENSOR

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kit3 0:a87600bebf7b 1 /* Megha Sharma, Rishabh Sehgal, Ragheshwar Singh Ranawat ,E.C.E.D NIT Hamirpur
kit3 0:a87600bebf7b 2
kit3 0:a87600bebf7b 3 OBSTACLE AVOIDING ROBOT USING ULTRASONIC SENSOR HCSR04
kit3 0:a87600bebf7b 4
kit3 0:a87600bebf7b 5 In this project LPC 1768 embed board along with NGX Expresso baseboard are successfuly used to make an obstacle avoiding/
kit3 0:a87600bebf7b 6 self navigating robot which makes use of Ultrasonic Sensor HCSR04 for obstacle detection and Ranging. The maximum range of
kit3 0:a87600bebf7b 7 Ultrasonic sensor is about 3-4 meters. A program called "ping" sends a serial trigger pulse of 10 nS to the Trigger pin of
kit3 0:a87600bebf7b 8 HCSR04 and on receiving the trigger HCSR04 beams out an 8 cycle Sonic burst. Then Echo pulse is received by the Ultrasonic sensor
kit3 0:a87600bebf7b 9 upto 25ms. On receiving the Echo pulse distance is calculated. The HCSR04 is mounted Axially on a servo motor. Initially the HCSR04
kit3 0:a87600bebf7b 10 is kept in a straight direction. Motors make bot move in forward direction and continuosly calculating the distance of the
kit3 0:a87600bebf7b 11 nearest obstacle in the forward direction. After seeing a distance at less than a "Constraint Value" (i.e. 10 cm here),
kit3 0:a87600bebf7b 12 the robot goes in a scan mode. It first moves the Servo Motor to the Right, waits for 2 seconds and stores the value of
kit3 0:a87600bebf7b 13 Distance in right direction in "rightscanval" variable.Then it comes back to centre position. Now it moves towards the
kit3 0:a87600bebf7b 14 Left direction, waits for 2 seconds and stores the value of Distance in "leftscanval" variable. Then it comes back to the
kit3 0:a87600bebf7b 15 centre position. Now it does the comparison of the two variables to determine in which direction it should move. It
kit3 0:a87600bebf7b 16 successfully turns towards the direction with the farthest distance of obstacle.
kit3 0:a87600bebf7b 17
kit3 0:a87600bebf7b 18 Again the Servo motor is in forward direction keeping the sensor straight and the Robot also moves towards Forward direction all
kit3 0:a87600bebf7b 19 the while calculating the distance of obstacle in forward direction. The loop continues.....
kit3 0:a87600bebf7b 20
kit3 0:a87600bebf7b 21 */
kit3 0:a87600bebf7b 22 #include "mbed.h"
kit3 0:a87600bebf7b 23 #include "hcsr04.h"
kit3 0:a87600bebf7b 24 #include "motordriver.h"
kit3 0:a87600bebf7b 25 #include "TextLCD.h"
kit3 0:a87600bebf7b 26 #include "Servo.h"
kit3 0:a87600bebf7b 27 int ping ();
kit3 0:a87600bebf7b 28 void scan();
kit3 0:a87600bebf7b 29 void right (void);
kit3 0:a87600bebf7b 30 void stop (void );
kit3 0:a87600bebf7b 31 void straight (void);
kit3 0:a87600bebf7b 32 void left (void);
kit3 0:a87600bebf7b 33 Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can brake
kit3 0:a87600bebf7b 34 Motor B(p21, p7, p8, 1);
kit3 0:a87600bebf7b 35 HCSR04 usensor(p13,p14);
kit3 0:a87600bebf7b 36 TextLCD lcd(p24, p26, p27, p28, p29, p30); // rs, e, d4-d7
kit3 0:a87600bebf7b 37 PwmOut servo(p23); // Servo pin 21
kit3 0:a87600bebf7b 38
kit3 0:a87600bebf7b 39
kit3 0:a87600bebf7b 40 unsigned int dist;
kit3 0:a87600bebf7b 41 DigitalOut myled(LED1);
kit3 0:a87600bebf7b 42 DigitalOut myled1(LED2);
kit3 0:a87600bebf7b 43 DigitalOut myled2(LED3);
kit3 0:a87600bebf7b 44 int count = 0;
kit3 0:a87600bebf7b 45
kit3 0:a87600bebf7b 46 int main ()
kit3 0:a87600bebf7b 47 {
kit3 0:a87600bebf7b 48
kit3 0:a87600bebf7b 49 while (1) // continuos loop to perform the scanning function of HCSR04 ..
kit3 0:a87600bebf7b 50 //a trigger pulse of Ultrasonic waves having duration 10 nS is sent through the echo pin
kit3 0:a87600bebf7b 51 {
kit3 0:a87600bebf7b 52
kit3 0:a87600bebf7b 53 servo.period(0.020);
kit3 0:a87600bebf7b 54 int distance = ping(); // ping function
kit3 0:a87600bebf7b 55 straight(); // use the ping() function to see if anything is ahead.
kit3 0:a87600bebf7b 56
kit3 0:a87600bebf7b 57
kit3 0:a87600bebf7b 58 if (distance < 10) // the condition for stopping the Obstacle Avoider...
kit3 0:a87600bebf7b 59 // ...whenever the defined proximity to obstacle is reached.
kit3 0:a87600bebf7b 60 {
kit3 0:a87600bebf7b 61 stop();
kit3 0:a87600bebf7b 62 scan();
kit3 0:a87600bebf7b 63
kit3 0:a87600bebf7b 64
kit3 0:a87600bebf7b 65 /*switch (a) // switch case for initial testing of the Obstacle detection by sensor and
kit3 0:a87600bebf7b 66 // Corresponding L.E.D. of the best direction will glow.
kit3 0:a87600bebf7b 67
kit3 0:a87600bebf7b 68 {
kit3 0:a87600bebf7b 69 case 'r':
kit3 0:a87600bebf7b 70 myled=1;
kit3 0:a87600bebf7b 71 break;
kit3 0:a87600bebf7b 72 case 'l':
kit3 0:a87600bebf7b 73 myled1=1;
kit3 0:a87600bebf7b 74 break;
kit3 0:a87600bebf7b 75 case 'f':
kit3 0:a87600bebf7b 76 myled2=1;
kit3 0:a87600bebf7b 77 break;
kit3 0:a87600bebf7b 78 }*/
kit3 0:a87600bebf7b 79
kit3 0:a87600bebf7b 80 }
kit3 0:a87600bebf7b 81 }
kit3 0:a87600bebf7b 82 }
kit3 0:a87600bebf7b 83
kit3 0:a87600bebf7b 84 // A functon to perform the continuos pinging function of HCSR04 ..
kit3 0:a87600bebf7b 85 //a trigger pulse of Ultrasonic waves having duration 10 nS is sent through the echo pin
kit3 0:a87600bebf7b 86
kit3 0:a87600bebf7b 87 int ping() // ultrasonic sensor is started and data collected is displayed on the LCD
kit3 0:a87600bebf7b 88 // ping function returns an int value of distance scanned
kit3 0:a87600bebf7b 89 {
kit3 0:a87600bebf7b 90 usensor.start();
kit3 0:a87600bebf7b 91 wait_ms(200);
kit3 0:a87600bebf7b 92 dist=usensor.get_dist_cm();
kit3 0:a87600bebf7b 93 lcd.cls();
kit3 0:a87600bebf7b 94 lcd.locate(0,0);
kit3 0:a87600bebf7b 95 if (dist <50)
kit3 0:a87600bebf7b 96 lcd.printf("cm:%ld",dist );
kit3 0:a87600bebf7b 97 else
kit3 0:a87600bebf7b 98 lcd.printf("no object");
kit3 0:a87600bebf7b 99 return (dist);
kit3 0:a87600bebf7b 100
kit3 0:a87600bebf7b 101 }
kit3 0:a87600bebf7b 102
kit3 0:a87600bebf7b 103
kit3 0:a87600bebf7b 104 void scan ()
kit3 0:a87600bebf7b 105 {
kit3 0:a87600bebf7b 106
kit3 0:a87600bebf7b 107 int leftscanval, rightscanval; // variables for storing distance scan values...
kit3 0:a87600bebf7b 108 //... of nearest obstacle in the left and right direction
kit3 0:a87600bebf7b 109
kit3 0:a87600bebf7b 110
kit3 0:a87600bebf7b 111 // Scanning RIGHT side; turning the servo attached to HCSR04 right to check for obstacle distance
kit3 0:a87600bebf7b 112
kit3 0:a87600bebf7b 113 // 0.0016 corresponds to the neutral position of servo motor
kit3 0:a87600bebf7b 114 // 0.0027 corresponds to the rightmost position of the servo motor
kit3 0:a87600bebf7b 115 // 0.0009 corresponds to the leftmost position of the servomotor
kit3 0:a87600bebf7b 116
kit3 0:a87600bebf7b 117 for(float offset=0.0016; offset<=0.0027; offset+=0.0001) // turning RIGHT
kit3 0:a87600bebf7b 118
kit3 0:a87600bebf7b 119 {
kit3 0:a87600bebf7b 120 servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
kit3 0:a87600bebf7b 121 wait(0.10);
kit3 0:a87600bebf7b 122 }
kit3 0:a87600bebf7b 123 wait (2); // stopping at RIGHTMOST position for 2 seconds
kit3 0:a87600bebf7b 124 rightscanval = ping(); // putting the distance of right direction's obstacle in ''rightscanval variable''
kit3 0:a87600bebf7b 125
kit3 0:a87600bebf7b 126
kit3 0:a87600bebf7b 127
kit3 0:a87600bebf7b 128 for(float offset=0.0027; offset>=0.0016; offset+=-0.0001) // turning back to the CENTER position
kit3 0:a87600bebf7b 129
kit3 0:a87600bebf7b 130 {
kit3 0:a87600bebf7b 131 servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
kit3 0:a87600bebf7b 132 wait(0.10);
kit3 0:a87600bebf7b 133 }
kit3 0:a87600bebf7b 134 wait(2); // stopping at CENTERMOST position for 2 seconds
kit3 0:a87600bebf7b 135
kit3 0:a87600bebf7b 136
kit3 0:a87600bebf7b 137 // Scanning LEFT side; turning the servo attached to HCSR04 right to check for obstacle distance
kit3 0:a87600bebf7b 138
kit3 0:a87600bebf7b 139
kit3 0:a87600bebf7b 140 for(float offset=0.0016; offset>=0.0009; offset-=0.0001) // Turning towards LEFT
kit3 0:a87600bebf7b 141
kit3 0:a87600bebf7b 142 {
kit3 0:a87600bebf7b 143 servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
kit3 0:a87600bebf7b 144 wait(0.10);
kit3 0:a87600bebf7b 145 }
kit3 0:a87600bebf7b 146 wait(2); // stopping at LEFTMOST position for 2 seconds
kit3 0:a87600bebf7b 147 leftscanval = ping(); // putting the distance of right direction's obstacle in ''leftscanval variable''
kit3 0:a87600bebf7b 148
kit3 0:a87600bebf7b 149
kit3 0:a87600bebf7b 150 for(float offset=0.0009; offset<=0.0016; offset+=0.0001)
kit3 0:a87600bebf7b 151
kit3 0:a87600bebf7b 152 {
kit3 0:a87600bebf7b 153 servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
kit3 0:a87600bebf7b 154 wait(0.10);
kit3 0:a87600bebf7b 155 }
kit3 0:a87600bebf7b 156 /*center scan servo (for changing the default position of Servo to the Centre Position ) This method didnt work
kit3 0:a87600bebf7b 157 though. There is a library of Servo
kit3 0:a87600bebf7b 158 */
kit3 0:a87600bebf7b 159 //servo.pulsewidth(0.00017);
kit3 0:a87600bebf7b 160
kit3 0:a87600bebf7b 161
kit3 0:a87600bebf7b 162 /* Comparisons to choose the most appropiate direction for the turning of Robot. Based on the distances stored in variables "leftscanvalue" and "rightscanvalue" */
kit3 0:a87600bebf7b 163
kit3 0:a87600bebf7b 164 if (leftscanval>rightscanval )
kit3 0:a87600bebf7b 165
kit3 0:a87600bebf7b 166 {
kit3 0:a87600bebf7b 167 left();
kit3 0:a87600bebf7b 168 }
kit3 0:a87600bebf7b 169
kit3 0:a87600bebf7b 170 else if (rightscanval>leftscanval )
kit3 0:a87600bebf7b 171
kit3 0:a87600bebf7b 172 {
kit3 0:a87600bebf7b 173 right();
kit3 0:a87600bebf7b 174 }
kit3 0:a87600bebf7b 175
kit3 0:a87600bebf7b 176 else
kit3 0:a87600bebf7b 177 {
kit3 0:a87600bebf7b 178 left();
kit3 0:a87600bebf7b 179 }
kit3 0:a87600bebf7b 180
kit3 0:a87600bebf7b 181 //return choice;
kit3 0:a87600bebf7b 182 }
kit3 0:a87600bebf7b 183
kit3 0:a87600bebf7b 184 void stop () // function for stopping the DC motors
kit3 0:a87600bebf7b 185
kit3 0:a87600bebf7b 186 {
kit3 0:a87600bebf7b 187 A.speed(0);
kit3 0:a87600bebf7b 188 B.speed(0);
kit3 0:a87600bebf7b 189 }
kit3 0:a87600bebf7b 190
kit3 0:a87600bebf7b 191
kit3 0:a87600bebf7b 192 void left() // function for moving the RIGHT motor in FORWARD direction and LEFT motor in REVERSE direction so...
kit3 0:a87600bebf7b 193 // ..that the robot turns to the LEFT
kit3 0:a87600bebf7b 194
kit3 0:a87600bebf7b 195 {
kit3 0:a87600bebf7b 196 for (float s= -1.0; s < 0.0 ; s += 0.01)
kit3 0:a87600bebf7b 197 {
kit3 0:a87600bebf7b 198 A.speed(-1);
kit3 0:a87600bebf7b 199 B.speed(-1);
kit3 0:a87600bebf7b 200 wait(0.02);
kit3 0:a87600bebf7b 201 }
kit3 0:a87600bebf7b 202 }
kit3 0:a87600bebf7b 203
kit3 0:a87600bebf7b 204
kit3 0:a87600bebf7b 205 void right () // function for moving the RIGHT motor in REVERSE direction and LEFT motor in FORWARD direction so...
kit3 0:a87600bebf7b 206 // ..that the robot turns to the LEFT
kit3 0:a87600bebf7b 207
kit3 0:a87600bebf7b 208 {
kit3 0:a87600bebf7b 209 for (float s= -1.0; s < 0.0 ; s += 0.01)
kit3 0:a87600bebf7b 210 {
kit3 0:a87600bebf7b 211 A.speed(1);
kit3 0:a87600bebf7b 212 B.speed(1);
kit3 0:a87600bebf7b 213 wait(0.02);
kit3 0:a87600bebf7b 214 }
kit3 0:a87600bebf7b 215 }
kit3 0:a87600bebf7b 216
kit3 0:a87600bebf7b 217
kit3 0:a87600bebf7b 218 void straight () // function for moving the RIGHT motor in FORWARD direction and LEFT motor in REVERSE direction so...
kit3 0:a87600bebf7b 219 // ..that the robot turns to the LEFT
kit3 0:a87600bebf7b 220
kit3 0:a87600bebf7b 221 {
kit3 0:a87600bebf7b 222 //for (float s= -1.0; s < -0.7 ; s += 0.01)
kit3 0:a87600bebf7b 223 {
kit3 0:a87600bebf7b 224 A.speed(-1);
kit3 0:a87600bebf7b 225 B.speed(1);
kit3 0:a87600bebf7b 226 wait(0.02);
kit3 0:a87600bebf7b 227 }
kit3 0:a87600bebf7b 228
kit3 0:a87600bebf7b 229 }