Направление тележки (двигателя) с использованием 2 ультразвуковых датчиков расстояния

Я застрял, пытаясь создать код для тележки. В прошлом у меня был код, в котором для выполнения задачи использовались герконы, но из-за шума на дорожке они не работали надежно. Поэтому я хочу использовать 2 ультразвуковых датчика для выполнения задачи. Программа будет считывать входные данные двух разных ультразвуковых датчиков для управления направлением и скоростью тележки. У меня будет ультразвуковой датчик на каждом конце пути, и я хочу, чтобы тележка преформировалась следующим образом:

Начать движение на восток на 100 % при запуске программы (включено питание) Замедлить до 75% скорости на расстоянии 20 см от сеньора 1 Замедлить до 50% скорости на расстоянии 15 см от датчика 1 Замедлить до 25% скорости на расстоянии 10 см от датчика 1 Остановитесь на расстоянии 5 см от датчика 1. оставаться остановленным в течение случайного времени от 2 до 5 секунд Начинайте движение на запад со скоростью 25% до расстояния 10 см от датчика 1. увеличьте скорость до 50% на расстоянии 15 см от датчика 1 увеличить скорость до 75% на расстоянии 20 см от датчика 1 увеличить скорость до 100% на расстоянии 25 см от датчика 1 Оставайтесь на 100% скорости Замедлить до 75% скорости на расстоянии 20 см от сеньора 2 Замедлить до 50% скорости на расстоянии 15 см от датчика 2 Замедлить до 25% скорости на расстоянии 10 см от датчика 2 Остановитесь на расстоянии 5 см от датчика 2. оставаться остановленным в течение случайного времени от 2 до 5 секунд повторять неограниченное время (пока не отключат питание) Двигатель будет управляться контроллером двигателя TB6612FNG (будет использоваться только один двигатель). Я все еще работаю на макетной плате, поэтому можно использовать любые входные/выходные контакты на плате (arduino nano).

Ниже приведен код, который у меня есть. Восточный датчик, кажется, работает (у меня пока нет всех случаев замедления), но западный датчик возвращает 0 расстояний и сразу же переключается обратно на случай движения на восток. Я не могу понять, почему он сразу возвращает 0. Будем признательны за любую помощь.

#include <SparkFun_TB6612.h> //библиотека контроллера двигателя

int trigPinE = 6;     // триггерный датчик расстояния Восток
int echoPinE = 7;     // эхо-контакт Восточный датчик расстояния
int trigPinW = 10;      // триггер западного датчика расстояния
int echoPinW = 11;     // эхо-контакт Западный датчик расстояния
int drive; // определяет константу для прямого или обратного сигнала
int travelSpeed; // определяет константу скорости движения тележки

// Определяем контакты, используемые для tb6612fng
#define AIN1 4
#define AIN2 3
#define PWMA 2
#define STBY 5

long durationE, distanceE;
long durationW, distanceW;

const int offsetA = -1; // константы для конфигурации двигателя Значение может быть 1 или -1


Motor motor1 = Motor(AIN1, AIN2, PWMA, offsetA, STBY); // Инициализация моторов

void setup() {

  Serial.begin(9600);

  pinMode(trigPinE, OUTPUT);         // устанавливаем триггер датчика Востока в качестве выхода

  pinMode(echoPinE, INPUT);          //установить эхо-вывод датчика Востока в качестве входа для захвата отраженных волн

  pinMode(trigPinW, OUTPUT);         // устанавливаем триггер датчика West в качестве выхода

  pinMode(echoPinW, INPUT);          //установить эхо-вывод датчика West в качестве входа для захвата отраженных волн

  travelSpeed = 200;
  motor1.drive(travelSpeed); //пускает тележку в прямом направлении
  drive=1;
}


void loop() {
 //Код ниже******Вагонетка движется на восток – используется датчик Востока******
  if (drive > 0)  // состояние, если тележка движется на восток - будет считываться восточный датчик
    {
  digitalWrite(trigPinE, LOW);

  delayMicroseconds(2);   

  digitalWrite(trigPinE, HIGH);     // отправляем волны на 10 мкс

  delayMicroseconds(10);

  durationE = pulseIn(echoPinE, HIGH); // принимаем отраженные волны

  distanceE = durationE / 58.2;   // преобразовать в расстояние

  Serial.println("East distance sensor distance ");
  Serial.println(distanceE);
  delay(500);
    if(distanceE>=20)
    {
      Serial.println("full speed");
      motor1.drive(200);
    }
    if ((distanceE>=10)&&(distanceE<=20)) // выражение if для замедления тележки между расстоянием от 10 до 20
    {
      Serial.println("slowing down to 50% power");
      motor1.drive(travelSpeed/2);
      delay(500);
    }
    if (distanceE<10)
    {
      Serial.println("Stopping");
      travelSpeed=0;
      motor1.drive(travelSpeed);
      Serial.println("Stopped");
      delay(random(500,2000));   // задержка на случайное время
      Serial.println("Change direction");
      drive=-1;
    }
    }
//Код выше******Вагонетка движется на восток – используется датчик Востока******
//Код ниже******Вагонетка движется на запад – используется западный датчик******

if (drive < 0)  // состояние, если тележка движется на запад - будет считывать датчик запада
    {
  digitalWrite(trigPinW, LOW);

  delayMicroseconds(2);   

  digitalWrite(trigPinW, HIGH);     // отправляем волны на 10 мкс

  delayMicroseconds(10);

  durationW = pulseIn(echoPinW, HIGH); // принимаем отраженные волны

  distanceW = durationW / 58.2;   // преобразовать в расстояние

  Serial.println("West distance sensor distance ");
  Serial.println(distanceW);

  if(distanceW>=20)
    {
      Serial.println("full speed");
      motor1.drive(200);
    }
  
    if ((distanceW>=10)&&(distanceW<=20)) // выражение if для замедления тележки между расстоянием от 10 до 20
    {
      Serial.println("slowing down to 50% power");
      motor1.drive(travelSpeed/2);
      delay(500);
    }
    if (distanceW<10)
    {
      Serial.println("Stopping");
      travelSpeed=0;
      motor1.drive(travelSpeed);
      Serial.println("Stopped");
      delay(random(500,2000));   // задержка на случайное время
      Serial.println("Change direction");
      drive=1;
    }
    }

 //Код выше******Вагонетка движется на запад - используется западный датчик******
  }