Les Presento Mi Auto Controlado Por Bluetooth

// incluimos la biblioteca Servo 

#include <SoftwareSerial.h>
#include <Servo.h>   

// Se crean los objetos Servo
Servo servoAngulo;       
Servo servoPinza; 
 

int comando = 0; // variable a la cual de daremos el valor de la que venga por la comunicacion serial

//pines de entrada de motores
int Motor4 = 5;
int Motor3 = 4;
int Motor2 = 3;
int Motor1 = 2;

//angulos de controlpara el servo
int angulo =90;
int anguloPinza =30;
/*

Los pines GND y VCC del Servo no aparecen porque use un regulador de tencion para que no me consumiera toda la bateria 


             
                    _________________
                    |d13         D12| Servo
                    |3v3         D11| Servo
                    |REF         D10|     
                    |A0   Arduino D9|
                    |A1    NANO   D8|
                    |A2           D7|
                    |A3           D6|
                    |A4           D5| Motor3
                    |A5           D4| Motor4
                    |A6           D3| Motor2
                    |A7           D2| Motor1
     VCC bluetooth  |5v          GND| gnd bluetooth, gnd PuenteH(para igualar tierra)
                    |RST         RST|
     - 9v, - PunteH |GND          TR| tr bluetooh
     + 9v, + PunteH |VIN          TX| tx bluetooh
                    |_______________|        

*/

void setup() {

//inicializar servos
servoAngulo.attach(12); 
servoPinza.attach(11); 

//se declaran como OUTPUT los pines para controlar los motores junto al puente H
pinMode (Motor3, OUTPUT); 
pinMode (Motor4, OUTPUT); 
pinMode (Motor2, OUTPUT);
pinMode (Motor1, OUTPUT);

 // Se declara la recepcion Serial a 9600 baud
Serial.begin(9600);
}
 
void loop() {
//si viene algo por Serial se continua
if(Serial.available() > 0){
//el contenido de Serial se guarda en comando
comando = Serial.read();
}


   if (comando == '1') {
        digitalWrite (Motor3, HIGH);
        digitalWrite (Motor4, LOW);
        digitalWrite (Motor2, HIGH);
        digitalWrite (Motor1, LOW);
        Serial.println("Derecha");
        delay(20);
   }
   else if (comando == '0') {
        digitalWrite (Motor3, LOW);
        digitalWrite (Motor4, HIGH);
        digitalWrite (Motor1, HIGH);
        digitalWrite (Motor2, LOW);
        Serial.println("izquierda");
        delay(20);
   }
   else if (comando == '3') {
        digitalWrite (Motor3, HIGH);
        digitalWrite (Motor4, LOW);
        digitalWrite (Motor1, HIGH);
        digitalWrite (Motor2, LOW);
        Serial.println("Abajo");
        delay(20);
   }
   else if (comando == '4') {
        digitalWrite (Motor4, HIGH);
        digitalWrite (Motor3, LOW);
        digitalWrite (Motor2, HIGH);
        digitalWrite (Motor1, LOW);
        Serial.println("Arriba");
        delay(20);
   }
   else if (comando == '5') {
   //servo arriba
         angulo = angulo + 5;
        Serial.println(angulo);
        servoAngulo.write(angulo);               
        delay(15);                        
        angulo =constrain(angulo,0,180);
        Serial.println("angulo arriba");
   }
   else if (comando == '6') {
       angulo = angulo - 5;
        Serial.println(angulo);
        servoAngulo.write(angulo);               
        delay(15); 
        angulo =constrain(angulo,0,180);
        Serial.println("angulo abajo");


   }
   else if (comando == '8') {
        anguloPinza = anguloPinza + 5;
        Serial.println(anguloPinza);
        servoPinza.write(anguloPinza);               
        delay(15);                                  
        anguloPinza =constrain(anguloPinza,0,180);
   Serial.println("pinza cierra");
   }
   else if (comando == '9') {
       anguloPinza = anguloPinza - 5;
       Serial.println(anguloPinza);
       servoPinza.write(anguloPinza);               
       delay(15);                                  
       anguloPinza =constrain(anguloPinza,0,180);
       Serial.println("pinza abre");

   }else{
        // se ponen todos los valores en LOW para detener la detener los motores hasta que llege una nueva orden
        digitalWrite (Motor3, LOW); 
        digitalWrite (Motor4, LOW); 
        digitalWrite (Motor2, LOW); 
        digitalWrite (Motor1, LOW); 
   }
}