Problemas programación grúa torre

Hola a todos.
Para resumir, estoy realizando un proyecto de control remoto de una grúa torre. Comunico mediante bluetooth con una app de mit app inventor en la que activo los motores, y recibo los valores de un sensor de distancia, uno de peso, y dos finales de carrera. Podrías darme recomendaciones para mejorar el código, o decirme si hay algo incorrecto? Soy nuevo en esto y se agradece cualquier comentario. Saludos


//Bibliotecas
#include <NewPing.h>
#include "HX711.h"

//Configuración de los pines
#define TRIGGER_PIN1  32
#define ECHO_PIN1     33
#define MAX_DISTANCE 400

#define pwmGrua 4
#define giroDchaGrua  46
#define giroIzqGrua  45
#define ledGrua  38
 
#define pwmCarro  3
#define giroDchaCarro  25
#define giroIzqCarro  24
#define ledCarro   39

#define pwmCable  2
#define giroDchaCable  22
#define giroIzqCable  23
#define ledCable  44

//Declaración del sensor de ultrasonidos
int uS1 = 0;
/*Crear el objeto de la clase NewPing*/
NewPing sonar1(TRIGGER_PIN1, ECHO_PIN1, MAX_DISTANCE);


// Pin de datos y de reloj de la celda de carga
byte pinData = A1;
byte pinClk = A0;
//Variables de la celda de carga
float peso;
float pesogr;
HX711 bascula;
// Parámetro para calibrar el peso y el sensor
float factor_calibracion = 58000.0; //Este valor del factor de calibración funciona para mi. El tuyo probablemente será diferente.

//Variables de accionamiento y control de motores
String cadena, motor, dato;
int velocidad;
//Variable de encendido y apagado de programa
int onOff = 0;

//La cadena llega en formato: "Letra número". 
//Letra identifica el motor y sentido, número la velocidad
void partirCadena(){ 
  if (cadena.indexOf(" ") >=0){
    motor = cadena.substring(0, (cadena.indexOf(" ")));
    dato = cadena.substring(cadena.indexOf(" ")+1); 
  }
}

//Función para encendido de leds
void Leds(int seleccion){
  if (seleccion == 1){
    digitalWrite(ledGrua, HIGH);
    digitalWrite(ledCarro, LOW);
    digitalWrite(ledCable, LOW);    
  }
  else if(seleccion == 2){
    digitalWrite(ledGrua, LOW);
    digitalWrite(ledCarro, HIGH);
    digitalWrite(ledCable, LOW); 
  }
  else if(seleccion == 3){
    digitalWrite(ledGrua, LOW);
    digitalWrite(ledCarro, LOW);
    digitalWrite(ledCable, HIGH);     
  }
  else{
    digitalWrite(ledGrua, LOW);
    digitalWrite(ledCarro, LOW);
    digitalWrite(ledCable, LOW);     
  }
}
//Función para controlar un motor, indicando dirección y velocidad
void controlMotor(int velocidad, int direccionON, int direccionOFF, int pwm ){
    digitalWrite(direccionOFF, LOW);
    delay(500);
    digitalWrite(direccionON, HIGH);
    analogWrite(pwm, velocidad);
}

//Función de inicialización
void setup() {
  Serial.begin(9600);
  Serial3.begin(9600);
  pinMode(pwmGrua, OUTPUT);
  pinMode(pwmCarro, OUTPUT);
  pinMode(pwmCable, OUTPUT);
  pinMode(giroDchaGrua, OUTPUT);
  pinMode(giroIzqGrua, OUTPUT);
  pinMode(giroDchaCarro, OUTPUT);
  pinMode(giroIzqCarro, OUTPUT);  
  pinMode(giroDchaCable, OUTPUT);
  pinMode(giroIzqCable, OUTPUT);    
   // Iniciar sensor de peso
  bascula.begin(pinData, pinClk);
 
  // Aplicar la calibración
  bascula.set_scale();
  // Iniciar la tara
  // No tiene que haber nada sobre el peso
  bascula.tare();
  // Obtener una lectura de referencia
  long zero_factor = bascula.read_average();
  //Leds apagados
  Leds(0); 
}
//Programa
void loop() {
  //Leer puerto Bluetooth
  //Si está disponbile, leer datos
      
  if (Serial3.available()){
    cadena = String("");
    while(Serial3.available()){
      cadena = cadena + char(Serial3.read());
      delay(1);
    }
    //Separar datos y obtener valores
    partirCadena();
    velocidad = dato.toInt();
    Serial.println(cadena);
  //Apagar programa
    if (motor == "I"){
      onOff = 0;
      Serial.println(onOff);     
    }
    //Encender programa
    else if (motor == "H"){
      onOff = 1;
      Serial.println(onOff);
    }    
    //Si no hay ningún motor activado o está apagado el programa, se apagan los motores
    else if (motor == "A" or onOff == 0){   
      digitalWrite(giroDchaGrua, LOW);
      digitalWrite(giroIzqGrua, LOW);
      analogWrite(pwmGrua, 0);
      digitalWrite(giroDchaCarro, LOW);
      digitalWrite(giroIzqCarro, LOW);
      analogWrite(pwmCarro, 0);
      digitalWrite(giroDchaCable, LOW);
      digitalWrite(giroIzqCable, LOW);
      analogWrite(pwmCable, 0);
      Leds(0);   
      Serial.println(onOff);    
    }
    //Encender motores en función del que se active si el programa está encendido
    else if (motor == "B" && onOff == 1){
      controlMotor(velocidad, giroIzqGrua, giroDchaGrua, pwmGrua);
      Leds(1);
    }
    else if (motor == "C" && onOff == 1){
      controlMotor(velocidad, giroDchaGrua, giroIzqGrua, pwmGrua);
      Leds(1);
    }  
    else if (motor == "D" && onOff == 1){
      controlMotor(velocidad, giroIzqCarro, giroDchaCarro, pwmCarro);
      Leds(2);
    }
    else if (motor == "E" && onOff == 1){
      controlMotor(velocidad, giroDchaCarro, giroIzqCarro, pwmCarro);
      Leds(2);
    }
    else if (motor == "F" && onOff == 1){
      controlMotor(velocidad, giroIzqCable, giroDchaCable, pwmCable);
      Leds(3);
    }
    else if (motor == "G" && onOff == 1){
      controlMotor(velocidad, giroDchaCable, giroIzqCable, pwmCable);
      Leds(3);
    }
  }
             // Aplicar calibración
    bascula.set_scale(factor_calibracion);
     
    // Obtener peso en gramos
    float peso = ( bascula.get_units(10));
    float pesogr = peso * 1000;
    
    //Obtener distancia  
    int uS1 = sonar1.ping_median();
  
    //Leer finales de carrera, si están a nivel bajo, se desactiva el motor del carro en ese sentido
    int finCarrera1 = digitalRead(43);
    //Serial.println(finCarrera1);
    if (finCarrera1 == LOW){
      digitalWrite(giroIzqCarro, LOW);
    }
      
    int finCarrera2 = digitalRead(42);
    //Serial.println(finCarrera2);
    if (finCarrera2 == LOW){
      digitalWrite(giroDchaCarro, LOW);
    }
    //Si el valor de distancia es menor a 5 o mayor a 120 cm, se detiene el motor del cable en ese sentido  
    if (uS1 / US_ROUNDTRIP_CM < 5){
      digitalWrite(giroDchaCable, LOW);
    }
      
    if (uS1 / US_ROUNDTRIP_CM > 120){
      digitalWrite(giroIzqCable, LOW);
    }
  
    //Envío de valores a la aplicación mediante Bluetooth
    Serial3.print(uS1 / US_ROUNDTRIP_CM);
    Serial3.print(" cm");
    Serial3.print("|");
    
    Serial3.print(pesogr);
    Serial3.print(" gr");
    Serial3.print("|");

    Serial3.print(finCarrera1);
    Serial3.print("|");

    Serial3.print(finCarrera2);
    Serial3.println("|");    
    delay(100);
  
}