Obstacle detecting Robot

1/**
2*
3* This code is for driving a two motor robot. Components:
4*  1. Arduino
5* 2. HC-SR04 Ultrasonic Sensor
6* 3. Dual motor driver L298N or  similar.
7* 
8* Important NoteS: 
9* 1. DO NOT POWER YOUR MOTORS FROM ARDUINO
10*  2. DO NOT POWER YOUR ARDUINO FROM MOTOR DRIVER
11* 3. You will most likely need  dual DC power source for 
12*    3.1 Arduino (Most models should work, upto 12  V)
13*    3.2 Motor Driver
14* 4. DUAL POWER IS ONLY NEEDED IN CASE SINGLE POWER
15*    SOURCE CANNOT POWER THE MOTORS.
16*
17* THIS IS A FATHER DAUGHTER PROJECT  AND IS TESTED AS MUCH AS WE COULD. IF
18* YOU FIND ANY BUGS WE WILL TRY TO FIX  IT BUT NO GURANTEES.
19*
20* The code works on a simple logic, the robot tries  to find a way when
21* it encounters an obstacle. It turns counter clockwise by  90 degrees, if it finds an obstacle again
22* it turns clockwise by 180 degrees  and if it finds an obstacle again it will turn around. The turn around
23* depends  on the spatial geometry. To achieve the turn by degrees the macro degreesToMillis
24*  is used. Basically what this does is it calculates the time in milliseconds that  the wheels
25* must spin in order to make the turn. 
26* The Robot will always  turn on it's central axis, so both wheels would turn in opposite
27* directions.  This makes the turning radius much smaller.
28* 
29* The RPM of the two motors  we had were not exactly same, so MOTOR_SPEED_ADJUSTMENT_DELAY is used to adjust  that. 
30* You may set that to 0 if the RPM of the two motors are same.
31* The  code tries to keep track of 2D spatial geometry.
32*/
33
34
35#define MAX_OBSTACLE_DISTANCE  25    // MAX distance in mm for obstacle
36#define MOTOR_RPM 60.0              //  Actual RPM of the motor
37#define PI 3.141
38#define ROTATION_RADIUS_MM 70.0     //  Radius of rotation of the entire Robot
39#define WHEEL_RADIUS_MM 32.5        //  Radius of each wheel
40#define WHEEL_TURNS_PER_DEGREE ROTATION_RADIUS_MM / (WHEEL_RADIUS_MM  * 360)   // Turns / degree
41#define MIN_TO_MILLIS 60000.0
42#define degreesToMillis(x)  (x * WHEEL_TURNS_PER_DEGREE * MIN_TO_MILLIS) / MOTOR_RPM
43#define revsToMillis(x)  (MIN_TO_MILLIS / MOTOR_RPM) * x
44#define MOTOR_SPEED_ADJUSTMENT_DELAY 900
45
46//  Change the values below as per your need
47uint8_t trigPin = 3;      // Trig pin  of HC-SR04
48uint8_t echoPin = 2;      // Echo pin of HC-SR04
49
50uint8_t revright6  = 5;    //Reverse motion of Right motor
51uint8_t fwdright7 = 6;    //Forward motion  of Right motor
52uint8_t revleft8 = 10;    //Reverse motion of Left motor
53uint8_t  fwdleft9 = 11;    //Forward motion of Left motor
54
55// DO NOT MODIFY UNLESS  YOU ARE ABSOLUTELY SURE
56
57int8_t pos2d = 0;
58
59int findDistance() {
60  int distance = 0;
61  int duration = 0;
62  digitalWrite(trigPin, LOW);
63  delayMicroseconds(2);   
64  digitalWrite(trigPin, HIGH);     // send waves for  10 us
65  delayMicroseconds(10);
66  duration = pulseIn(echoPin, HIGH); // receive  reflected waves
67  distance = duration / 58.2;   // convert to distance  
68  Serial.write("Distance\
\
69");
70  Serial.println(distance, DEC);
71  return  distance;
72}
73
74void forward() {
75  // Reset the 2d coordinate space
76  pos2d = 0;
77  Serial.write("Forward\
\
78");  
79  digitalWrite(revleft8,  LOW);          
80  digitalWrite(revright6, LOW);
81  digitalWrite(fwdright7,  HIGH); 
82  delayMicroseconds(MOTOR_SPEED_ADJUSTMENT_DELAY);  
83  digitalWrite(fwdleft9,  HIGH);    
84}
85
86void reverse() {
87  // Reset the 2d coordinate space
88  pos2d = 0;
89  Serial.write("Reverse\
\
90");
91  digitalWrite(fwdright7,  LOW);  
92  digitalWrite(fwdleft9, LOW);
93  digitalWrite(revright6, HIGH);  
94  digitalWrite(revleft8, HIGH);  
95  delay(revsToMillis(1));
96}
97
98void  stop() {
99  Serial.write("Stop\
\
100");
101  digitalWrite(revright6, LOW);
102  digitalWrite(fwdright7, LOW);                          
103  digitalWrite(revleft8,  LOW);  
104  digitalWrite(fwdleft9, LOW); 
105  delay(500);
106}
107
108void turnLeft(int  degrees) {
109  Serial.write("Left\
\
110");
111  pos2d += degrees;
112  digitalWrite(revright6,  LOW);   
113  digitalWrite(fwdleft9, LOW); 
114  digitalWrite(fwdright7, HIGH);  
115  digitalWrite(revleft8, HIGH);       
116  delay(degreesToMillis(degrees));
117  digitalWrite(fwdright7, LOW);  
118  digitalWrite(revleft8, LOW);   
119}
120
121void  turnRight(int degrees) {
122  Serial.write("Right\
\
123");
124  pos2d += (degrees  * -1);  
125  digitalWrite(revleft8, LOW);
126  digitalWrite(fwdright7, LOW);       
127  digitalWrite(revright6, HIGH);
128  digitalWrite(fwdleft9, HIGH);       
129  delay(degreesToMillis(degrees));
130  digitalWrite(revright6, LOW);
131  digitalWrite(fwdleft9, LOW); 
132}
133
134void  turnAround() {
135  Serial.write("Turn around\
\
136");
137  if (pos2d >= 0) {
138    digitalWrite(revright6, LOW);       
139    digitalWrite(fwdleft9, LOW);  
140    digitalWrite(revleft8, HIGH);    
141    digitalWrite(fwdright7, HIGH);
142    delay(degreesToMillis(180 + pos2d));
143    digitalWrite(revleft8, LOW);    
144    digitalWrite(fwdright7, LOW);
145  } else {    
146    digitalWrite(revleft8,  LOW);    
147    digitalWrite(fwdright7, LOW);
148    digitalWrite(revright6, HIGH);       
149    digitalWrite(fwdleft9, HIGH);  
150    delay(degreesToMillis(180  + pos2d));
151    digitalWrite(revright6, LOW);       
152    digitalWrite(fwdleft9,  LOW);  
153  }
154}
155
156void findPath() {  
157  Serial.write("Find path\
\
158");
159  int dist = findDistance();
160  // First try to find a path on the right
161  turnRight(90);
162  stop();
163  dist = findDistance();  
164  if (dist <= MAX_OBSTACLE_DISTANCE)  {
165    turnLeft(180);
166    stop();
167  }    
168  dist = findDistance();
169  if (dist <= MAX_OBSTACLE_DISTANCE) {
170    turnAround();
171  }
172}
173
174void  setup() {
175  Serial.begin(9600);
176  pinMode(revright6, OUTPUT);
177  pinMode(fwdright7,  OUTPUT);
178  pinMode(revleft8, OUTPUT);      // set Motor pins as output
179  pinMode(fwdleft9,  OUTPUT); 
180  pinMode(trigPin, OUTPUT);         // set trig pin as output
181  pinMode(echoPin,  INPUT);          //set echo pin as input to capture reflected waves*/
182}
183
184void  loop() {
185  int distance = findDistance();
186  if (distance > 0 && distance <=  MAX_OBSTACLE_DISTANCE) {
187    Serial.write("Obstacle\
\
188");
189    //Stop
190    stop();
191    reverse();
192    stop();
193    findPath();
194  } else {    
195    forward();    
196  }
197}
198