130 lines
2.3 KiB
C++
130 lines
2.3 KiB
C++
#include <hcsr04.h>
|
|
|
|
/* Define your pins here */
|
|
/* Wheels */
|
|
const int left_wheel_1 = 0;
|
|
const int left_wheel_2 = 1;
|
|
const int right_wheel_1 = 2;
|
|
const int right_wheel_2 = 3;
|
|
|
|
/* Ultrasonic sensor */
|
|
const int sensor_echo = 9;
|
|
const int sensor_trigger = 10;
|
|
|
|
HCSR04 hcsr04(sensor_trigger, sensor_echo, 20, 4000);
|
|
|
|
/* Array bellow contains all the wheels */
|
|
int wheels[] = {left_wheel_1, left_wheel_2, right_wheel_1, right_wheel_2};
|
|
|
|
/* The counter */
|
|
int i;
|
|
|
|
int mm;
|
|
|
|
/* Ultrasonic sensor variables */
|
|
long duration;
|
|
int distance;
|
|
|
|
void setup() {
|
|
/* Setup pins */
|
|
/* wheels */
|
|
for (i = 0; i < sizeof(wheels) - 1; i++) {
|
|
pinMode(wheels[i], OUTPUT);
|
|
}
|
|
|
|
/* Ultrasonic sensor */
|
|
pinMode(sensor_echo, INPUT);
|
|
pinMode(sensor_trigger, OUTPUT);
|
|
|
|
/* Starts serial communication */
|
|
Serial.begin(9600);
|
|
}
|
|
|
|
/* Wheel actions */
|
|
|
|
/* Forward */
|
|
void move_forward_wheel_left() {
|
|
digitalWrite(left_wheel_1, HIGH);
|
|
digitalWrite(left_wheel_2, LOW);
|
|
}
|
|
|
|
void move_forward_wheel_right() {
|
|
digitalWrite(right_wheel_1, HIGH);
|
|
digitalWrite(right_wheel_2, LOW);
|
|
}
|
|
|
|
void move_forward() {
|
|
move_forward_wheel_left();
|
|
move_forward_wheel_right();
|
|
}
|
|
|
|
/* Backward */
|
|
void move_backward_wheel_left() {
|
|
digitalWrite(left_wheel_1, LOW);
|
|
digitalWrite(left_wheel_2, HIGH);
|
|
}
|
|
|
|
void move_backward_wheel_right() {
|
|
digitalWrite(right_wheel_1, LOW);
|
|
digitalWrite(right_wheel_2, HIGH);
|
|
}
|
|
|
|
void move_backward() {
|
|
move_backward_wheel_left();
|
|
move_backward_wheel_right();
|
|
}
|
|
|
|
/* Stops */
|
|
void stop_all() {
|
|
for (i = 0; sizeof(wheels) - 1; i++) {
|
|
digitalWrite(wheels[i], LOW);
|
|
}
|
|
}
|
|
|
|
void stop_left() {
|
|
digitalWrite(left_wheel_1, LOW);
|
|
digitalWrite(left_wheel_2, LOW);
|
|
}
|
|
|
|
void stop_right() {
|
|
digitalWrite(right_wheel_1, LOW);
|
|
digitalWrite(right_wheel_2, LOW);
|
|
}
|
|
|
|
/* Turns */
|
|
void turn_left(int delayMS) {
|
|
stop_left();
|
|
move_forward_wheel_right();
|
|
delay(delayMS);
|
|
}
|
|
|
|
void turn_right(int delayMS) {
|
|
stop_right();
|
|
move_forward_wheel_left();
|
|
delay(delayMS);
|
|
}
|
|
|
|
/* Ultrasonic sensor actions */
|
|
|
|
/* get_distance -- returns distance to an object in centimeters */
|
|
int get_distance() {
|
|
mm = hcsr04.distanceInMillimeters();
|
|
if (mm > 0) {
|
|
return mm / 10;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/* Main code */
|
|
void loop() {
|
|
if (get_distance() > 25) {
|
|
move_forward();
|
|
delay(1000);
|
|
} else {
|
|
turn_left(500);
|
|
move_backward();
|
|
delay(1000);
|
|
turn_right(500);
|
|
}
|
|
}
|