1

I am currently programming my Zumo and I am trying to get it on key press to enter a void. It can enter other voids when a wall is detected e.g. but for some reason I can't get it to go into my enterRoom() void on keypress. I have tried just calling it, now I'm trying to use a flag and if statement but it just won't enter it.

Code below:

   #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 12


const 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);
   }*/
}

Can anyone think of another way this can be done as I'm drawing a blank.

Thanks in advance.

benjano
  • 369
  • 1
  • 5
  • 17

0 Answers0