//mattie's test troep
#include "pitches.h"
#include <Servo.h>          // Include servo library

const int baudRate = 9600;  //Default bauldRate
// notes in the melody:
//int melody[] = { NOTE_C4, NOTE_G3,NOTE_G3, NOTE_A3, NOTE_G3,0, NOTE_B3, NOTE_C4};
int melody[] = { NOTE_C4, NOTE_G3};
// note durations: 4 = quarter note, 8 = eighth note, etc.:
//int noteDurations[] = { 4, 8, 8, 4,4,4,4,4 };
int noteDurations[] = { 4, 8 };
//servo troep
Servo servoLeft;            // Declare left servo
Servo servoRight;


void setup()
{
//  Serial.begin(baudRate);
//  Serial.println("ohaaaai");
  pinMode(13, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(10, OUTPUT);
  ledON();
  muziek();

  servoLeft.attach(2);
  servoRight.attach(4);
  stopServo();

  knipperLed();
}

void stopServo()
{
  servoLeft.writeMicroseconds(1500);
  servoRight.writeMicroseconds(1500);
}

void forward()
{
    servoLeft.writeMicroseconds(1700);
    servoRight.writeMicroseconds(1300);
}

void backward()
{
    servoLeft.writeMicroseconds(1300);
    servoRight.writeMicroseconds(1700);
}


void muziek()
{
      // iterate over the notes of the melody:
  for (int thisNote = 0; thisNote < 2; thisNote++) {

    // to calculate the note duration, take one second
    // divided by the note type.
    //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
    int noteDuration = 1000/noteDurations[thisNote];
    tone(8, melody[thisNote],noteDuration);

    // to distinguish the notes, set a minimum time between them.
    // the note's duration + 30% seems to work well:
    int pauseBetweenNotes = noteDuration * 1.30;
    delay(pauseBetweenNotes);
    // stop the tone playing:
    noTone(8);
  }
}

void knipperLed()
{

   // Serial.println("dit is een test");
 // delay(1000);

 digitalWrite(13, HIGH);   //LED AAN
 digitalWrite(12, LOW);   //LED UIT
 digitalWrite(11, HIGH);
 digitalWrite(10, LOW);

 delay(250);

 digitalWrite(11, LOW);
 digitalWrite(10, HIGH);

 delay(250);


 digitalWrite(13, LOW);    //LED UIT
 digitalWrite(12, HIGH);  //LED AAN
 digitalWrite(11, HIGH);
 digitalWrite(10, LOW);

 delay(250);

 digitalWrite(11, LOW);
 digitalWrite(10, HIGH);

 delay(250);

}

void ledON()
{
  digitalWrite(13, HIGH);
  digitalWrite(12, HIGH);
  digitalWrite(11, HIGH);
  digitalWrite(10, HIGH);
}

void loop()
{
  knipperLed();
  forward();
  delay(5000);
  stopServo();
  delay(1000);
  backward();
  delay(5000);
  stopServo();
  delay(1000);
}