Primo abbozzo di macchina (consigli e suggerimenti)

Salve,
Sono ancora alle prime armi ma ho cercato di cimentarmi nella progettazione di una “macchina”: guidata da un motore DC a 6-9V collegato a 2 ruote (controllato con un ponte H ed un potenziometro per la velocità), un servo-motore anteriore collegato ad una ruota per dare la direzione, un sensore HC-SR04 per la rivelazione di eventuali ostacoli sul percorso, infine un bottone per accendere o spegnere. So che magari un sensore è poco (come pure un solo motore), però questo è più una cosa su cui cimentarmi fine a sé stessa. Posto lo sketch e mi piacerebbe sapere se può funzionare, se è fatto bene, come potrei renderlo più “funzionale”, aggiungendo o togliendo qualcosa (specialmente per i sensori).

Grazie in anticipo!

#include <Servo.h>;
Servo servo; //servo-motore
const int HOnOffPin = 3;
const int HDirezPin2 = 4;
const int HDirezPin7 = 5; //pin del ponte H
int motorOnOff = 0;
int motorSpeed = 0;
int motorDirez = 1; //variabili per il motore
const int trigger = 12;
const int echo = 13; // sensore ad ultrasuoni HC-SR04
const int potPin = A0; //potenziometro per la velocità
const int bottoneOnOff = 11;
int statoBottone = 0;
int precStatoBottone = 0; //bottone per accendere e spegnere il motore

void setup() {
  servo.attach(9);
  pinMode(HOnOffPin, OUTPUT);
  pinMode(HDirezPin2, OUTPUT);
  pinMode(HDirezPin7, OUTPUT);
  digitalWrite(HOnOffPin, LOW);
  pinMode(trigger, OUTPUT);
  digitalWrite(trigger, LOW);
  pinMode(echo, OUTPUT);
  pinMode(bottoneOnOff, INPUT);
  }

void loop() {
  servo.write(90); //imposto il servo per andare dritto (se dovesse aver girato in precedenza per un ostacolo, ora torna in posizione)
  delay(80);//piccola attesa per il servo
  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
  
  digitalWrite(trigger, HIGH);//faccio partire il sensore per rilevare la distanza 
  delayMicroseconds(10);
  digitalWrite(trigger, LOW);
  long tempoSegnale = pulseIn(echo, HIGH);
  long spazio = 0.034*tempoSegnale/2;//ho rilevato la distanza in cm
  delay(10);
  /*Dopo aver rivelato la distanza, la uso per far capire ad Arduino
  se deve andare dritto o tornare indietro e girare*/
  if(motorOnOff == 1){
    if(spazio >= 4){//se lo spazio è maggiore di 4 cm, Arduino fa procedere la "macchina"
      digitalWrite(HDirezPin2, HIGH);
      digitalWrite(HDirezPin7, LOW);//verso orario del motore
      analogWrite(HOnOffPin, motorSpeed);
      }
    else{     /*se lo spazio è inferiore a 4cm, Arduino si ferma e torna indietro
                fino a quando la distanza non torna nei parametri*/
      while(spazio >= 4){
       digitalWrite(HDirezPin2, LOW);
       digitalWrite(HDirezPin7, HIGH);//verso anti-orario del motore
       analogWrite(HOnOffPin, motorSpeed);
       }
      servo.write(25);//Dopo essere tornato indietro abbastanza, il servo gira per curvare ed evitare l'ostacolo
      delay(90);
     }
   }
 precStatoBottone = statoBottone;//memorizzo lo stato del bottone
}

Ad una prima occhiata io ti consiglierei di usare i tipi di variabile in modo coerente. Un intero per mantenere in memoria uno stato booleano ha poco senso. Identifica il numero di livelli che devi rappresentare con la singola variabile e scegli un tipo appropriato.