Ошибка поворота робота, решающего лабиринт, из-за увеличения расстояния датчика до угла во время поворота

sensors algorithm

В моей школе есть проект, в котором мы должны создать робота, который может пройти трехмерный лабиринт, используя робота, управляемого 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;
  }

long timerCounter = 0;

void setup()
{
    // Выполните настройку здесь

    timerCounter = millis();
}

void loop()
{
    // Делаем какой-то код только раз в 1 секунду (1000 мс)
    if (millis() - timerCounter >= 1000)
    {
        ScanSensor1();
        ScanSensor2();
        ScanSensor3();

        robState state = DetermineStateFromSensorReadings();

        DoActionForState(state);
    }
}