Sumo

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

#define TRIGGER_PIN  12  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     13 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
#define QTR_THRESHOLD  250
Pushbutton button(ZUMO_BUTTON); // pushbutton on pin 12
ZumoBuzzer buzzer;
ZumoMotors motors;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

#define NUM_SENSORS 6
unsigned int sensor_values[NUM_SENSORS];
ZumoReflectanceSensorArray sensors(QTR_NO_EMITTER_PIN);

void waitForButtonAndCountDown()
{
 
  button.waitForButton();
  
   
  // 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()
{

  sensors.calibrate();
  
  waitForButtonAndCountDown();
  
}

void loop()
{
unsigned int uS = sonar.ping(); 
 
 if (uS / US_ROUNDTRIP_CM < 1){
 }
 else{
  
  if (uS / US_ROUNDTRIP_CM < 50) {
    
     sensors.read(sensor_values);
  
  if (sensor_values[0] < QTR_THRESHOLD)
  {
    // if leftmost sensor detects line, reverse and turn to the right
    
    motors.setSpeeds(-200, -200);
    delay(200);
    motors.setSpeeds(200, -200);
    delay(200);
    motors.setSpeeds(200, 200);
  }
  else if (sensor_values[5] < QTR_THRESHOLD)
  {
    // if rightmost sensor detects line, reverse and turn to the left
    
    motors.setSpeeds(-200, -200);
    delay(200);
    motors.setSpeeds(-200, 200);
    delay(200);
    motors.setSpeeds(200, 200);
  }
   motors.setSpeeds(250,250);
 }
 
 else {
       
    motors.setSpeeds(200,-200);
  }
  }}