/*
** TrashDroid head controller - serial input
 **
 **  (c) 2007 Scott Lawrence - yorgle@gmail.com
 **  http://umlautllama.com
 */

/* servo control routines originally by
 Tom Igoe and Carlyn Maw
 Created 28 Jan. 2006
 Updated 7 Jun. 2006
 */

/*
**  hook up a Futaba S3003 servo's data line to pin 2
 **  uses the on-board LED display on pin 13 (optional)
 */

int servoPin = 2;     // Control pin for servo motor
int LEDPin = 13;      // On-board LED

int minPulse = 300;   // Minimum servo position (mouth open position)
int maxPulse = 1600;  // Maximum servo position (mouth closed position)
int pulse = 0;        // Amount to pulse the servo

long lastPulse = 0;    // the time in milliseconds of the last pulse
int refreshTime = 20;  // the time needed in between pulses

int MouthOpen   = minPulse;
int MouthClosed = maxPulse;

int commandPosition = 0;
int incomingByte;
int sendTime = 0;


void setup() {
  pinMode( servoPin, OUTPUT);  // Set servo pin as an output pin
  pinMode( LEDPin, OUTPUT );   // Set the LED pin as an output also
  pulse = MouthClosed;         // Set the motor position value to the minimum
  randomSeed( analogRead(0) + analogRead(1) ); // seed the random number

  Serial.begin( 9600 );
  commandPosition = 0;
  sendTime = millis() + 500;
}

// what we're doing here is basically:
//  get a character from the serial port, if it is available
//      remap '0'..'9' to MouthClosed..MouthOpen
//      set the movement timeout to (now + some time)
//  if we're before the movement timeout
//      move the servo to the correct position

void loop()
{
  if (Serial.available() > 0) {
    incomingByte = Serial.read();
    if( incomingByte >= '0' && incomingByte <= '9' )
    {
      commandPosition = incomingByte - '0';
      sendTime = millis() + 500;
    }
  }

  pulse = minPulse + ((10-commandPosition) * (maxPulse - minPulse) / 10);

  if( millis() <= sendTime )
  {
    digitalWrite( LEDPin, HIGH );
    
    // pulse the servo again if rhe refresh time (20 ms) have passed:
    if (millis() - lastPulse >= refreshTime) {
      digitalWrite(servoPin, HIGH);   // Turn the motor on
      delayMicroseconds(pulse);       // Length of the pulse sets the motor position
      digitalWrite(servoPin, LOW);    // Turn the motor off
      lastPulse = millis();           // save the time of the last pulse
    }
  } else {
    digitalWrite( LEDPin, LOW );
  }
}