#include <Servo.h>
// Author: McFarland, Justin S. 5/20/2008
// TwoServo_Example_motion.pde
/* Purpose: Program is a hybrid combination of the Servo_Example, Blink, and Photosensor_Arduino code
It works by user specifying two servo objects, a series of observation variables
Servo commands work by being able to work by sending a PW to the servo on servo PIN
Navigation works by using conditional structures to evaluate photoresistance; LEDS indicate 
sensing of light above the 'threshold' values
*/
// Reference: Essential C++ for Engineers and Scientists by Jeri R. Hanley
// Reference: Chapter 6: Light Sensitive Navigation with Photoresitors, Robotics with the Boe-Bot by Andy Lindsay

// INITIALIZE SERVO CLASS
Servo servo1; //left servo
Servo servo2; //right servo

int servo1_pin = 14;  // PIN 14 is analog PIN 0 on the Arduino Diecimila board
int servo2_pin = 15;  // PIN 15 is analog PIN 1 on the Arduino Diecimila board

// INITIALIZE PHOTORESISTORS
int photores1_pin = 2; //left pr
int photores2_pin = 3; //right pr
double leftambient  = 400.0;
double rightambient = 400.0;
double leftbright  = 40.0;
double rightbright = 40.0;
double leftthreshold  = leftbright  + leftambient  / 2.0 * 5.0 / 8.0;
double rightthreshold = rightbright + rightambient / 2.0 * 5.0 / 8.0;

//INITIALIZE LEDS
int LED1_pin = 4;
int LED2_pin = 8;

void setup()
{
  pinMode(photores1_pin, INPUT); // Sets the pin as an input
  pinMode(photores2_pin, INPUT); // Sets the pin as an input
  pinMode(LED1_pin, OUTPUT); // Sets the pin as an output
  pinMode(LED2_pin, OUTPUT); // Sets the pin as an output
  
  servo1.attach(servo1_pin); // Need to review in servo library
  servo1.setMaximumPulse(2200); // 16-bit integer value is divided by 16
  
  servo2.attach(servo2_pin); // Need to review in servo library
  servo2.setMaximumPulse(2200); // 16-bit integer value is divided by 16
}


void loop()
{ 
static int v1=105;  
static int v2=105;  
double photores1_pinval = photoresistor( photores1_pin);  // assess value of the photoresistor
double photores2_pinval = photoresistor( photores2_pin);  // assess value of the photoresistor

//NAVIGATION LOGIC 
if (( photores1_pinval < leftthreshold ) & (photores2_pinval < rightthreshold )) { // FORWARD
 //Left Servo (1) CW, Right servo (2) CCW
  v1 = 0;  v2 = 180;
  servo1.write(v1);   servo2.write(v2); //write to port
  digitalWrite(LED1_pin, HIGH);   // sets the LED on
  digitalWrite(LED2_pin, HIGH);   // sets the LED on
  
}
else if ( photores1_pinval < leftthreshold) { // TURN LEFT
  // Left Servo (1) CW, Right Servo (2) CW
  v1 = 0; v2 = 0;
  servo1.write(v1);   servo2.write(v2); //write to port
    digitalWrite(LED1_pin, HIGH);   // sets the LED on
  digitalWrite(LED2_pin, LOW);   // sets the LED off
}
else if  ( photores2_pinval < rightthreshold) { // TURN RIGHT
// Left Servo (1) CCW, Right Servo (2) CCW
  v1 = 180; v2 = 180;
  servo1.write(v1);   servo2.write(v2); //write to port
      digitalWrite(LED1_pin, LOW);   // sets the LED off
  digitalWrite(LED2_pin, HIGH);   // sets the LED on
}
else {// BACKWARD
 //Left Servo (1) CCW, Right servo (2) CW
  v1 = 180;  v2 = 0;
  servo1.write(v1);   servo2.write(v2); //write to port
      digitalWrite(LED1_pin, LOW);   // sets the LED off
  digitalWrite(LED2_pin, LOW);   // sets the LED off
}
  
  Servo::refresh(); // library function
}


// PHOTORESISTOR CALL OBSERVES VOLTAGE ACROSS DIVIDER
double photoresistor( int photores_pin){ 
  double photores_val = analogRead(photores_pin); // Reads the analog value for the photoresistor
  delay(30); // 30 ms delay
  return photores_val;
}