// Police Car Robot Code with police siren and flashing leds
//
// c 2019 Enoch Hwang
//
/////////////////////////////////////////////////////////////
// Ultrasonic distance sensor stuff

// this function calculates and returns the distance
// as obtained from the ultrasonic sensor
int distance() {
  int trigPin = 11;
  int echoPin = 12;  
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // STEP 1
  // The next five lines will send out one ultrasound pulse
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // STEP 2
  // The pulseIn command starts the timing and wait for the echo
  // when it hears the echo it will return the time in microseconds
  int duration = pulseIn(echoPin, HIGH, 5000); // the 5000 timeout is to limit reading to < 30 cm
                                               // changing this timing will affect the police siren

  // STEP 3
  // Calculate the distance (in cm) based on the time from STEP 2 and the speed of sound
  // the calculated distance is returned by the function
  return duration/58.2;  // in cm
  //return duration/148;// in inches
} // end of function


/////////////////////////////////////////////////////////////
// LEDs stuff
int redPin = 13;
int bluePin = 1;

/////////////////////////////////////////////////////////////
// Buzzer stuff
int buzzerPin = 8;

/////////////////////////////////////////////////////////////
// Motor stuff

// In order to control the motor speed all the pins must
// use one of the PWM ~ pins
// connections for motor 1
int in1Pin = 6;   // Input 1, must be one of the PWM ~ pins
int in2Pin = 5;   // Input 2, must be one of the PWM ~ pins
// connections for motor 2
int in3Pin = 9;   // Input 3, must be one of the PWM ~ pins
int in4Pin = 10;  // Input 4, must be one of the PWM ~ pins

void stopMotor() {
  analogWrite(in1Pin, 0);   // stop motor 1
  analogWrite(in2Pin, 0);
  analogWrite(in3Pin, 0);   // stop motor 2
  analogWrite(in4Pin, 0);
}

void goForward(int motorSpeed = 255) {
  analogWrite(in1Pin, motorSpeed);  // one input > 0
  analogWrite(in2Pin, 0);           // the other = 0   
  analogWrite(in3Pin, motorSpeed);  // one input > 0
  analogWrite(in4Pin, 0);           // the other = 0  
}

void goBackward(int motorSpeed = 255) {
  analogWrite(in1Pin, 0);           // one input = 0
  analogWrite(in2Pin, motorSpeed);  // the other > 0   
  analogWrite(in3Pin, 0);           // one input = 0
  analogWrite(in4Pin, motorSpeed);  // the other > 0  
}

void turnLeft(int motorSpeed = 255) {
  analogWrite(in1Pin, 0);           // one input = 0
  analogWrite(in2Pin, motorSpeed);  // the other > 0
  analogWrite(in3Pin, motorSpeed);  // one input > 0
  analogWrite(in4Pin, 0);           // the other = 0
}

void turnRight(int motorSpeed = 255) {
  analogWrite(in1Pin, motorSpeed);  // one input > 0
  analogWrite(in2Pin, 0);           // the other = 0
  analogWrite(in3Pin, 0);           // one input = 0
  analogWrite(in4Pin, motorSpeed);  // the other > 0
}

void testMotors() {
  goForward();
  delay(1000);
  goBackward();
  delay(1000);
  turnLeft();
  delay(600);
  turnRight();
  delay(600);
  stopMotor();
}

/////////////////////////////////////////////////////////////
// Police car stuff

void flashRedBlue() {
  static unsigned long startTime = 0;
  static bool redOn = true;
  if (millis() - startTime > 200) { // flash leds every 200 milliseconds
    startTime = millis();
    digitalWrite(redPin, redOn);
    digitalWrite(bluePin, !redOn);
    redOn = !redOn; // flip value
  }
}

void siren() {
  static unsigned long startTime = 0;
  static int frequency = 650;  // starting frequency
  static bool goingUp = true;  // go up first
  if (millis() - startTime > 10) { // play note every 10 milliseconds
    startTime = millis(); 
    tone(buzzerPin, frequency);
    if (goingUp == true) {
      frequency++;            // increment frequency for going up
      if (frequency == 900) { // reached the top?
        goingUp = false;      // change direction
      }
    } else {                  // for going down
      frequency--;            // decrement frequency for going down
      if (frequency == 650) { // reached the bottom?
        goingUp = true; 
      }
    }
  }
}

void delayWithSiren(int wait) {
  for (int i=0; i<wait; i++) {
    flashRedBlue();
    siren();
    delay(1);
  }
}

void PoliceCar() {
  int d = distance();
  if (d > 0 && d < 20) {  // check for object less than 20
    // stop, backup, and then turn left or right
    stopMotor();
    delayWithSiren(1000);
    goBackward(130);
    delayWithSiren(600);
    stopMotor();
    delayWithSiren(1000);
    // randomly generate a number between 0 and 1 inclusive
    if (random(0,2) == 0) {  //test if the number is a 0
       turnLeft(130);   // turn left if it is a 0
    } else {
       turnRight(130);  // turn right if it is a 1
    }
    delayWithSiren(500);
    stopMotor(); 
    delayWithSiren(1000);
  } else {
    goForward(255);          // continue to go forward
  }
  delayWithSiren(1);   // one millisecond each time round the loop
}

void setup() {
  // motor stuff
  pinMode(in1Pin, OUTPUT);
  pinMode(in2Pin, OUTPUT);
  pinMode(in3Pin, OUTPUT);
  pinMode(in4Pin, OUTPUT);
  // LED stuff
  pinMode(redPin, OUTPUT);
  pinMode(bluePin, OUTPUT);
  // buzzer pin
  pinMode(buzzerPin, OUTPUT);
}

void loop() {
  PoliceCar();
}