#include <Servo.h>                           // Include servo library
Servo servoRight;
Servo servoLeft;

int PIEZOPIN = A0;

int button = 2;

int lwskr = 5;
int rwskr = 7;

int ired = A1;

//int note_A4 = 440;
//int note_As4 = 466; int note_Bb4 = note_As4;
//int note_B4 = 494;
//int note_C5 = 523;
//int note_Cs5 = 554; int note_Db5 = note_Cs5;
//int note_D5 = 587;
//int note_Ds5 = 622; int note_Eb5 = note_Ds5;
//int note_E5 = 659;
//int note_F5 = 698;
//int note_Fs5 = 740; int note_Gb5 = note_Fs5;
//int note_G5 = 784;
//int note_Gs5 = 830; int note_Ab5 = note_Gs5;

void setup() {
  Serial.begin(9600); // open serial port
  pinMode(lwskr,INPUT); // declare whiskers as inputs
  pinMode(rwskr,INPUT); // read values
  pinMode(button, INPUT);
  servoLeft.attach(13);                      // Attach left signal to pin 13
  servoRight.attach(12);                     // Attach right signal to pin 12
  servoLeft.writeMicroseconds(1500);        // Calibrate left servo
  servoRight.writeMicroseconds(1500);// Calibrate right servo
}
void moveKitty(){
  forward(500);
  delay(500);
}

void stopKitty(){
  servoRight.writeMicroseconds(1500);
  servoLeft.writeMicroseconds(1500);
  delay(1);
}

// clock is 1500 - 2000
// counter is 1000 - 1500

void clockw(int tim){
  servoLeft.writeMicroseconds(2000);
  servoRight.writeMicroseconds(2000);
  delay(tim);
}

void counter(int tim){
  servoRight.writeMicroseconds(1000);
  servoLeft.writeMicroseconds(1000);
  delay(tim);
}

void leftturn(int tim){
  servoRight.writeMicroseconds(1000);
  delay(tim);
}

void rightturn(int tim){
  servoLeft.writeMicroseconds(1000);
  delay(tim);
}

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

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

void loop() {
//  tone(PIEZOPIN,200,500);
//  delay(500);

  int lwskrval = digitalRead(lwskr);
  int rwskrval = digitalRead(rwskr);
  int pushval = digitalRead(button);
  int iredval = analogRead(ired);

  if (lwskrval == 1  && rwskrval == 1){
    moveKitty();
  }
  else if(lwskrval == 0  || rwskrval == 0){
    stopKitty();
    backward(500);
    clockw(500);
  }
  else if (lwskrval ==  0){
    stopKitty();
    rightturn(500);
  }
  else if (rwskrval == 0){
    stopKitty();
    leftturn(500);
  }
  delay(500);

//  if (pushval == 0){
//    clockw(1000);
//    counter(1000);
//    delay(2000);
//  }
}