Запускаю робота с двумя передними датчиками, но он тормозит

Вкратце: у меня есть небольшой робот с четырьмя серводвигателями непрерывного действия и двумя ультразвуковыми датчиками. Он управляется платой UNO с сенсорным экраном V5 сверху. Моя цель состоит в том, чтобы запрограммировать передний правый и передний левый датчики так, чтобы оба возвращали расстояние в дюймах, и заставить робота поворачиваться в направлении с наибольшим пространством, когда один из датчиков возвращает расстояние меньше некоторого минимума. Я совершенно уверен, что проблема не в двигателях и датчиках, так как их проверка сама по себе не выявила проблем. Однако, когда я запускаю приведенный ниже код, робот заикается, в основном просто двигаясь назад, но заикаясь вперед и иногда поворачиваясь на секунду или две. Есть идеи, что происходит?

#include <Servo.h>

Servo FL;
Servo FR;
Servo BL;
Servo BR;

const int FRecho = 11;
const int FRtrig = 10;
const int FLecho = 9;
const int FLtrig = 8;

long durationFL;
long durationFR;
int distanceInchR = 24;
int distanceCmR;
int distanceInchL = 24;
int distanceCmL;

const int minDistanceInch = 18;
const int minDistanceCm;

enum movementStates {
 STILL,
 FORWARD,
 BACK,
 TURNFR,
 TURNFL,
 NOTHING
};

enum movementStates movement;


void setup() {

  FL.attach(4);
  FR.attach(5);
  BL.attach(6);
  BR.attach(7);

  pinMode(FRtrig, OUTPUT);
  pinMode(FRecho, INPUT);
 pinMode(FLtrig, OUTPUT);
  pinMode(FLecho, INPUT);

  movement = NOTHING;

  forward();

}

void loop() {

updateDistanceFL();
updateDistanceFR();

updateDirection();

}


void forward () {
  FL.write(180);
  FR.write(0);
  BL.write(180);
  BR.write(0);
  movement = FORWARD;
}

void turnFR () {  
  FL.write(180);
  FR.write(180);
  BL.write(180);
  BR.write(180);
  movement = TURNFR;
}

void turnFL () {
  FL.write(0);
  FR.write(0);
  BL.write(0);
  BR.write(0);
  movement = TURNFL;
}

void backUp () {
  FL.write(0);
  FR.write(180);
  BL.write(0);
  BR.write(180);
  movement = BACK;
}

void still () {
  FL.write(90);
  FR.write(90);
  BL.write(90);
  BR.write(90);
  movement = STILL;
}

void updateDirection() {

  if (distanceInchR < minDistanceInch && distanceInchL < minDistanceInch) {
    if (movement != BACK) {
      backUp();
    }
    else if (distanceInchR < minDistanceInch) {
     if (movement != TURNFL) {
      turnFL();
     }
    }
    else if (distanceInchL < minDistanceInch) {
     if (movement != TURNFR) {
      turnFR();
     }
    }
    else if (distanceInchR > minDistanceInch && distanceInchL > minDistanceInch)
    if (movement != FORWARD) {
      forward();
    }
  }
    else {
      if (movement != STILL) {
        still();
      }
    }
}



void updateDistanceFR(){
digitalWrite(FRtrig, LOW);
delayMicroseconds(2);
digitalWrite(FRtrig, HIGH);
delayMicroseconds(10);
digitalWrite(FRtrig, LOW);
durationFR = pulseIn(FRecho, HIGH);
distanceCmR= durationFR*0.034/2;
distanceInchR = durationFR*0.0133/2;
  }

void updateDistanceFL(){
digitalWrite(FLtrig, LOW);
delayMicroseconds(2);
digitalWrite(FLtrig, HIGH);
delayMicroseconds(10);
digitalWrite(FLtrig, LOW);
durationFL = pulseIn(FLecho, HIGH);
distanceCmL= durationFL*0.034/2;
distanceInchL = durationFL*0.0133/2;
  }