Ошибка поворота робота, решающего лабиринт, из-за увеличения расстояния датчика до угла во время поворота
В моей школе есть проект, в котором мы должны создать робота, который может пройти трехмерный лабиринт, используя робота, управляемого Arduino, который использует ультразвуковые датчики для определения направления поворота. Робот не может быть предварительно запрограммирован и должен работать во всех простых лабиринтах с 90d поворотами.
Я разобрался с большей частью кода, но моя проблема в том, что когда роботу нужно повернуться. Поскольку то, насколько и в каком направлении робот должен повернуться, определяется датчиками расстояния, расстояние до углов увеличивается, что делает его длиннее, чем требуется для поворота. Это заставляет робота хотеть вернуться в угол, а затем он застревает в петле поворота и выхода из угла (см. прикрепленное изображение).
Примечание: мы не хотим программировать поворот на 90 d, так как позже это может привести к сбоям, и мы считаем, что лучше попытаться определить, как долго робот должен поворачиваться, исходя из показаний различных датчиков.
Робот состоит из трех ультразвуковых датчиков: спереди, справа и слева. У робота есть гусеницы, которые позволяют ему разворачиваться на месте. Arduino — это Arduino Mega, поэтому он быстро обновляет и измеряет значения (если только время, необходимое для измерения расстояния, не велико).
Файлы:
Изображения, описывающие проблему с поворотом:
Изображения робота:
Если нужна какая-либо другая информация, которая не указана, я постараюсь ее получить (например, изображения, размеры, точные названия компонентов и т. д.)
Весь код:
enum robState {
robInit,
robInitForward,
robInitRotL,
robInitRotR,
robStop,
// ограбитьWaitCrystals,
robInitBack,
robReset,
robFindWay
};
struct LFR {
int L;
int F;
int R;
};
struct robMotor {
byte pwm;
byte dir_1;
byte dir_2;
};
robState robStatus;
//const int robTurnSpeed = 255;
robMotor motorL = {2, 4, 3};
robMotor motorR = {5, 6, 7};
int dirInt = 1;
LFR echoPin = {12, 11, 13};
LFR trigPin = {24, 26, 22};
LFR distance;
const int robStartStopPin;
void setup() {
// поместите сюда код установки для однократного запуска:
// инициализируем пин-режимы
initPins();
Serial.begin(9600);
robStatus = robInit;
}
void loop() {
int disWall = 12;
// поместите сюда ваш основной код для многократного запуска:
distance.L = distanceUpdater(trigPin.L, echoPin.L);
distance.F = distanceUpdater(trigPin.F, echoPin.F);
distance.R = distanceUpdater(trigPin.R, echoPin.R);
Serial.print("/n distance.L= ");
Serial.println(distance.L);
Serial.print("/n distance.F= ");
Serial.println(distance.F);
Serial.print("/n distance.R= ");
Serial.println(distance.R);
// задержка в микросекундах (50);
switch (robStatus) {
case robInit:
// инициализируем моторы и прочее
motor(0, 0);
motor(1, 0);
// ждем старт/стоп
//пока (digitalRead(robStartStopPin) == LOW);
Serial.print("/n robInit");
robStatus = robFindWay;
break;
case robInitForward:
motor(0, 1);
motor(1, 1);
Serial.print("/n robInitForward");
robStatus = robFindWay;
break;
case robFindWay:
Serial.print("/n robFindWay");
if (distance.R > disWall) {
//если открыть направо, повернуть направо.
robStatus = robInitRotR;
}
else if ((distance.R < disWall) && (distance.F > disWall)) {
//идти вперед, если не можешь пойти направо
robStatus = robInitForward;
}
else if ((distance.R < disWall) && (distance.F < disWall) && (distance.L > disWall)) {
//если больше ничего не работает, повернуть налево.
robStatus = robInitRotL;
}
else if ((distance.R < disWall) && (distance.F < disWall) && (distance.L < disWall)) {
//нашел конец, надо повернуть. поворачивает налево.
robStatus = robInitRotL;
}
else {
Serial.print("shit somethings wrong with path finding");
}
break;
case robInitRotL:
Serial.print("robInitRotL");
//прямое направление мотораR
motor(1, 1);
//обратное направление мотораL
motor(0, -1);
//отправить правильную скорость двигателя*/
robStatus = robFindWay;
/*analogWrite(motorR.pwm, robTurnSpeed);
analogWrite(motorL.pwm, robTurnSpeed);*/
break;
case robInitRotR:
Serial.print("robInitRotR");
//обратное направление двигателя R
motor(1, -1);
//прямое направление мотораL
motor(0, 1);
//отправляем правильную скорость двигателя
robStatus = robFindWay;
/*analogWrite(motorR.pwm, robTurnSpeed);
analogWrite(motorL.pwm, robTurnSpeed);*/
break;
case robStop:
motor(0, 0);
motor(1, 0);
//пока (digitalRead(robStartStopPin) == LOW);
robStatus = robInit;
break;
case robInitBack:
// поменять местами оба мотора
motor(0, -1);
motor(1, -1);
break;
}
}
void initPins() {
// инициализируем двигатели
pinMode(motorL.pwm, OUTPUT);
pinMode(motorL.dir_1, OUTPUT);
pinMode(motorL.dir_2, OUTPUT);
pinMode(motorR.pwm, OUTPUT);
pinMode(motorR.dir_1, OUTPUT);
pinMode(motorR.dir_2, OUTPUT);
// инициализировать датчики
pinMode(trigPin.L, OUTPUT);
pinMode(trigPin.F, OUTPUT);
pinMode(trigPin.R, OUTPUT);
pinMode(echoPin.L, INPUT);
pinMode(echoPin.F, INPUT);
pinMode(echoPin.R, INPUT);
//кнопка инициализации
pinMode(robStartStopPin, INPUT);
//инициируем источники питания
/*pinMode(VCCPin1, OUTPUT);
digitalWrite(VCCPin1, HIGH);
pinMode(GNDPin1, OUTPUT);
digitalWrite(GNDPin1, LOW);*/
}
void motor(int side, int action) {
//сторона 0=Л 1=П
//действие -1=назад 0=неподвижно 1=вперед
int normalSpeed = 255;
if ((side == 0) && (action == -1)) {
//повернуть налево
digitalWrite(motorL.dir_1, LOW);
digitalWrite(motorL.dir_2, HIGH);
analogWrite(motorL.pwm, normalSpeed);
}
else if ((side == 0) && (action == 0)) {
//еще осталось
digitalWrite(motorL.dir_1, HIGH);
digitalWrite(motorL.dir_2, LOW);
analogWrite(motorL.pwm, 0);
}
else if ((side == 0) && (action == 1)) {
//вперед налево
digitalWrite(motorL.dir_1, HIGH);
digitalWrite(motorL.dir_2, LOW);
analogWrite(motorL.pwm, normalSpeed);
}
else if ((side == 1) && (action == -1)) {
//повернуть направо
digitalWrite(motorR.dir_1, LOW);
digitalWrite(motorR.dir_2, HIGH);
analogWrite(motorR.pwm, normalSpeed);
}
else if ((side == 1) && (action == 0)) {
//по-прежнему верно
digitalWrite(motorR.dir_1, HIGH);
digitalWrite(motorR.dir_2, LOW);
analogWrite(motorR.pwm, 0);
}
else if ((side == 1) && (action == 1)) {
//вперед вправо
digitalWrite(motorR.dir_1, HIGH);
digitalWrite(motorR.dir_2, LOW);
analogWrite(motorR.pwm, normalSpeed);
}
else {
Serial.print("monkaS wrong motor inputs");
}
}
int distanceUpdater(int trigPin, int echoPin) {
long duration;
int distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration * 0.034 / 2);
return distance;
}
Поиск пути и поворот:
switch (robStatus) {
case robInit:
// инициализируем моторы и прочее
motor(0, 0);
motor(1, 0);
// ждем старт/стоп
//пока (digitalRead(robStartStopPin) == LOW);
Serial.print("/n robInit");
robStatus = robFindWay;
break;
case robInitForward:
motor(0, 1);
motor(1, 1);
Serial.print("/n robInitForward");
robStatus = robFindWay;
break;
case robFindWay:
Serial.print("/n robFindWay");
if (distance.R > disWall) {
//если открыть направо, повернуть направо.
robStatus = robInitRotR;
}
else if ((distance.R < disWall) && (distance.F > disWall)) {
//идти вперед, если не можешь пойти направо
robStatus = robInitForward;
}
else if ((distance.R < disWall) && (distance.F < disWall) && (distance.L > disWall)) {
//если больше ничего не работает, повернуть налево.
robStatus = robInitRotL;
}
else if ((distance.R < disWall) && (distance.F < disWall) && (distance.L < disWall)) {
//нашел конец, надо повернуть. поворачивает налево.
robStatus = robInitRotL;
}
else {
Serial.print("shit somethings wrong with path finding");
}
break;
case robInitRotL:
Serial.print("robInitRotL");
//прямое направление мотораR
motor(1, 1);
//обратное направление мотораL
motor(0, -1);
//отправить правильную скорость двигателя*/
robStatus = robFindWay;
/*analogWrite(motorR.pwm, robTurnSpeed);
analogWrite(motorL.pwm, robTurnSpeed);*/
break;
case robInitRotR:
Serial.print("robInitRotR");
//обратное направление двигателя R
motor(1, -1);
//прямое направление мотораL
motor(0, 1);
//отправляем правильную скорость двигателя
robStatus = robFindWay;
/*analogWrite(motorR.pwm, robTurnSpeed);
analogWrite(motorL.pwm, robTurnSpeed);*/
break;
case robStop:
motor(0, 0);
motor(1, 0);
//пока (digitalRead(robStartStopPin) == LOW);
robStatus = robInit;
break;
case robInitBack:
// поменять местами оба мотора
motor(0, -1);
motor(1, -1);
break;
}
@, 👍2
2 ответа
Ваш основной цикл (loop()) непрерывно обрабатывает весь код. Было бы неплохо сделать несколько «перерывов» между каждым циклом сканирования сенсора. Например:
long timerCounter = 0;
void setup()
{
// Выполните настройку здесь
timerCounter = millis();
}
void loop()
{
// Делаем какой-то код только раз в 1 секунду (1000 мс)
if (millis() - timerCounter >= 1000)
{
ScanSensor1();
ScanSensor2();
ScanSensor3();
robState state = DetermineStateFromSensorReadings();
DoActionForState(state);
}
}
Это разбивает цикл до 1 Гц (изменяйте по своему усмотрению). Когда вы используете ультразвук (согласно вашим изображениям), всегда полезно уменьшить интервалы между каждым чтением, так как это позволяет уменьшить остаточный шум. Представьте себе звуковой сигнал гидролокатора на подводной лодке.
Вы получили правильное представление об использовании конечного автомата в своем коде. Каждый цикл может определить, в каком состоянии он должен находиться после считывания данных датчика. Хорошая работа!
Небольшая дополнительная информация об ультразвуке относительно вашего проекта:
- Сонар имеет угол излучения 30 градусов (не прямая линия!). Представьте код, указывающий под углом 30 градусов. Это означает, что все, что находится внутри этого диапазона, может быть обнаружено датчиком. Глядя на ваше изображение проблемы с поворотом 2, я ожидаю много ошибочных данных от этого датчика, указывающего на угол.
- Несколько датчиков, работающих на одной частоте (40 кГц?), могут создавать помехи друг другу.
Проблема, которую вы видите, в основном вызвана тем, что вы проверяете стены вокруг себя во время поворота. Я бы посоветовал вам использовать датчик, обращенный вперед, или датчик, противоположный тому, что используется при переворачивании проблемного изображения 2, чтобы значительно уменьшить ошибочные данные.
Если позволяет ваш бюджет (и время), вы можете воспользоваться дешевым лидаром (например, этим). ) для более точного определения вашего окружения. Лидар не имеет угла излучения, что означает, что его точка измерения находится на прямой линии от источника.
Вам нужно два состояния. Делать поворот и не делать поворот.
Как только вы начнете поворачивать, игнорируйте боковые датчики и продолжайте поворачивать, пока расстояние до переднего датчика не достигнет максимального значения.
- Вычисление отклонения от курса по магнитометру и акселерометру
- Как определить, когда выходной сигнал датчика значительно меняется?
- Какова работа pulseIn?
- Сколько датчиков может поддерживать один модуль Arduino?
- Получение BPM из данного кода
- DS18B20 дает высокие показания. Как заставить его вернуть правильную температуру?
- Какой тип разъема использует система GROVE?
- Улавливают ли ультразвуковые датчики прозрачные материалы?
Автономные роботы — это очень весело (и сложно!) Прямо сейчас ваш робот делает именно то, что вы ему сказали — следовать за стеной. Так или иначе вам придется «запрограммировать поворот на 90 градусов». Сложный способ сделать это — смоделировать поворот с точки зрения ожидаемого профиля расстояния в зависимости от времени и постоянно корректировать смещение цели робота для плавного прохождения поворота. Простым способом было бы позволить роботу распознать необходимость поворота, выполнить его, а затем переоценить ситуацию. FMI, см. категорию «роботы» на https://www.fpaynter.com/, @user3765883