#include <Servo.h>

/*
  Dual servo control controlled via serial input.  Uses the Software
  Servo library from
  <http://www.arduino.cc/playground/ComponentLib/Servo>.

  Intended for use with the Pandora pan/tilt head from dpcav.com, with
  a Hitec HS-65HB as the pan servo and a Hitec HS-81 as the tilt servo.

  Serial commands are two pairs of two ASCII digits, where each pair
  is a percentage from 00-99.  The first percentage controls the pan
  servo and the second controls the tilt servo.  The maximum pan angle
  is about 180 degrees while the maximum tilt angle is about 45
  degrees.

        Serial command      Pan angle   Tilt angle
        ---------------     ----------  ----------
             0000                 0           0
             1000                18           0
             0099                 0          45
             9900               178           0
             5050                90          23

  By John Wiseman 1/31/2008
  jjwiseman@yahoo.com
  http://lemonodor.com/
 */

Servo panServo;
Servo tiltServo;

void setup() {
  Serial.begin(19200);                // Init serial port to 19200 bps.
  Serial.println("Running");

  panServo.attach(3);                 // Pan servo on pin 3.
  panServo.setMaximumPulse(2350);
  panServo.setMinimumPulse(700);
  
  tiltServo.attach(2);                // Tilt servo on pin 2.
  tiltServo.setMaximumPulse(2000);
  tiltServo.setMinimumPulse(1400);
}

int panAngle = 90;
int tiltAngle = 90;


/*
  Read a two-digit percentage from the serial interface.
*/

void readPercents(int *pan, int *tilt) {
  int c = Serial.read();
  while (c != 'a' && Serial.available() >= 4) {
    c = Serial.read();
  }
  if (c != 'a') { 
    *pan = -1;
    *tilt = -1;
    return;
  }
  
  int a = Serial.read();
  int b = Serial.read();
  if (a < '0' || a > '9' || b < '0' || b > '9') {
    *pan = -1;
  } else {
    *pan = (a - '0') * 10 + (b - '0');
  }
  
  a = Serial.read();
  b = Serial.read();
  if (a < '0' || a > '9' || b < '0' || b > '9') {
    *tilt = -1;
  } else {
    *tilt = (a - '0') * 10 + (b - '0');
  }
}

void loop() {
  // Read a commad from the serial interface if there is one.
  if (Serial.available() >= 5) {
    int pan, tilt;
    readPercents(&pan, &tilt);
    if (pan != -1) {
      panAngle = (180 * pan) / 100;
    }
    if (tilt != -1) {
      tiltAngle = (180 * tilt) / 100;
    }
  }
  
  // Set servo values.
  panServo.write(panAngle);
  tiltServo.write(tiltAngle);

  // Pulse the servos.
  Servo::refresh();
}