Дана программа arduino mega для робота, который должен пройти трассу в виде лабиринта, используя ультразвуковые датчики. Все порты проверила
C++:
#include <Servo.h>
#include <NewPing.h>
Servo motor; // Объект для управления мотором
Servo myservo; // Объект для управления сервоприводом
// Пины для ультразвуковых датчиков
#define FRONT_TRIGGER_PIN 13
#define FRONT_ECHO_PIN 12
#define LEFT_TRIGGER_PIN 4
#define LEFT_ECHO_PIN 5
#define RIGHT_TRIGGER_PIN 10
#define RIGHT_ECHO_PIN 11
#define BACK_TRIGGER_PIN 6
#define BACK_ECHO_PIN 7
// Максимальное расстояние для измерения (в сантиметрах)
#define MAX_DISTANCE 300
// Пороговые расстояния для определения препятствий (в сантиметрах)
#define OBSTACLE_FRONT_DISTANCE 20
#define OBSTACLE_SIDE_DISTANCE 30
#define OBSTACLE_BACK_DISTANCE 10
// Объекты для ультразвуковых датчиков
NewPing sonarFront(FRONT_TRIGGER_PIN, FRONT_ECHO_PIN, MAX_DISTANCE);
NewPing sonarLeft(LEFT_TRIGGER_PIN, LEFT_ECHO_PIN, MAX_DISTANCE);
NewPing sonarRight(RIGHT_TRIGGER_PIN, RIGHT_ECHO_PIN, MAX_DISTANCE);
NewPing sonarBack(BACK_TRIGGER_PIN, BACK_ECHO_PIN, MAX_DISTANCE);
void setup() {
// Serial.begin(9600); // Инициализация последовательной связи для вывода данных в монитор порта
motor.attach(8); // Подключение мотора к пину 8
myservo.attach(9); // Подключение сервопривода к пину 9
motor.writeMicroseconds(1500); // Установка мотора в нейтральное положение
myservo.write(90); // Устанавливает сервопривод в положение 90 градусов
delay(1000); // Задержка для стабилизации
}
void loop() {
int distanceFront = sonarFront.ping_cm(); // Измерение расстояния спереди
int distanceLeft = sonarLeft.ping_cm(); // Измерение расстояния слева
int distanceRight = sonarRight.ping_cm(); // Измерение расстояния справа
int distanceBack = sonarBack.ping_cm(); // Измерение расстояния сзади
Serial.print("Front Distance: ");
Serial.print(distanceFront);
Serial.println(" cm");
Serial.print("Left Distance: ");
Serial.print(distanceLeft);
Serial.println(" cm");
Serial.print("Right Distance: ");
Serial.print(distanceRight);
Serial.println(" cm");
Serial.print("Back Distance: ");
Serial.print(distanceBack);
Serial.println(" cm");
// Логика движения в зависимости от обнаруженных препятствий
if (distanceFront > 0 && distanceFront <= OBSTACLE_FRONT_DISTANCE) {
// Обнаружено препятствие спереди, выбираем направление поворота
if (distanceLeft > OBSTACLE_SIDE_DISTANCE && distanceRight > OBSTACLE_SIDE_DISTANCE) {
// Если по бокам свободно, поворачиваем налево или направо случайным образом
if (random(0, 2) == 0) {
turnLeft();
} else {
turnRight();
}
} else if (distanceLeft > OBSTACLE_SIDE_DISTANCE) {
// Если слева свободно, поворачиваем налево
turnLeft();
} else if (distanceRight > OBSTACLE_SIDE_DISTANCE) {
// Если справа свободно, поворачиваем направо
turnRight();
} else {
// Если спереди и по бокам препятствия, поворачиваем назад
turnBack();
}
} else if (distanceBack > 0 && distanceBack <= OBSTACLE_BACK_DISTANCE) {
// Обнаружено препятствие сзади, поворачиваем назад
turnBack();
} else {
// Нет препятствий, двигаемся вперед
moveForward();
}
delay(50); // Небольшая задержка между итерациями цикла
}
// Функция движения вперед
void moveForward() {
motor.writeMicroseconds(1570); // Увеличиваем пульс для движения вперед
}
// Функция движения назад
void moveBackward() {
motor.writeMicroseconds(1400); // Уменьшаем пульс для движения назад
}
// Функция поворота налево
void turnLeft() {
myservo.write(60); // Поворот налево
delay(500); // Задержка для поворота
myservo.write(90); // Возврат в нейтральное положение
}
// Функция поворота направо
void turnRight() {
myservo.write(120); // Поворот направо
delay(500); // Задержка для поворота
myservo.write(90); // Возврат в нейтральное положение
}
// Функция разворота назад
void turnBack() {
myservo.write(180); // Разворот назад
delay(1000); // Задержка для разворота
myservo.write(90); // Возврат в нейтральное положение
}
