Zumo — вход не может войти и остаться в пустоте

В настоящее время я программирую свой Zumo и пытаюсь нажать его, чтобы войти в пустоту. Он может войти в другие пустоты при обнаружении стены, например, но по какой-то причине я не могу заставить его войти в пустоту enterRoom () при нажатии клавиш. Я пытался просто вызвать его, теперь я пытаюсь использовать флаг и оператор if, но он просто не будет вводиться.

Код ниже:

   #include <ZumoBuzzer.h>
#include <ZumoMotors.h>
#include <Pushbutton.h>
#include <QTRSensors.h>
#include <ZumoReflectanceSensorArray.h>

#define LED 13

// this might need to be tuned for different lighting conditions, surfaces, etc.
#define QTR_THRESHOLD  300 // microseconds

// these might need to be tuned for different motor types
#define REVERSE_SPEED     50 // 0 is stopped, 400 is full speed
#define TURN_SPEED        100
#define FORWARD_SPEED     50
#define REVERSE_DURATION  200 // ms
#define TURN_DURATION     150 // ms

ZumoBuzzer buzzer;
ZumoMotors motors;
Pushbutton button(ZUMO_BUTTON); // pushbutton on pin 12const int trigPin = 2;
const int echoPin = 6;
long duration;#define NUM_SENSORS 6
unsigned int sensor_values[NUM_SENSORS];
int j = 0;
int x = 1;
int z=0;
int incomingByte;
int roomNumber = 0;
ZumoReflectanceSensorArray sensors(QTR_NO_EMITTER_PIN);

void waitForButtonAndCountDown()
{
digitalWrite(LED, HIGH);
button.waitForButton();
digitalWrite(LED, LOW);

// play audible countdown
for (int i = 0; i < 3; i++)
{
delay(1000);
buzzer.playNote(NOTE_G(3), 200, 15);
}
delay(1000);
buzzer.playNote(NOTE_G(4), 500, 15);
delay(1000);
}

void setup()
{
Serial.begin(9600);
incomingByte = Serial.read();
pinMode(LED, HIGH);

waitForButtonAndCountDown();
}

void loop()
{
switch(j) {
case 0:
navigate();
case 1:
control();
}
}

void navigate() {

sensors.read(sensor_values);
incomingByte = Serial.read();

if ((incomingByte == 'K') || (incomingByte == 'k'))
{
motors.setSpeeds(0, 0);
j=1;
}
else if ((sensor_values[5] > QTR_THRESHOLD) && (sensor_values[0] > QTR_THRESHOLD))
{
motors.setSpeeds(0, 0);
j = 1;
}
else if (sensor_values[0] > QTR_THRESHOLD)
{
// if leftmost sensor detects line, reverse and turn to the right
motors.setSpeeds(TURN_SPEED, -TURN_SPEED);
delay(TURN_DURATION);
motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
}
else if (sensor_values[5] > QTR_THRESHOLD)
{
// if rightmost sensor detects line, reverse and turn to the left
motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
delay(TURN_DURATION);
motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
}
else
{
// otherwise, go straight
motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
}

}void control()
{

incomingByte = Serial.read();
Serial.print("test");
// if it's a capital H (ASCII 72), turn on the LED:
if (incomingByte == 'W') {
digitalWrite(LED, HIGH);

motors.setRightSpeed(100);
motors.setLeftSpeed(100);
delay(2);

}
// if it's an L (ASCII 76) turn off the LED:
if (incomingByte == 'S') {
digitalWrite(LED, LOW);
motors.setLeftSpeed(-100);
motors.setRightSpeed(-100);
delay(2);

}

if (incomingByte == 'A') {
digitalWrite(LED, HIGH);

motors.setRightSpeed(150);
motors.setLeftSpeed(-150);
delay(2);
}

if (incomingByte == 'D') {
digitalWrite(LED, HIGH);

motors.setLeftSpeed(150);
motors.setRightSpeed(-150);
delay(2);
}

if (incomingByte == ' ') {
digitalWrite(LED, HIGH);

motors.setLeftSpeed(0);
motors.setRightSpeed(0);
delay(2);
}

if (incomingByte == 'C')
{
j = 0;
}

if (incomingByte == 'R')
{
x=0;
if (x=0)
{
enterRoom();
}
}

}

void enterRoom()
{
motors.setSpeeds(0, 0);

incomingByte = Serial.read();

if (incomingByte == 'A') {
motors.setLeftSpeed(-50);
motors.setRightSpeed(50);
delay(2000);
z=1;
if (z=1)
{
enterLeftRoom();
}
}

if (incomingByte == 'D') {
motors.setLeftSpeed(50);
motors.setRightSpeed(-50);
delay(2000);
z=2;
if (z=2)
{
enterRightRoom();
}
}

if (incomingByte == 'C') {
x=1;
j=1;
}
}

void enterLeftRoom()
{
motors.setSpeeds(0, 0);
/*roomNumber++;
Serial.print("Room");
Serial.print(roomNumber);
Serial.print("Entered");
Serial.println("(Left)");

pinMode(trigPin, OUTPUT);
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);

pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);

if (duration < 1000)
{
Serial.print("Person Detected in room: ");
Serial.println(roomNumber);
}*/
}

void enterRightRoom()
{
motors.setSpeeds(0, 0);
/*roomNumber++;
Serial.print("Room");
Serial.print(roomNumber);
Serial.print("Entered");
Serial.println("(Right)");

pinMode(trigPin, OUTPUT);
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);

pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);

if (duration < 1000)
{
Serial.print("Person Detected");
Serial.println(roomNumber);
}*/
}

Может кто-нибудь придумать другой способ, которым это можно сделать, поскольку я рисую бланк.

Заранее спасибо.