154 lines
2.9 KiB
C++
154 lines
2.9 KiB
C++
#include <hcsr04.h>
|
|
|
|
/* constans */
|
|
/* ******** */
|
|
|
|
/* Define your pins here */
|
|
/* Wheels */
|
|
#define left_wheel_1 0
|
|
#define left_wheel_2 1
|
|
#define right_wheel_1 2
|
|
#define right_wheel_2 3
|
|
|
|
/* Ultrasonic sensor */
|
|
#define sensor_echo 9
|
|
#define sensor_trigger 10
|
|
|
|
/* Other constans */
|
|
#define move_delay 500
|
|
#define turn_delay 500
|
|
|
|
/* disconnected pin for random seed */
|
|
#define analog_disconnected 0
|
|
|
|
/* Initialize sensor class */
|
|
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};
|
|
|
|
void setup() {
|
|
/* Setup pins */
|
|
/* wheels */
|
|
for (int i = 0; i < sizeof(wheels) - 1; i++) {
|
|
pinMode(wheels[i], OUTPUT);
|
|
}
|
|
|
|
/* Starts serial communication */
|
|
Serial.begin(9600);
|
|
Serial.println("Bot is ready");
|
|
}
|
|
|
|
/* 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();
|
|
delay(move_delay);
|
|
}
|
|
|
|
/* 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();
|
|
delay(move_delay);
|
|
}
|
|
|
|
/* Stops */
|
|
void stop_all() {
|
|
for (int 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);
|
|
}
|
|
|
|
void turn_left_or_right(int delayMS) {
|
|
randomSeed(analogRead(analog_disconnected));
|
|
int rand_number = random(1, 99);
|
|
if (rand_number > 50) {
|
|
turn_right(delayMS);
|
|
} else {
|
|
turn_left(delayMS);
|
|
}
|
|
}
|
|
|
|
/* Ultrasonic sensor actions */
|
|
/* ************************* */
|
|
|
|
/* get_distance -- returns distance to an object in centimeters */
|
|
int get_distance() {
|
|
int mm = hcsr04.distanceInMillimeters();
|
|
if (mm > 0) {
|
|
return mm / 10;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/* Main code */
|
|
/* ********* */
|
|
|
|
/* The idea:
|
|
|
|
If robot see an object in distance less than 25cm,
|
|
he must do the following actions:
|
|
|
|
* stop the wheels
|
|
* turn on wheels to go backward
|
|
* stop wheels
|
|
* turn left or right
|
|
* then go forward
|
|
*/
|
|
void loop() {
|
|
if (get_distance() > 25) {
|
|
move_forward();
|
|
} else {
|
|
stop_all();
|
|
move_backward();
|
|
turn_left_or_right(turn_delay);
|
|
}
|
|
}
|