Код, похоже, адекватный.
Может с чего датчиками не так? Если от программы не зависит, сталобыть аппаратно шшупать нужно.
Кстати, полезно было бы поправить код так, и посмотреть, что выводится во время пересечения границы.
// Подключение модуля ультрозвукового сенсора
#include <Ultrasonic.h>
//объявление глобальных переменных
int L_Sensor=0; //Левый сенсор
int R_Sensor=0; //Правый сенсор
float dist_cm=0; // Значение расстояния
Ultrasonic ultrasonic(4, 2); // Назначение портов УЗ сенсора. Trig, Echo
void setup()// Модуль инициализации
{
pinMode( 15 , INPUT); // Вход правого сенсора
pinMode( 17 , INPUT); // Вход левого сенсора
pinMode(3, OUTPUT); //B-B Назад - Левый мотор
pinMode(5, OUTPUT); //B-1A Вперед - Левый мотор
pinMode(6, OUTPUT); //A-B Назад - Правый мотор
pinMode(9, OUTPUT); //A-1A Вперед - Правый мотор
Serial.begin(9600); // Активация передачи данных через терминал
delay(5000); // Задержка старта 5 сек.
}
void go_forward() //Атакуем движение вперед
{
analogWrite(3, 0); //LEFT MOTOR
analogWrite(5, 255); //LEFT MOTOR
analogWrite(6, 0); //RIGHT MOTOR
analogWrite(9, 255); //RIGHT MOTOR
}
void go_left() // поиск цели или движение налево
{
analogWrite(3, 0); //LEFT MOTOR
analogWrite(5, 100); //LEFT MOTOR
analogWrite(6, 100); //RIGHT MOTOR
analogWrite(9, 0); //RIGHT MOTOR
}
void go_back () //двидение назад
{
analogWrite(3, 255); //LEFT MOTOR
analogWrite(5, 0); //LEFT MOTOR
analogWrite(6, 255); //RIGHT MOTOR
analogWrite(9, 0); //RIGHT MOTOR
}
void go_right () //поиск цели или движение направо
{
analogWrite(3, 100); //LEFT MOTOR
analogWrite(5, 0); //LEFT MOTOR
analogWrite(6, 0); //RIGHT MOTOR
analogWrite(9, 100); //RIGHT MOTOR
}
void go_stop() //остановка
{
analogWrite(3, 255); //LEFT MOTOR
analogWrite(5, 255); //LEFT MOTOR
analogWrite(6, 255); //RIGHT MOTOR
analogWrite(9, 255); //RIGHT MOTOR
}
void check_sensor() // Подпрограмма проверки сенсоров.
{
R_Sensor=analogRead(15); // считываем показания с правого сенсора
delay(10); // Ожидание преобразования АЦП
L_Sensor=analogRead(17);// считываем показания с левого сенсора
delay(10); // Ожидание преобразования АЦП
dist_cm = ultrasonic.Ranging(CM);
delay(10);// Ожидание преобразования
}
void display(const char* text) {
Serial.print(text);
Serial.print(" R_Sensor:"); Serial.print(R_Sensor);
Serial.print(" L_Sensor:"); Serial.print(L_Sensor);
Serial.print(" dist_cm:"); Serial.println(dist_cm);
}
void loop()
{
check_sensor(); // Проверяем сенсоры
if (R_Sensor<100 and L_Sensor<100)
{
display("WALK"); // <----------------------------------------------
if (dist_cm>40) //Если дистанция до объекта больше 40 см то
{
display("SEARCH"); // <----------------------------------------------
go_left(); // поиск цели
}
else
{
display("PUSH"); // <----------------------------------------------
go_forward(); //если поймана цель атакуем
}
}
else
{
display("STOP&TURN"); // <----------------------------------------------
go_stop(); //Стоп!
delay(100); //Джем 100 мс
go_back(); //Движемся назад
delay(1000); //1 секунду.
go_right(); //Поворачиваем направо
delay(300); //300 мс
go_forward(); //Движемся вперед
delay(300); //300 мс
}
}
Вход
Регистрация
FAQ
Поиск
