Automower v1

Automated robot that moves around the perimeter based on inputs from its sensors.

Aug 26, 2022

•

883 views

•

1 respects

Components and supplies

LED (generic)

Pushbutton Switch, Pushbutton

PWM Motor Speed Controller

L298N

DC Motor, Miniature

DC Motor, 12 V

Resistor 221 ohm

Arduino UNO

9V battery (generic)

Inertial Measurement Unit (IMU) (6 deg of freedom)

Ultrasonic Sensor - HC-SR04 (Generic)

Tools and machines

Soldering iron (generic)

Solder Wire, Lead Free

Premium Female/Male Extension Jumper Wires, 40 x 6" (150mm)

Project description

Code

Automower v1

c_cpp

1// library for I2C devices
2#include "Wire.h" 
3
4// variables  for ultrasonic sensors
5int trigPinMid = 9;      
6int echoPinMid = 8;     
7int  trigPinRight = A0;
8int echoPinRight = A1;
9int trigPinLeft = A2;
10int echoPinLeft  = A3;
11
12// measured distance by three ultrasonic sensors
13long durationMid  = 0; 
14long distanceMid = 0;
15long durationRight = 0; 
16long distanceRight  = 0;
17long durationLeft = 0; 
18long distanceLeft = 0;
19
20// vars for buttons
21const  int buttonPin = 3;     
22int button = 0; // is pressed?
23
24// vars for motors
25int  backwardRightMotor = 4;       
26int forwardRightMotor = 5;        
27int backwardLeftMotor  = 6;        
28int forwardLeftMotor = 7;         
29int speedLeftMotor = 10;
30int  speedRightMotor = 11;
31
32// vars for gyro
33const int MPU_ADDR = 0x68; // I2C  address MPU-6050
34int16_t accelerometer_x, accelerometer_y, accelerometer_z;
35int16_t  temperature; // temperature
36char tmp_str[7]; // help variable for conversion
37
38//  vars for leds
39int ledZ = 12;
40int ledC = 13;
41
42// convert int16 to string
43char*  convert_int16_to_str(int16_t i) { 
44  sprintf(tmp_str, "%6d", i);
45  return  tmp_str;
46}
47
48// void setup is called after microcontroller start up
49void  setup() {
50  Serial.begin (9600);
51  
52  // gyro config
53  Wire.begin();
54  Wire.beginTransmission(MPU_ADDR);
55  Wire.write(0x6B); 
56  Wire.write(0);  
57  Wire.endTransmission(true);
58  
59  // ultrasonic sensors pins config
60  pinMode(trigPinMid, OUTPUT);
61  pinMode(echoPinMid, INPUT);
62  pinMode(trigPinRight,  OUTPUT);
63  pinMode(echoPinRight, INPUT);
64  pinMode(trigPinLeft, OUTPUT);
65  pinMode(echoPinLeft, INPUT);
66  
67  // led config
68  pinMode(ledZ, OUTPUT);
69  pinMode(ledC, OUTPUT);
70  
71  // button pin config
72  pinMode(buttonPin,  INPUT);
73  
74  // motor pins config
75  pinMode(backwardRightMotor, OUTPUT);      
76  pinMode(forwardRightMotor, OUTPUT);
77  pinMode(backwardLeftMotor,  OUTPUT);
78  pinMode(forwardLeftMotor, OUTPUT);
79  pinMode(speedLeftMotor, OUTPUT);
80  pinMode(speedRightMotor, OUTPUT);
81}
82
83void loop() {
84  // gets slope  data, stored in global vars, if the device is tilted, motors stop
85  getSlope();
86
87  // gets distance from all three ultrasonic sensors, stored in global vars
88  getDistance();
89  
90  // get button pin value (1 if pressed, else 0)
91  button  = digitalRead(buttonPin);  
92
93  // test conditions for move functions (distance  in cm, maximum slope +- 30)
94  if (accelerometer_x > 9000 or accelerometer_y >  9000 or accelerometer_x < -9000 or accelerometer_y < -9000 or accelerometer_z <  0) {
95    // device is tilted, stopping
96    stopMotor();
97  } else if ((distanceMid  < 25 and distanceLeft >= 25 and distanceLeft >= 25) or button == 1) {
98    //  there is an obstacle, seen by mid sensor, or the device bumped into it and the button  was pressed
99    stopMotor();  
100    goBackward();
101    delay(100);
102    turnAround();  
103    delay(200);
104  } else if (distanceLeft < 25) {
105    // there is an obstacle  on the left side
106    stopMotor();
107    delay(20);
108    turnRight();
109    delay(20);  
110  } else if (distanceRight < 25) {
111    // there is an obstacle on the right  side
112    stopMotor();
113    delay(20);
114    turnRight();
115    delay(20);    
116  } else if (distanceMid >= 25 and distanceLeft >= 25 and distanceRight  >= 25 and button == 0) {
117    // no obstacle, can go forward
118    goForward();  
119  }
120  // do this every 50 miliseconds
121  delay(50);
122}
123
124void  getSlope() {
125  // https://mschoeffler.com/2017/12/09/example-application-gy-521-module-mpu-6050-breakout-board-and-arduino-uno/
126  Wire.beginTransmission(MPU_ADDR);
127  Wire.write(0x3B); // starting with register  0x3B (ACCEL_XOUT_H) [MPU-6000 and MPU-6050 Register Map and Descriptions Revision  4.2, p.40]
128  Wire.endTransmission(false); // the parameter indicates that the  Arduino will send a restart. As a result, the connection is kept active.
129  Wire.requestFrom(MPU_ADDR,  7*2, true); // request a total of 7*2=14 registers
130  
131  // "Wire.read()<<8  | Wire.read();" means two registers are read and stored in the same variable
132  accelerometer_x = Wire.read()<<8 | Wire.read(); // reading registers: 0x3B (ACCEL_XOUT_H)  a 0x3C (ACCEL_XOUT_L)
133  accelerometer_y = Wire.read()<<8 | Wire.read(); // reading  registers: 0x3D (ACCEL_YOUT_H) a 0x3E (ACCEL_YOUT_L)
134  accelerometer_z = Wire.read()<<8  | Wire.read(); // reading registers: 0x3F (ACCEL_ZOUT_H) a 0x40 (ACCEL_ZOUT_L)
135  temperature = Wire.read()<<8 | Wire.read(); // reading registers: 0x41 (TEMP_OUT_H)  a 0x42 (TEMP_OUT_L)
136
137  Serial.print("aX = "); Serial.print(convert_int16_to_str(accelerometer_x));
138  Serial.print(" | aY = "); Serial.print(convert_int16_to_str(accelerometer_y));
139  Serial.print(" | aZ = "); Serial.print(convert_int16_to_str(accelerometer_z));
140  // the following equation was taken from the documentation [MPU-6000/MPU-6050  Register Map and Description, p.30]
141  Serial.print(" | tmp = "); Serial.print(temperature/340.00+36.53);
142  Serial.println();
143}
144
145void getDistance() {
146  // https://www.instructables.com/Using-a-SR04/
147  digitalWrite(trigPinMid, LOW);
148  delayMicroseconds(2);
149  digitalWrite(trigPinMid,  HIGH);
150  delayMicroseconds(10);
151  digitalWrite(trigPinMid, LOW);
152  durationMid  = pulseIn(echoPinMid, HIGH);
153  distanceMid = (durationMid/2) / 29.1;
154  
155  digitalWrite(trigPinRight, LOW);
156  delayMicroseconds(2);
157  digitalWrite(trigPinRight,  HIGH);
158  delayMicroseconds(10);
159  digitalWrite(trigPinRight, LOW);
160  durationRight  = pulseIn(echoPinRight, HIGH);
161  distanceRight = (durationRight/2) / 29.1;
162  
163  digitalWrite(trigPinLeft, LOW);
164  delayMicroseconds(2);
165  digitalWrite(trigPinLeft,  HIGH);
166  delayMicroseconds(10);
167  digitalWrite(trigPinLeft, LOW);
168  durationLeft  = pulseIn(echoPinLeft, HIGH);
169  distanceLeft = (durationLeft/2) / 29.1;
170  
171  Serial.print("dS = "); Serial.print(distanceMid);
172  Serial.print(" | dP  = "); Serial.print(distanceRight);
173  Serial.print(" | dL = "); Serial.print(distanceLeft);
174  Serial.println(); 
175}
176
177void goBackward() {
178  Serial.println(" goint  back ");
179  
180  // signals to motor controller, left back and right back
181  digitalWrite(backwardLeftMotor, HIGH);              
182  digitalWrite(forwardLeftMotor,  LOW);
183  digitalWrite(backwardRightMotor, HIGH);                                
184  digitalWrite(forwardRightMotor, LOW);
185  
186  // signal with leds
187  digitalWrite(ledC,  HIGH);
188  digitalWrite(ledZ, HIGH);
189  
190  // do this for 1/10 second
191  delay(100);
192}
193
194void turnLeft() { 
195  Serial.println(" turning slightly  left ");
196  
197  // send PWM signal to motors - (pin number, speed 0-255)
198  analogWrite(speedLeftMotor, 255);
199  analogWrite(speedRightMotor, 255);
200  
201  // signals to motor controller, left back and right forward
202  digitalWrite(backwardLeftMotor,  HIGH);               
203  digitalWrite(forwardLeftMotor, LOW);
204  digitalWrite(backwardRightMotor,  LOW);                                
205  digitalWrite(forwardRightMotor, HIGH);
206  
207  // signal with leds
208  digitalWrite(ledC, HIGH);
209  digitalWrite(ledZ,  HIGH);
210  
211  // do this for 1/10 second
212  delay(100);
213  stopMotor();
214}
215
216void  turnRight() { 
217  Serial.println(" turning slightly right ");
218  
219  //  send PWM signal to motors - (pin number, speed 0-255)
220  analogWrite(speedLeftMotor,  255); 
221  analogWrite(speedRightMotor, 255);
222  
223  // signals to motor controller,  left forward and right back
224  digitalWrite(backwardLeftMotor, LOW);
225  digitalWrite(forwardLeftMotor,  HIGH);
226  digitalWrite(backwardRightMotor, HIGH);                                
227  digitalWrite(forwardRightMotor, LOW);
228  
229  // signal with leds
230  digitalWrite(ledC,  HIGH);
231  digitalWrite(ledZ, HIGH);
232  
233  // do this for 1/10 second
234  delay(100);
235  stopMotor();
236}
237
238void turnAround() { 
239  Serial.println("  turning around ");
240  
241  // send PWM signal to motors - (pin number, speed  0-255)
242  analogWrite(speedLeftMotor, 255); 
243  analogWrite(speedRightMotor,  255);
244  
245  // signals to motor controller, left forward and right back
246  digitalWrite(backwardLeftMotor, LOW);
247  digitalWrite(forwardLeftMotor, HIGH);
248  digitalWrite(backwardRightMotor, HIGH);                                
249  digitalWrite(forwardRightMotor,  LOW);
250  
251  // signal with leds
252  digitalWrite(ledC, HIGH);
253  digitalWrite(ledZ,  HIGH);
254  
255  // do this for 350 - 700 miliseconds, the device is turning with  added random element
256  delay(random(350, 700));
257  stopMotor();
258}
259
260void  stopMotor() { 
261  Serial.println(" stopMotor ");
262  
263  // signals to motor  controller, nothing
264  digitalWrite(forwardLeftMotor, LOW);
265  digitalWrite(backwardLeftMotor,  LOW);
266  digitalWrite(forwardRightMotor, LOW);                                
267  digitalWrite(backwardRightMotor, LOW);
268  
269  // signal with leds
270  digitalWrite(ledC,  HIGH);
271  digitalWrite(ledZ, LOW);
272}
273
274void goForward() { 
275  Serial.println("  going forward ");
276  
277  // send PWM signal to motors - (pin number, speed  0-255)
278  analogWrite(speedLeftMotor, 100); 
279  analogWrite(speedRightMotor,  100); 
280  
281  // signals to motor controller, left forward and right forward
282  digitalWrite(forwardLeftMotor, HIGH);
283  digitalWrite(backwardLeftMotor, LOW);
284  digitalWrite(forwardRightMotor, HIGH);                                
285  digitalWrite(backwardRightMotor,  LOW);
286  
287  // signal with leds
288  digitalWrite(ledC, LOW);
289  digitalWrite(ledZ,  HIGH);
290}
291

// library for I2C devices
#include "Wire.h" 

// variables  for ultrasonic sensors
int trigPinMid = 9;      
int echoPinMid = 8;     
int  trigPinRight = A0;
int echoPinRight = A1;
int trigPinLeft = A2;
int echoPinLeft  = A3;

// measured distance by three ultrasonic sensors
long durationMid  = 0; 
long distanceMid = 0;
long durationRight = 0; 
long distanceRight  = 0;
long durationLeft = 0; 
long distanceLeft = 0;

// vars for buttons
const  int buttonPin = 3;     
int button = 0; // is pressed?

// vars for motors
int  backwardRightMotor = 4;       
int forwardRightMotor = 5;        
int backwardLeftMotor  = 6;        
int forwardLeftMotor = 7;         
int speedLeftMotor = 10;
int  speedRightMotor = 11;

// vars for gyro
const int MPU_ADDR = 0x68; // I2C  address MPU-6050
int16_t accelerometer_x, accelerometer_y, accelerometer_z;
int16_t  temperature; // temperature
char tmp_str[7]; // help variable for conversion

//  vars for leds
int ledZ = 12;
int ledC = 13;

// convert int16 to string
char*  convert_int16_to_str(int16_t i) { 
  sprintf(tmp_str, "%6d", i);
  return  tmp_str;
}

// void setup is called after microcontroller start up
void  setup() {
  Serial.begin (9600);
  
  // gyro config
  Wire.begin();
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); 
  Wire.write(0);  
  Wire.endTransmission(true);
  
  // ultrasonic sensors pins config
  pinMode(trigPinMid, OUTPUT);
  pinMode(echoPinMid, INPUT);
  pinMode(trigPinRight,  OUTPUT);
  pinMode(echoPinRight, INPUT);
  pinMode(trigPinLeft, OUTPUT);
  pinMode(echoPinLeft, INPUT);
  
  // led config
  pinMode(ledZ, OUTPUT);
  pinMode(ledC, OUTPUT);
  
  // button pin config
  pinMode(buttonPin,  INPUT);
  
  // motor pins config
  pinMode(backwardRightMotor, OUTPUT);      
  pinMode(forwardRightMotor, OUTPUT);
  pinMode(backwardLeftMotor,  OUTPUT);
  pinMode(forwardLeftMotor, OUTPUT);
  pinMode(speedLeftMotor, OUTPUT);
  pinMode(speedRightMotor, OUTPUT);
}

void loop() {
  // gets slope  data, stored in global vars, if the device is tilted, motors stop
  getSlope();

  // gets distance from all three ultrasonic sensors, stored in global vars
  getDistance();
  
  // get button pin value (1 if pressed, else 0)
  button  = digitalRead(buttonPin);  

  // test conditions for move functions (distance  in cm, maximum slope +- 30)
  if (accelerometer_x > 9000 or accelerometer_y >  9000 or accelerometer_x < -9000 or accelerometer_y < -9000 or accelerometer_z <  0) {
    // device is tilted, stopping
    stopMotor();
  } else if ((distanceMid  < 25 and distanceLeft >= 25 and distanceLeft >= 25) or button == 1) {
    //  there is an obstacle, seen by mid sensor, or the device bumped into it and the button  was pressed
    stopMotor();  
    goBackward();
    delay(100);
    turnAround();  
    delay(200);
  } else if (distanceLeft < 25) {
    // there is an obstacle  on the left side
    stopMotor();
    delay(20);
    turnRight();
    delay(20);  
  } else if (distanceRight < 25) {
    // there is an obstacle on the right  side
    stopMotor();
    delay(20);
    turnRight();
    delay(20);    
  } else if (distanceMid >= 25 and distanceLeft >= 25 and distanceRight  >= 25 and button == 0) {
    // no obstacle, can go forward
    goForward();  
  }
  // do this every 50 miliseconds
  delay(50);
}

void  getSlope() {
  // https://mschoeffler.com/2017/12/09/example-application-gy-521-module-mpu-6050-breakout-board-and-arduino-uno/
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // starting with register  0x3B (ACCEL_XOUT_H) [MPU-6000 and MPU-6050 Register Map and Descriptions Revision  4.2, p.40]
  Wire.endTransmission(false); // the parameter indicates that the  Arduino will send a restart. As a result, the connection is kept active.
  Wire.requestFrom(MPU_ADDR,  7*2, true); // request a total of 7*2=14 registers
  
  // "Wire.read()<<8  | Wire.read();" means two registers are read and stored in the same variable
  accelerometer_x = Wire.read()<<8 | Wire.read(); // reading registers: 0x3B (ACCEL_XOUT_H)  a 0x3C (ACCEL_XOUT_L)
  accelerometer_y = Wire.read()<<8 | Wire.read(); // reading  registers: 0x3D (ACCEL_YOUT_H) a 0x3E (ACCEL_YOUT_L)
  accelerometer_z = Wire.read()<<8  | Wire.read(); // reading registers: 0x3F (ACCEL_ZOUT_H) a 0x40 (ACCEL_ZOUT_L)
  temperature = Wire.read()<<8 | Wire.read(); // reading registers: 0x41 (TEMP_OUT_H)  a 0x42 (TEMP_OUT_L)

  Serial.print("aX = "); Serial.print(convert_int16_to_str(accelerometer_x));
  Serial.print(" | aY = "); Serial.print(convert_int16_to_str(accelerometer_y));
  Serial.print(" | aZ = "); Serial.print(convert_int16_to_str(accelerometer_z));
  // the following equation was taken from the documentation [MPU-6000/MPU-6050  Register Map and Description, p.30]
  Serial.print(" | tmp = "); Serial.print(temperature/340.00+36.53);
  Serial.println();
}

void getDistance() {
  // https://www.instructables.com/Using-a-SR04/
  digitalWrite(trigPinMid, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinMid,  HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinMid, LOW);
  durationMid  = pulseIn(echoPinMid, HIGH);
  distanceMid = (durationMid/2) / 29.1;
  
  digitalWrite(trigPinRight, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinRight,  HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinRight, LOW);
  durationRight  = pulseIn(echoPinRight, HIGH);
  distanceRight = (durationRight/2) / 29.1;
  
  digitalWrite(trigPinLeft, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinLeft,  HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinLeft, LOW);
  durationLeft  = pulseIn(echoPinLeft, HIGH);
  distanceLeft = (durationLeft/2) / 29.1;
  
  Serial.print("dS = "); Serial.print(distanceMid);
  Serial.print(" | dP  = "); Serial.print(distanceRight);
  Serial.print(" | dL = "); Serial.print(distanceLeft);
  Serial.println(); 
}

void goBackward() {
  Serial.println(" goint  back ");
  
  // signals to motor controller, left back and right back
  digitalWrite(backwardLeftMotor, HIGH);              
  digitalWrite(forwardLeftMotor,  LOW);
  digitalWrite(backwardRightMotor, HIGH);                                
  digitalWrite(forwardRightMotor, LOW);
  
  // signal with leds
  digitalWrite(ledC,  HIGH);
  digitalWrite(ledZ, HIGH);
  
  // do this for 1/10 second
  delay(100);
}

void turnLeft() { 
  Serial.println(" turning slightly  left ");
  
  // send PWM signal to motors - (pin number, speed 0-255)
  analogWrite(speedLeftMotor, 255);
  analogWrite(speedRightMotor, 255);
  
  // signals to motor controller, left back and right forward
  digitalWrite(backwardLeftMotor,  HIGH);               
  digitalWrite(forwardLeftMotor, LOW);
  digitalWrite(backwardRightMotor,  LOW);                                
  digitalWrite(forwardRightMotor, HIGH);
  
  // signal with leds
  digitalWrite(ledC, HIGH);
  digitalWrite(ledZ,  HIGH);
  
  // do this for 1/10 second
  delay(100);
  stopMotor();
}

void  turnRight() { 
  Serial.println(" turning slightly right ");
  
  //  send PWM signal to motors - (pin number, speed 0-255)
  analogWrite(speedLeftMotor,  255); 
  analogWrite(speedRightMotor, 255);
  
  // signals to motor controller,  left forward and right back
  digitalWrite(backwardLeftMotor, LOW);
  digitalWrite(forwardLeftMotor,  HIGH);
  digitalWrite(backwardRightMotor, HIGH);                                
  digitalWrite(forwardRightMotor, LOW);
  
  // signal with leds
  digitalWrite(ledC,  HIGH);
  digitalWrite(ledZ, HIGH);
  
  // do this for 1/10 second
  delay(100);
  stopMotor();
}

void turnAround() { 
  Serial.println("  turning around ");
  
  // send PWM signal to motors - (pin number, speed  0-255)
  analogWrite(speedLeftMotor, 255); 
  analogWrite(speedRightMotor,  255);
  
  // signals to motor controller, left forward and right back
  digitalWrite(backwardLeftMotor, LOW);
  digitalWrite(forwardLeftMotor, HIGH);
  digitalWrite(backwardRightMotor, HIGH);                                
  digitalWrite(forwardRightMotor,  LOW);
  
  // signal with leds
  digitalWrite(ledC, HIGH);
  digitalWrite(ledZ,  HIGH);
  
  // do this for 350 - 700 miliseconds, the device is turning with  added random element
  delay(random(350, 700));
  stopMotor();
}

void  stopMotor() { 
  Serial.println(" stopMotor ");
  
  // signals to motor  controller, nothing
  digitalWrite(forwardLeftMotor, LOW);
  digitalWrite(backwardLeftMotor,  LOW);
  digitalWrite(forwardRightMotor, LOW);                                
  digitalWrite(backwardRightMotor, LOW);
  
  // signal with leds
  digitalWrite(ledC,  HIGH);
  digitalWrite(ledZ, LOW);
}

void goForward() { 
  Serial.println("  going forward ");
  
  // send PWM signal to motors - (pin number, speed  0-255)
  analogWrite(speedLeftMotor, 100); 
  analogWrite(speedRightMotor,  100); 
  
  // signals to motor controller, left forward and right forward
  digitalWrite(forwardLeftMotor, HIGH);
  digitalWrite(backwardLeftMotor, LOW);
  digitalWrite(forwardRightMotor, HIGH);                                
  digitalWrite(backwardRightMotor,  LOW);
  
  // signal with leds
  digitalWrite(ledC, LOW);
  digitalWrite(ledZ,  HIGH);
}

Automower v1

c_cpp

1// library for I2C devices
2#include "Wire.h" 
3
4// variables   for ultrasonic sensors
5int trigPinMid = 9;      
6int echoPinMid = 8;     
7int   trigPinRight = A0;
8int echoPinRight = A1;
9int trigPinLeft = A2;
10int echoPinLeft   = A3;
11
12// measured distance by three ultrasonic sensors
13long durationMid   = 0; 
14long distanceMid = 0;
15long durationRight = 0; 
16long distanceRight   = 0;
17long durationLeft = 0; 
18long distanceLeft = 0;
19
20// vars for buttons
21const   int buttonPin = 3;     
22int button = 0; // is pressed?
23
24// vars for motors
25int   backwardRightMotor = 4;       
26int forwardRightMotor = 5;        
27int backwardLeftMotor   = 6;        
28int forwardLeftMotor = 7;         
29int speedLeftMotor = 10;
30int   speedRightMotor = 11;
31
32// vars for gyro
33const int MPU_ADDR = 0x68; // I2C   address MPU-6050
34int16_t accelerometer_x, accelerometer_y, accelerometer_z;
35int16_t   temperature; // temperature
36char tmp_str[7]; // help variable for conversion
37
38//   vars for leds
39int ledZ = 12;
40int ledC = 13;
41
42// convert int16 to string
43char*   convert_int16_to_str(int16_t i) { 
44  sprintf(tmp_str, "%6d", i);
45  return   tmp_str;
46}
47
48// void setup is called after microcontroller start up
49void   setup() {
50  Serial.begin (9600);
51  
52  // gyro config
53  Wire.begin();
54   Wire.beginTransmission(MPU_ADDR);
55  Wire.write(0x6B); 
56  Wire.write(0);   
57  Wire.endTransmission(true);
58  
59  // ultrasonic sensors pins config
60   pinMode(trigPinMid, OUTPUT);
61  pinMode(echoPinMid, INPUT);
62  pinMode(trigPinRight,   OUTPUT);
63  pinMode(echoPinRight, INPUT);
64  pinMode(trigPinLeft, OUTPUT);
65   pinMode(echoPinLeft, INPUT);
66  
67  // led config
68  pinMode(ledZ, OUTPUT);
69   pinMode(ledC, OUTPUT);
70  
71  // button pin config
72  pinMode(buttonPin,   INPUT);
73  
74  // motor pins config
75  pinMode(backwardRightMotor, OUTPUT);       
76  pinMode(forwardRightMotor, OUTPUT);
77  pinMode(backwardLeftMotor,   OUTPUT);
78  pinMode(forwardLeftMotor, OUTPUT);
79  pinMode(speedLeftMotor, OUTPUT);
80   pinMode(speedRightMotor, OUTPUT);
81}
82
83void loop() {
84  // gets slope   data, stored in global vars, if the device is tilted, motors stop
85  getSlope();
86
87   // gets distance from all three ultrasonic sensors, stored in global vars
88   getDistance();
89  
90  // get button pin value (1 if pressed, else 0)
91  button   = digitalRead(buttonPin);  
92
93  // test conditions for move functions (distance   in cm, maximum slope +- 30)
94  if (accelerometer_x > 9000 or accelerometer_y >   9000 or accelerometer_x < -9000 or accelerometer_y < -9000 or accelerometer_z <   0) {
95    // device is tilted, stopping
96    stopMotor();
97  } else if ((distanceMid   < 25 and distanceLeft >= 25 and distanceLeft >= 25) or button == 1) {
98    //   there is an obstacle, seen by mid sensor, or the device bumped into it and the button   was pressed
99    stopMotor();  
100    goBackward();
101    delay(100);
102    turnAround();   
103    delay(200);
104  } else if (distanceLeft < 25) {
105    // there is an obstacle   on the left side
106    stopMotor();
107    delay(20);
108    turnRight();
109    delay(20);   
110  } else if (distanceRight < 25) {
111    // there is an obstacle on the right   side
112    stopMotor();
113    delay(20);
114    turnRight();
115    delay(20);     
116  } else if (distanceMid >= 25 and distanceLeft >= 25 and distanceRight   >= 25 and button == 0) {
117    // no obstacle, can go forward
118    goForward();   
119  }
120  // do this every 50 miliseconds
121  delay(50);
122}
123
124void   getSlope() {
125  // https://mschoeffler.com/2017/12/09/example-application-gy-521-module-mpu-6050-breakout-board-and-arduino-uno/
126   Wire.beginTransmission(MPU_ADDR);
127  Wire.write(0x3B); // starting with register   0x3B (ACCEL_XOUT_H) [MPU-6000 and MPU-6050 Register Map and Descriptions Revision   4.2, p.40]
128  Wire.endTransmission(false); // the parameter indicates that the   Arduino will send a restart. As a result, the connection is kept active.
129  Wire.requestFrom(MPU_ADDR,   7*2, true); // request a total of 7*2=14 registers
130  
131  // "Wire.read()<<8   | Wire.read();" means two registers are read and stored in the same variable
132   accelerometer_x = Wire.read()<<8 | Wire.read(); // reading registers: 0x3B (ACCEL_XOUT_H)   a 0x3C (ACCEL_XOUT_L)
133  accelerometer_y = Wire.read()<<8 | Wire.read(); // reading   registers: 0x3D (ACCEL_YOUT_H) a 0x3E (ACCEL_YOUT_L)
134  accelerometer_z = Wire.read()<<8   | Wire.read(); // reading registers: 0x3F (ACCEL_ZOUT_H) a 0x40 (ACCEL_ZOUT_L)
135   temperature = Wire.read()<<8 | Wire.read(); // reading registers: 0x41 (TEMP_OUT_H)   a 0x42 (TEMP_OUT_L)
136
137  Serial.print("aX = "); Serial.print(convert_int16_to_str(accelerometer_x));
138   Serial.print(" | aY = "); Serial.print(convert_int16_to_str(accelerometer_y));
139   Serial.print(" | aZ = "); Serial.print(convert_int16_to_str(accelerometer_z));
140   // the following equation was taken from the documentation [MPU-6000/MPU-6050   Register Map and Description, p.30]
141  Serial.print(" | tmp = "); Serial.print(temperature/340.00+36.53);
142   Serial.println();
143}
144
145void getDistance() {
146  // https://www.instructables.com/Using-a-SR04/
147   digitalWrite(trigPinMid, LOW);
148  delayMicroseconds(2);
149  digitalWrite(trigPinMid,   HIGH);
150  delayMicroseconds(10);
151  digitalWrite(trigPinMid, LOW);
152  durationMid   = pulseIn(echoPinMid, HIGH);
153  distanceMid = (durationMid/2) / 29.1;
154  
155   digitalWrite(trigPinRight, LOW);
156  delayMicroseconds(2);
157  digitalWrite(trigPinRight,   HIGH);
158  delayMicroseconds(10);
159  digitalWrite(trigPinRight, LOW);
160  durationRight   = pulseIn(echoPinRight, HIGH);
161  distanceRight = (durationRight/2) / 29.1;
162   
163  digitalWrite(trigPinLeft, LOW);
164  delayMicroseconds(2);
165  digitalWrite(trigPinLeft,   HIGH);
166  delayMicroseconds(10);
167  digitalWrite(trigPinLeft, LOW);
168  durationLeft   = pulseIn(echoPinLeft, HIGH);
169  distanceLeft = (durationLeft/2) / 29.1;
170  
171   Serial.print("dS = "); Serial.print(distanceMid);
172  Serial.print(" | dP   = "); Serial.print(distanceRight);
173  Serial.print(" | dL = "); Serial.print(distanceLeft);
174   Serial.println(); 
175}
176
177void goBackward() {
178  Serial.println(" goint   back ");
179  
180  // signals to motor controller, left back and right back
181   digitalWrite(backwardLeftMotor, HIGH);              
182  digitalWrite(forwardLeftMotor,   LOW);
183  digitalWrite(backwardRightMotor, HIGH);                                
184   digitalWrite(forwardRightMotor, LOW);
185  
186  // signal with leds
187  digitalWrite(ledC,   HIGH);
188  digitalWrite(ledZ, HIGH);
189  
190  // do this for 1/10 second
191   delay(100);
192}
193
194void turnLeft() { 
195  Serial.println(" turning slightly   left ");
196  
197  // send PWM signal to motors - (pin number, speed 0-255)
198   analogWrite(speedLeftMotor, 255);
199  analogWrite(speedRightMotor, 255);
200   
201  // signals to motor controller, left back and right forward
202  digitalWrite(backwardLeftMotor,   HIGH);               
203  digitalWrite(forwardLeftMotor, LOW);
204  digitalWrite(backwardRightMotor,   LOW);                                
205  digitalWrite(forwardRightMotor, HIGH);
206   
207  // signal with leds
208  digitalWrite(ledC, HIGH);
209  digitalWrite(ledZ,   HIGH);
210  
211  // do this for 1/10 second
212  delay(100);
213  stopMotor();
214}
215
216void   turnRight() { 
217  Serial.println(" turning slightly right ");
218  
219  //   send PWM signal to motors - (pin number, speed 0-255)
220  analogWrite(speedLeftMotor,   255); 
221  analogWrite(speedRightMotor, 255);
222  
223  // signals to motor controller,   left forward and right back
224  digitalWrite(backwardLeftMotor, LOW);
225  digitalWrite(forwardLeftMotor,   HIGH);
226  digitalWrite(backwardRightMotor, HIGH);                                
227   digitalWrite(forwardRightMotor, LOW);
228  
229  // signal with leds
230  digitalWrite(ledC,   HIGH);
231  digitalWrite(ledZ, HIGH);
232  
233  // do this for 1/10 second
234   delay(100);
235  stopMotor();
236}
237
238void turnAround() { 
239  Serial.println("   turning around ");
240  
241  // send PWM signal to motors - (pin number, speed   0-255)
242  analogWrite(speedLeftMotor, 255); 
243  analogWrite(speedRightMotor,   255);
244  
245  // signals to motor controller, left forward and right back
246   digitalWrite(backwardLeftMotor, LOW);
247  digitalWrite(forwardLeftMotor, HIGH);
248   digitalWrite(backwardRightMotor, HIGH);                                
249  digitalWrite(forwardRightMotor,   LOW);
250  
251  // signal with leds
252  digitalWrite(ledC, HIGH);
253  digitalWrite(ledZ,   HIGH);
254  
255  // do this for 350 - 700 miliseconds, the device is turning with   added random element
256  delay(random(350, 700));
257  stopMotor();
258}
259
260void   stopMotor() { 
261  Serial.println(" stopMotor ");
262  
263  // signals to motor   controller, nothing
264  digitalWrite(forwardLeftMotor, LOW);
265  digitalWrite(backwardLeftMotor,   LOW);
266  digitalWrite(forwardRightMotor, LOW);                                
267   digitalWrite(backwardRightMotor, LOW);
268  
269  // signal with leds
270  digitalWrite(ledC,   HIGH);
271  digitalWrite(ledZ, LOW);
272}
273
274void goForward() { 
275  Serial.println("   going forward ");
276  
277  // send PWM signal to motors - (pin number, speed   0-255)
278  analogWrite(speedLeftMotor, 100); 
279  analogWrite(speedRightMotor,   100); 
280  
281  // signals to motor controller, left forward and right forward
282   digitalWrite(forwardLeftMotor, HIGH);
283  digitalWrite(backwardLeftMotor, LOW);
284   digitalWrite(forwardRightMotor, HIGH);                                
285  digitalWrite(backwardRightMotor,   LOW);
286  
287  // signal with leds
288  digitalWrite(ledC, LOW);
289  digitalWrite(ledZ,   HIGH);
290}
291

// library for I2C devices
#include "Wire.h" 

// variables   for ultrasonic sensors
int trigPinMid = 9;      
int echoPinMid = 8;     
int   trigPinRight = A0;
int echoPinRight = A1;
int trigPinLeft = A2;
int echoPinLeft   = A3;

// measured distance by three ultrasonic sensors
long durationMid   = 0; 
long distanceMid = 0;
long durationRight = 0; 
long distanceRight   = 0;
long durationLeft = 0; 
long distanceLeft = 0;

// vars for buttons
const   int buttonPin = 3;     
int button = 0; // is pressed?

// vars for motors
int   backwardRightMotor = 4;       
int forwardRightMotor = 5;        
int backwardLeftMotor   = 6;        
int forwardLeftMotor = 7;         
int speedLeftMotor = 10;
int   speedRightMotor = 11;

// vars for gyro
const int MPU_ADDR = 0x68; // I2C   address MPU-6050
int16_t accelerometer_x, accelerometer_y, accelerometer_z;
int16_t   temperature; // temperature
char tmp_str[7]; // help variable for conversion

//   vars for leds
int ledZ = 12;
int ledC = 13;

// convert int16 to string
char*   convert_int16_to_str(int16_t i) { 
  sprintf(tmp_str, "%6d", i);
  return   tmp_str;
}

// void setup is called after microcontroller start up
void   setup() {
  Serial.begin (9600);
  
  // gyro config
  Wire.begin();
   Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); 
  Wire.write(0);   
  Wire.endTransmission(true);
  
  // ultrasonic sensors pins config
   pinMode(trigPinMid, OUTPUT);
  pinMode(echoPinMid, INPUT);
  pinMode(trigPinRight,   OUTPUT);
  pinMode(echoPinRight, INPUT);
  pinMode(trigPinLeft, OUTPUT);
   pinMode(echoPinLeft, INPUT);
  
  // led config
  pinMode(ledZ, OUTPUT);
   pinMode(ledC, OUTPUT);
  
  // button pin config
  pinMode(buttonPin,   INPUT);
  
  // motor pins config
  pinMode(backwardRightMotor, OUTPUT);       
  pinMode(forwardRightMotor, OUTPUT);
  pinMode(backwardLeftMotor,   OUTPUT);
  pinMode(forwardLeftMotor, OUTPUT);
  pinMode(speedLeftMotor, OUTPUT);
   pinMode(speedRightMotor, OUTPUT);
}

void loop() {
  // gets slope   data, stored in global vars, if the device is tilted, motors stop
  getSlope();

   // gets distance from all three ultrasonic sensors, stored in global vars
   getDistance();
  
  // get button pin value (1 if pressed, else 0)
  button   = digitalRead(buttonPin);  

  // test conditions for move functions (distance   in cm, maximum slope +- 30)
  if (accelerometer_x > 9000 or accelerometer_y >   9000 or accelerometer_x < -9000 or accelerometer_y < -9000 or accelerometer_z <   0) {
    // device is tilted, stopping
    stopMotor();
  } else if ((distanceMid   < 25 and distanceLeft >= 25 and distanceLeft >= 25) or button == 1) {
    //   there is an obstacle, seen by mid sensor, or the device bumped into it and the button   was pressed
    stopMotor();  
    goBackward();
    delay(100);
    turnAround();   
    delay(200);
  } else if (distanceLeft < 25) {
    // there is an obstacle   on the left side
    stopMotor();
    delay(20);
    turnRight();
    delay(20);   
  } else if (distanceRight < 25) {
    // there is an obstacle on the right   side
    stopMotor();
    delay(20);
    turnRight();
    delay(20);     
  } else if (distanceMid >= 25 and distanceLeft >= 25 and distanceRight   >= 25 and button == 0) {
    // no obstacle, can go forward
    goForward();   
  }
  // do this every 50 miliseconds
  delay(50);
}

void   getSlope() {
  // https://mschoeffler.com/2017/12/09/example-application-gy-521-module-mpu-6050-breakout-board-and-arduino-uno/
   Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // starting with register   0x3B (ACCEL_XOUT_H) [MPU-6000 and MPU-6050 Register Map and Descriptions Revision   4.2, p.40]
  Wire.endTransmission(false); // the parameter indicates that the   Arduino will send a restart. As a result, the connection is kept active.
  Wire.requestFrom(MPU_ADDR,   7*2, true); // request a total of 7*2=14 registers
  
  // "Wire.read()<<8   | Wire.read();" means two registers are read and stored in the same variable
   accelerometer_x = Wire.read()<<8 | Wire.read(); // reading registers: 0x3B (ACCEL_XOUT_H)   a 0x3C (ACCEL_XOUT_L)
  accelerometer_y = Wire.read()<<8 | Wire.read(); // reading   registers: 0x3D (ACCEL_YOUT_H) a 0x3E (ACCEL_YOUT_L)
  accelerometer_z = Wire.read()<<8   | Wire.read(); // reading registers: 0x3F (ACCEL_ZOUT_H) a 0x40 (ACCEL_ZOUT_L)
   temperature = Wire.read()<<8 | Wire.read(); // reading registers: 0x41 (TEMP_OUT_H)   a 0x42 (TEMP_OUT_L)

  Serial.print("aX = "); Serial.print(convert_int16_to_str(accelerometer_x));
   Serial.print(" | aY = "); Serial.print(convert_int16_to_str(accelerometer_y));
   Serial.print(" | aZ = "); Serial.print(convert_int16_to_str(accelerometer_z));
   // the following equation was taken from the documentation [MPU-6000/MPU-6050   Register Map and Description, p.30]
  Serial.print(" | tmp = "); Serial.print(temperature/340.00+36.53);
   Serial.println();
}

void getDistance() {
  // https://www.instructables.com/Using-a-SR04/
   digitalWrite(trigPinMid, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinMid,   HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinMid, LOW);
  durationMid   = pulseIn(echoPinMid, HIGH);
  distanceMid = (durationMid/2) / 29.1;
  
   digitalWrite(trigPinRight, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinRight,   HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinRight, LOW);
  durationRight   = pulseIn(echoPinRight, HIGH);
  distanceRight = (durationRight/2) / 29.1;
   
  digitalWrite(trigPinLeft, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinLeft,   HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinLeft, LOW);
  durationLeft   = pulseIn(echoPinLeft, HIGH);
  distanceLeft = (durationLeft/2) / 29.1;
  
   Serial.print("dS = "); Serial.print(distanceMid);
  Serial.print(" | dP   = "); Serial.print(distanceRight);
  Serial.print(" | dL = "); Serial.print(distanceLeft);
   Serial.println(); 
}

void goBackward() {
  Serial.println(" goint   back ");
  
  // signals to motor controller, left back and right back
   digitalWrite(backwardLeftMotor, HIGH);              
  digitalWrite(forwardLeftMotor,   LOW);
  digitalWrite(backwardRightMotor, HIGH);                                
   digitalWrite(forwardRightMotor, LOW);
  
  // signal with leds
  digitalWrite(ledC,   HIGH);
  digitalWrite(ledZ, HIGH);
  
  // do this for 1/10 second
   delay(100);
}

void turnLeft() { 
  Serial.println(" turning slightly   left ");
  
  // send PWM signal to motors - (pin number, speed 0-255)
   analogWrite(speedLeftMotor, 255);
  analogWrite(speedRightMotor, 255);
   
  // signals to motor controller, left back and right forward
  digitalWrite(backwardLeftMotor,   HIGH);               
  digitalWrite(forwardLeftMotor, LOW);
  digitalWrite(backwardRightMotor,   LOW);                                
  digitalWrite(forwardRightMotor, HIGH);
   
  // signal with leds
  digitalWrite(ledC, HIGH);
  digitalWrite(ledZ,   HIGH);
  
  // do this for 1/10 second
  delay(100);
  stopMotor();
}

void   turnRight() { 
  Serial.println(" turning slightly right ");
  
  //   send PWM signal to motors - (pin number, speed 0-255)
  analogWrite(speedLeftMotor,   255); 
  analogWrite(speedRightMotor, 255);
  
  // signals to motor controller,   left forward and right back
  digitalWrite(backwardLeftMotor, LOW);
  digitalWrite(forwardLeftMotor,   HIGH);
  digitalWrite(backwardRightMotor, HIGH);                                
   digitalWrite(forwardRightMotor, LOW);
  
  // signal with leds
  digitalWrite(ledC,   HIGH);
  digitalWrite(ledZ, HIGH);
  
  // do this for 1/10 second
   delay(100);
  stopMotor();
}

void turnAround() { 
  Serial.println("   turning around ");
  
  // send PWM signal to motors - (pin number, speed   0-255)
  analogWrite(speedLeftMotor, 255); 
  analogWrite(speedRightMotor,   255);
  
  // signals to motor controller, left forward and right back
   digitalWrite(backwardLeftMotor, LOW);
  digitalWrite(forwardLeftMotor, HIGH);
   digitalWrite(backwardRightMotor, HIGH);                                
  digitalWrite(forwardRightMotor,   LOW);
  
  // signal with leds
  digitalWrite(ledC, HIGH);
  digitalWrite(ledZ,   HIGH);
  
  // do this for 350 - 700 miliseconds, the device is turning with   added random element
  delay(random(350, 700));
  stopMotor();
}

void   stopMotor() { 
  Serial.println(" stopMotor ");
  
  // signals to motor   controller, nothing
  digitalWrite(forwardLeftMotor, LOW);
  digitalWrite(backwardLeftMotor,   LOW);
  digitalWrite(forwardRightMotor, LOW);                                
   digitalWrite(backwardRightMotor, LOW);
  
  // signal with leds
  digitalWrite(ledC,   HIGH);
  digitalWrite(ledZ, LOW);
}

void goForward() { 
  Serial.println("   going forward ");
  
  // send PWM signal to motors - (pin number, speed   0-255)
  analogWrite(speedLeftMotor, 100); 
  analogWrite(speedRightMotor,   100); 
  
  // signals to motor controller, left forward and right forward
   digitalWrite(forwardLeftMotor, HIGH);
  digitalWrite(backwardLeftMotor, LOW);
   digitalWrite(forwardRightMotor, HIGH);                                
  digitalWrite(backwardRightMotor,   LOW);
  
  // signal with leds
  digitalWrite(ledC, LOW);
  digitalWrite(ledZ,   HIGH);
}

Downloadable files

Schema

Comments

Only logged in users can leave comments

MariMarcel

0 Followers

•

0 Projects

Intro

Trademarks & CopyrightsWhistleblowingDigital Services ActTerms of ServicePrivacy PolicySecurityCookie Settings

Report content