Problema con la programmazione dell'RC

Salve,
Sto costruendo un RC in vista degli esami, che dovrebbe essere telecomandata con un modulo Bluetooth HC-05, ma prima volevo cercare di farla andare da sola. Tuttavia, il programma non fa quello che io penso dovrebbe fare, cioè invertire il verso del motore e girare quando incontra un ostacolo, ma non parte affatto o comunque non reagisce e va solo avanti. Potreste aiutarmi a capire dove siano gli errori?
Posto lo sketch:

#include <Servo.h>;
Servo servo;
const int HOnOffPin = 5;
const int HDirezPin2 = 4;
const int HDirezPin7 = 3; //pin del ponte H
int motorOnOff = 1;
int motorSpeed = 0;//variabili per il motore
const int potPin = A0; //potenziometro per la velocità
const int bottoneOnOff = 6;
boolean statoBottone = 0;
boolean precStatoBottone = 0;//bottone per accendere e spegnere il motore
const int trigger = 7;
const int echo = 8;
long tempoSegnale = 0;
long spazio = 0;

void setup() {
  pinMode(HOnOffPin, OUTPUT);
  pinMode(HDirezPin2, OUTPUT);
  pinMode(HDirezPin7, OUTPUT);
  digitalWrite(HOnOffPin, LOW);
  pinMode(bottoneOnOff, INPUT);
  pinMode(trigger, OUTPUT);
  digitalWrite(trigger, LOW);
  pinMode(echo, INPUT);
  servo.attach(9);
  servo.write(54);
  delay(90);
  Serial.begin(9600);
  }

void loop() {
  statoBottone = digitalRead(bottoneOnOff); //leggo lo stato del bottone per accendere o spegnere il motore
  delay(15);//piccola attesa per evitare rimbalzi
  motorSpeed = analogRead(potPin)/4;//leggo il potenziometro per la velocità del motore
  
  if(statoBottone != precStatoBottone){
    if(statoBottone == HIGH){
      motorOnOff = !motorOnOff;
      }
   } //collego lo stato del bottone all'accensione del motore
  
   if(motorOnOff == 1){
     //calcolo la distanza
     digitalWrite(trigger, HIGH);
     delayMicroseconds(10);
     digitalWrite(trigger, LOW);
     long tempoSegnale = pulseIn(echo, HIGH);
     long spazio = 0.034 * tempoSegnale / 2; //la distanza è già in centimetri
            
      if(spazio>=4){
        while(spazio>=4){      
        digitalWrite(trigger, HIGH);
        delayMicroseconds(10);
        digitalWrite(trigger, LOW);
        long tempoSegnale = pulseIn(echo, HIGH);
        long spazio = 0.034 * tempoSegnale / 2;
        servo.write(54);
        delay(90);
        digitalWrite(HDirezPin2, HIGH);
        digitalWrite(HDirezPin7, LOW);
        analogWrite(HOnOffPin, motorSpeed);
        }
      }
      else{
        while(spazio<4){
        digitalWrite(trigger, HIGH);
        delayMicroseconds(10);
        digitalWrite(trigger, LOW);
        long tempoSegnale = pulseIn(echo, HIGH);
        long spazio = 0.034 * tempoSegnale / 2;  
        servo.write(75);
        delay(90);
        digitalWrite(HDirezPin2, LOW);
        digitalWrite(HDirezPin7, HIGH);
        analogWrite(HOnOffPin, motorSpeed);
        }
      }
   }
   else{
     digitalWrite(HDirezPin2, LOW);
     digitalWrite(HDirezPin7, LOW);
     analogWrite(HOnOffPin, 0);
     }
 
 precStatoBottone = statoBottone;//memorizzo lo stato del bottone
 Serial.print("Il bottone è: ");
 Serial.println(statoBottone);
 Serial.print("velocità del motore: ");
 Serial.println(motorSpeed);
 Serial.print("La distanza è: ");
 Serial.println(spazio);
 delay(120); //delay da eliminare quando l'RC sarà autonoma dal pc grazie ad una alimentazione esterna
}

Aiutarti così la vedo dura non credo che si riesca a capire cosa possa non funzionare ti conviene mettere dei log sulla seriale per capire innanzitutto se il calcolo della distanza dell'oggetto lo esegue correttamente...insomma devi fare una sorta di debug da solo sorry