Tanu Tanu
Obstacle detection and avoidance Robot
Dependencies: HCSR-04 Servo mbed
Fork of
Obstacle_avoidance
by 
ece nith
Diff: main.cpp
- Revision:
- 1:26d645dc47f8
- Parent:
- 0:a87600bebf7b
--- a/main.cpp Sat May 18 04:39:55 2013 +0000
+++ b/main.cpp Sun Mar 27 16:25:03 2016 +0000
@@ -1,229 +1,243 @@
-/* Megha Sharma, Rishabh Sehgal, Ragheshwar Singh Ranawat ,E.C.E.D NIT Hamirpur
-
- OBSTACLE AVOIDING ROBOT USING ULTRASONIC SENSOR HCSR04
-
-In this project LPC 1768 embed board along with NGX Expresso baseboard are successfuly used to make an obstacle avoiding/
-self navigating robot which makes use of Ultrasonic Sensor HCSR04 for obstacle detection and Ranging. The maximum range of
-Ultrasonic sensor is about 3-4 meters. A program called "ping" sends a serial trigger pulse of 10 nS to the Trigger pin of
-HCSR04 and on receiving the trigger HCSR04 beams out an 8 cycle Sonic burst. Then Echo pulse is received by the Ultrasonic sensor
-upto 25ms. On receiving the Echo pulse distance is calculated. The HCSR04 is mounted Axially on a servo motor. Initially the HCSR04
-is kept in a straight direction. Motors make bot move in forward direction and continuosly calculating the distance of the
-nearest obstacle in the forward direction. After seeing a distance at less than a "Constraint Value" (i.e. 10 cm here),
-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
-Distance in right direction in "rightscanval" variable.Then it comes back to centre position. Now it moves towards the
-Left direction, waits for 2 seconds and stores the value of Distance in "leftscanval" variable. Then it comes back to the
-centre position. Now it does the comparison of the two variables to determine in which direction it should move. It
-successfully turns towards the direction with the farthest distance of obstacle.
-
-Again the Servo motor is in forward direction keeping the sensor straight and the Robot also moves towards Forward direction all
-the while calculating the distance of obstacle in forward direction. The loop continues.....
-
-*/
#include "mbed.h"
#include "hcsr04.h"
-#include "motordriver.h"
-#include "TextLCD.h"
#include "Servo.h"
-int ping ();
-void scan();
-void right (void);
-void stop (void );
-void straight (void);
-void left (void);
-Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can brake
-Motor B(p21, p7, p8, 1);
-HCSR04 usensor(p13,p14);
-TextLCD lcd(p24, p26, p27, p28, p29, p30); // rs, e, d4-d7
-PwmOut servo(p23); // Servo pin 21
+#define HIGH 1
+#define LOW 0
+void forward();
+void backward();
+void right();
+void left();
+void stop();
+void loop();
+
+int distance; //Variable to store distance from an object
+int checkRight;
+int checkLeft;
+int function=1; //Variable to store function of robot: '1' running or '0' stoped. By
+int pos=90;
+
+
+Serial pc(USBTX,USBRX);
+PwmOut servo(D6);
+
+
+
+HCSR04 usensor(A4,A5);
+
+//Variables
+
+int main()
+{
+
+ while(1)
+ {
+loop();
+}
+
+}
+
+
+
+void forward(){
+
+ pc.printf("fwd");
+ DigitalOut(D13,0);
+ DigitalOut(D12, 1);
+ DigitalOut(D9, 1);
+ DigitalOut(D8, 0);
+}
+
+
+
+void backward(){
+ DigitalOut(D13,1);
+ DigitalOut(D12, 0);
+ DigitalOut(D9, 0);
+ DigitalOut(D8, 1);
+}
+
+void right()
+
+{
+
+ DigitalOut(D13,0);
+ DigitalOut(D12, 1);
+ DigitalOut(D9, 0);
+ DigitalOut(D8, 1);
+
+
+ }
+
+
+
+ void left()
+
+{
+
+ DigitalOut(D13,1);
+ DigitalOut(D12, 0);
+ DigitalOut(D9, 1);
+ DigitalOut(D8, 0);
+
+
+ }
-unsigned int dist;
-DigitalOut myled(LED1);
-DigitalOut myled1(LED2);
-DigitalOut myled2(LED3);
-int count = 0;
-
-int main ()
- {
-
- while (1) // continuos loop to perform the scanning function of HCSR04 ..
- //a trigger pulse of Ultrasonic waves having duration 10 nS is sent through the echo pin
- {
+ void stop()
- servo.period(0.020);
- int distance = ping(); // ping function
- straight(); // use the ping() function to see if anything is ahead.
-
-
- if (distance < 10) // the condition for stopping the Obstacle Avoider...
- // ...whenever the defined proximity to obstacle is reached.
- {
- stop();
- scan();
-
-
- /*switch (a) // switch case for initial testing of the Obstacle detection by sensor and
- // Corresponding L.E.D. of the best direction will glow.
+{
+
+ DigitalOut(D13,0);
+ DigitalOut(D12, 0);
+ DigitalOut(D9, 0);
+ DigitalOut(D8, 0);
+
+
+ }
+
+
+ void loop()
+
+ {
+
+ servo.period(0.020);
+ usensor.start();
+ wait_ms(500);
+ distance = usensor.get_dist_cm(); //Tip: Use 'CM' for centimeters or 'INC' for inches
+ pc.printf("Dist: %d ",distance);
+ //Check for objects...
+ if (distance > 10){
+ pc.printf(" No Object \n");
+ forward(); //All clear, move forward!
+ //noTone(buzzer);
+ // digitalOut LED(0);
+ }
+ else if (distance <=10){
+ pc.printf("Halt - Object on the Way \n");
+ stop();
+
+
+ for(float offset=0.0016; offset<=0.0027; offset+=0.0001) // turning RIGHT
+
+{
- {
- case 'r':
- myled=1;
- break;
- case 'l':
- myled1=1;
- break;
- case 'f':
- myled2=1;
- break;
- }*/
+ pc.printf("Turning Right");
+
+servo.pulsewidth(offset); // servo position determined by a pulsewidth between
+
+wait(0.10);
+
+}
+
+wait (2); // stopping at RIGHTMOST position for 2 seconds
+
+
+
+
+ usensor.start();
+
+ wait_ms(500);
+
+ checkRight = usensor.get_dist_cm(); //Take distance from the left side
+
+ pc.printf("DistL: %d ",checkLeft);
+
+
+ for (float offset=0.0027; offset>=0.0016;) // turning back to theCENTER position
+{
+ pc.printf("Turning Center");
+
+servo.pulsewidth(offset); // servo position determined by a pulsewidth between
+
+wait(0.10);
+offset=offset-0.0001;
+}
+
+wait(2);
+
+
+for (float offset=0.0016; offset>=0.0009; offset=offset-0.0001) // Turning towards LEFT
+
+{
+
+ pc.printf("Turning Left");
+servo.pulsewidth(offset); // servo position determined by a pulsewidth between
+
+wait(0.10);
+
+}
+
+wait(2); // stopping at LEFTMOST position for 2
+
+
+ usensor.start();
+
+ wait_ms(500);
+
+ checkLeft= usensor.get_dist_cm();
+
+ pc.printf("DistR: %d ",checkRight);
+
+
+for(float offset=0.0009; offset<=0.0016; offset=offset+0.0001)
+
+{
+ pc.printf("Turning Center");
+
+servo.pulsewidth(offset); // servo position determined by a pulsewidth between
+
+wait(0.10);
+
+}
+
+
+ //Finally, take the right decision, turn left or right?
+
+ if (checkLeft < checkRight){
+
+ left();
+
+ wait_ms(400);
+
+ // wait_ms, change value if necessary to make robot turn.
+ forward();
+ }
+
+ else if (checkLeft > checkRight){
+
+ right();
+
+ wait_ms(400);
+ forward();
+ // wait_ms, change value if necessary to make robot turn.
+
+ }
+
+ else if (checkLeft <=10 && checkRight <=10){
+
+ backward(); //The road is closed... go back and then left ;)
+
+ left();
+ forward();
+
+ }
+
+ }
+
- }
- }
- }
-
- // A functon to perform the continuos pinging function of HCSR04 ..
- //a trigger pulse of Ultrasonic waves having duration 10 nS is sent through the echo pin
-
- int ping() // ultrasonic sensor is started and data collected is displayed on the LCD
- // ping function returns an int value of distance scanned
- {
- usensor.start();
- wait_ms(200);
- dist=usensor.get_dist_cm();
- lcd.cls();
- lcd.locate(0,0);
- if (dist <50)
- lcd.printf("cm:%ld",dist );
- else
- lcd.printf("no object");
- return (dist);
-
- }
+
+}
+
+
+
- void scan ()
- {
-
- int leftscanval, rightscanval; // variables for storing distance scan values...
- //... of nearest obstacle in the left and right direction
-
-
- // Scanning RIGHT side; turning the servo attached to HCSR04 right to check for obstacle distance
-
- // 0.0016 corresponds to the neutral position of servo motor
- // 0.0027 corresponds to the rightmost position of the servo motor
- // 0.0009 corresponds to the leftmost position of the servomotor
-
- for(float offset=0.0016; offset<=0.0027; offset+=0.0001) // turning RIGHT
-
- {
- servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
- wait(0.10);
- }
- wait (2); // stopping at RIGHTMOST position for 2 seconds
- rightscanval = ping(); // putting the distance of right direction's obstacle in ''rightscanval variable''
-
-
-
- for(float offset=0.0027; offset>=0.0016; offset+=-0.0001) // turning back to the CENTER position
-
- {
- servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
- wait(0.10);
- }
- wait(2); // stopping at CENTERMOST position for 2 seconds
-
-
- // Scanning LEFT side; turning the servo attached to HCSR04 right to check for obstacle distance
-
-
- for(float offset=0.0016; offset>=0.0009; offset-=0.0001) // Turning towards LEFT
-
- {
- servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
- wait(0.10);
- }
- wait(2); // stopping at LEFTMOST position for 2 seconds
- leftscanval = ping(); // putting the distance of right direction's obstacle in ''leftscanval variable''
-
-
- for(float offset=0.0009; offset<=0.0016; offset+=0.0001)
-
- {
- servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
- wait(0.10);
- }
- /*center scan servo (for changing the default position of Servo to the Centre Position ) This method didnt work
- though. There is a library of Servo
- */
- //servo.pulsewidth(0.00017);
-
-
- /* Comparisons to choose the most appropiate direction for the turning of Robot. Based on the distances stored in variables "leftscanvalue" and "rightscanvalue" */
-
- if (leftscanval>rightscanval )
-
- {
- left();
- }
-
- else if (rightscanval>leftscanval )
-
- {
- right();
- }
-
- else
- {
- left();
- }
-
- //return choice;
- }
-
- void stop () // function for stopping the DC motors
-
- {
- A.speed(0);
- B.speed(0);
- }
-
-
- void left() // function for moving the RIGHT motor in FORWARD direction and LEFT motor in REVERSE direction so...
- // ..that the robot turns to the LEFT
-
- {
- for (float s= -1.0; s < 0.0 ; s += 0.01)
- {
- A.speed(-1);
- B.speed(-1);
- wait(0.02);
- }
- }
-
-
- void right () // function for moving the RIGHT motor in REVERSE direction and LEFT motor in FORWARD direction so...
- // ..that the robot turns to the LEFT
-
- {
- for (float s= -1.0; s < 0.0 ; s += 0.01)
- {
- A.speed(1);
- B.speed(1);
- wait(0.02);
- }
- }
+
+
+
+
+
+
-
- void straight () // function for moving the RIGHT motor in FORWARD direction and LEFT motor in REVERSE direction so...
- // ..that the robot turns to the LEFT
-
- {
- //for (float s= -1.0; s < -0.7 ; s += 0.01)
- {
- A.speed(-1);
- B.speed(1);
- wait(0.02);
- }
-
- }
\ No newline at end of file
+
+