#include <Servo.h>

/* Author: Justin S. McFarland 
Date: 6/11/2008
www.ceruleanplains.com
Program works by user interacting via the serial monitor
Servo commands work by being able to work by sending a PW to the servo on servo PIN
User sets up the baud rate to match the command setting
Serial is interacted with a PW value and a character string to send, detach, or reattach
the connection to the servo
Required: Servo class subfunctions stored in hardware libraries
*/

Servo servo1;
Servo servo2;

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

void setup()
{
servo1.attach(servo1pin); // Need to review in servo library
servo1.setMaximumPulse(2200); // 16-bit integer value is divided by 16
  
servo2.attach(servo2pin); // Need to review in servo library
servo2.setMaximumPulse(2200); // 16-bit integer value is divided by 16
  
  Serial.begin(19200);
  Serial.print("Ready"); // output to the serial command window
}


void loop()
{ 
  int v1=90;  
  int v2 = 90;
  if(Serial.available()){
    char ch = Serial.read(); 
    Serial.print("\n"); // outputs to screen for read character value
    Serial.print(ch);
    Serial.print("\n");
    switch(ch) { // evaluate a character string for send, detach, or reattach commands   
      case 'l':
       // TURN LEFT
       // Left Servo (1) CW, Right Servo (2) CW
       v1 = 80; v2 = 80;  
         servo1.write(v1); //write to port
         servo2.write(v2); //write to port  
    break;     
      case 'r':
       // TURN RIGHT
       // Left Servo (1) CCW, Right Servo (2) CCW
          v1 = 100; v2 = 100;
         servo1.write(v1); //write to port
         servo2.write(v2); //write to port 
      break;   
      case 'f':
        // FORWARD
        //Left Servo (1) CCW, Right servo (2) CW
        v1 = 100;  v2 = 80;
         servo1.write(v1); //write to port
         servo2.write(v2); //write to port
        break;
      case 'b': 
        // BACKWARDS
        // Left Servo (1) CW, Right Servo (2) CCW
        v1 = 80; v2 = 100;
        servo1.write(v1); //write to port
        servo2.write(v2); //write to port
        break;
        case 'h':
        // HALT!
        v1 = 90; v2 = 90;
         servo1.write(v1); //write to port
         servo2.write(v2); //write to port
        break;  
        case 's': //servo command for sending
         servo1.write(v1); //write to port
         servo2.write(v2); //write to port
         v1=90; v2 = 90;
         break;
       case 'd': //serial command for detatchment
         servo1.detach();
         servo2.detach(); //write to port
         break;
       case 'a': //serial command for attachment
          servo1.attach(servo1pin);
          servo2.attach(servo2pin);
           break;
    }
  }
  Servo::refresh(); // library function
}