Problema ascensor 3 plantas modulo bluetooth, cuando sube vuelve automaticamente

Buenas, estoy haciendo un proyecto de un ascensor de 3 plantas, usando un motorreductor, 3 sensores magneticos, un imán, un módulo bluetooth, un doble puente H, un Arduino Mega, una bateria de 12 volt

El problema es que cuando mando un caracter por el dispositivo android para subir el ascensor al piso correspondiente y sube correcto pero baja automaticamente sin que se lo ordene, ejemplo presiono el boton del piso 2. Sube al piso 2 y baja al 0.

Un profesor me dijo que la solucion era limpiar el buffer pero lo probé y no funciona, AYUDAA!

Codigo del programa:

#define IN1 2 // Salida del motor 1
#define IN2 3 // Salida del motor 2
#define RETARDO 2000
#define SPISO 7
#define SPISO1 6
#define SPISO2 5
#define RETARDO 5000
void Parar();
void Arriba();
void Abajo();
void chequearCarac (int letra);
void Go();
int ChequearS ();
char letra;
int pisoActual,nuevoPiso,band=0,c1;

void setup() {

Serial.begin(9600);
Serial1.begin(38400);
pinMode(SPISO,INPUT);
pinMode(SPISO1,INPUT);
pinMode(SPISO2,INPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
analogWrite(12,120);
digitalWrite(IN1,LOW);
digitalWrite(IN2,LOW);
pisoActual = 0;
nuevoPiso =0;
}
void loop() {
if (Serial1.available()>0){
c1=Serial1.parseInt();
band=1;
}
if(band == 1){
Go();
while (!ChequearS());

}
}
void Go(){
chequearCarac(c1);
if ( (nuevoPiso - pisoActual) > 0){
/*
Serial.print("Arriba");
/
Arriba();
}else {
/

Serial.print("Abajo");
*/
Abajo();
}
}
int ChequearS(){
if(digitalRead(SPISO) == 0){
if((SPISO-7) == nuevoPiso){
Serial.print("Sensor 0");
Parar();
band=0;
return 1;
}}
if(digitalRead(SPISO1) == 0){
if((SPISO1-5) == nuevoPiso){
Serial.print("Sensor 1");
Parar();
band=0;
return 1;
}}
if(digitalRead(SPISO2) == 0){
Serial.print("Sensor 2");
if((SPISO2-3) == nuevoPiso){
Parar();
band=0;
return 1;
}
}

return 0;
}
void Parar(){
digitalWrite(IN1,LOW);
digitalWrite(IN2,LOW);

}
void Arriba(){
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
} /*

if (Serial.available()>0){
c1=Serial.read();
if(( c1 == 'p' ) || ( c1 == 'P' )){
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
}}} */
void Abajo(){

digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
}
/*
if (Serial.available()>0){
c1=Serial.read();
if(( c1 == 'b' ) || ( c1 == 'B' )){
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
}}}

int ApretarBoton(){
/* if (Serial1.available()>0){
c1=Serial1.read();
Serial.write(c1);
if(( c1 == 'p' ) || ( c1 == 'P' )){
}}}
/
/

c1=Serial.read();
Serial.write(c1);
if(( c1 == 'p' ) || ( c1 == 'P' )){
}
}
return 1;
}
/
void chequearCarac (int letra){
switch( letra ){
case 0: nuevoPiso = 0;
break;
case 1: nuevoPiso = 1;
break;
case 2: nuevoPiso = 2;
}
}
/
if (Serial1.available()>0){
c1=Serial1.read();
Serial.write(c1);
if(( c1 == 'p' ) || ( c1 == 'P' )){
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
}

if(( c1 == 'n' ) || ( c1 == 'N' )){
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
}
if(( c1 == 'o' ) || ( c1 == 'O' )){
digitalWrite(IN1,LOW);
digitalWrite(IN2,LOW);
}
}
} /
/
if (Serial.available()>0){
c1=Serial.read();
if(( c1 == 'p' ) || ( c1 == 'P' )){
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
}

if(( c1 == 'n' ) || ( c1 == 'N' )){
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
}
if(( c1 == 'o' ) || ( c1 == 'O' )){
digitalWrite(IN1,LOW);
digitalWrite(IN2,LOW);
}
}
}

/* if(digitalRead(BDER)==0){
delay(300);
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
}
if(digitalRead(BIZQ)==0){
delay(300);
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
}
if(digitalRead(BOFF)==0){
delay(300);
digitalWrite(IN1,LOW);
digitalWrite(IN2,LOW);
}
*/

Por favor lee las Normas del foro y edita tu post usando etiquetas de código
Por favor no lo dejes pasar y tampoco lo hagas en un nuevo post, edita el actúal.

Deberias agregar le al codigo el estado actual del piso, algo asi :

  chequearCarac(c1);
   if ( (nuevoPiso - pisoActual) > 0){
          Arriba();
          pisoActual= nuevoPiso-pisoActual;
        }else {
          Abajo();
          pisoActual= nuevoPiso-pisoActual;
}

porque esa variable esta siempre en cero, por lo tanto deberia estar fallando ya que nunca tenes el piso en el cual esta el ascensor, ese despues otro error de logica no veo por ahora