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

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

/* here's basically what it does:
  State: STATE_WAIT     waits for 45 seconds, blinking the LED so you know it's not dead
  State: STATE_CHATTER  moves the servo randomly for 5 seconds
  State: STATE_CLOSE    closes the mouth (2 seconds)
  Then it loops back around to STATE_WAIT
*/

#define TIME_WAIT     (45 * 1000)
#define TIME_CHATTER  (5 * 1000)
#define TIME_CLOSE    (1 * 1000)


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;

#define STATE_INIT    0
#define STATE_WAIT    1
#define STATE_CHATTER 2
#define STATE_CLOSE   3

int State = STATE_INIT;
long NextStateTransition = 0;
int StateChanged = 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
  
  // setup the State machine - start off by closing the mouth
  State = STATE_CLOSE;
  StateChanged = 1;
}

void loop()
{
  
  switch( State ) {
    case( STATE_INIT ):
      if( StateChanged ) {
        NextStateTransition = 0;
      }
      break; 
      
    case( STATE_WAIT ):
      if( StateChanged ) {
        NextStateTransition = millis() + TIME_WAIT; /* wait for this long, idle */
      }
      // blink the on-board LED while we're waiting, so we know we're alive
      if( millis() & 0x400 )
        digitalWrite( LEDPin, HIGH );
      else
        digitalWrite( LEDPin, LOW );
      break;
      
    case( STATE_CHATTER ):
      if( StateChanged ) {
        NextStateTransition = millis() + TIME_CHATTER; /* chatter for this long */
        digitalWrite( LEDPin, LOW );
      }
      if( millis() & 0x0100 == 0x0100 )
        pulse = random( minPulse+100, maxPulse-100 );
      break;
      
    case( STATE_CLOSE ):
      if( StateChanged ) {
        NextStateTransition = millis() + TIME_CLOSE; /* close the mouth */
        digitalWrite( LEDPin, HIGH );
      }
      pulse = MouthClosed;
      break;
      
    default:
      break;
  }
  StateChanged = 0; /* clear the changed state flag */
  
  /* look for a state transition, if there was one, change the state, set the flag */
  if( millis() >= NextStateTransition )
  {
      State++;
      if( State > STATE_CLOSE ) State = STATE_WAIT;
      StateChanged = 1;
  }
  

  // only update the servo while we're moving it to save battery life
  if( State == STATE_CHATTER || State == STATE_CLOSE )
  {
    // 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
    }
  }
}