#include <MINDSi.h>
#include <Servo.h>

Servo pan, tilt;
byte data[4];
byte armed=0, pitch=0, rotation=0;
unsigned long cache;

// this example uses a running average and deadband when driving based off of a radio signal
// this will help in environments with a lot of noise etc.

void setup() 
{
  pan.attach(6);
  tilt.attach(7);
  Serial.begin(9600);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  rotation = 90;
}

void loop() {
  if(cache=Serial.available() > 0){
    data[3] = data[2]; data[2] = data[1]; data[1] = data[0]; data[0] = Serial.read();
    if(data[3] == '<' && (data[1] == data[0]) ){
      switch(data[2]){
        case 'f': armed = data[1]; break;
        case 'p': pitch = data[1]; break;
        case 'r': rotation = data[1]; break;
      }
    }
    if(cache > 100){
      Serial.flush();
      data[0] = 0; data[1] = 0; data[2] = 0; data[3] = 0;
    }
  }
  
  tilt.write(pitch);
  pan.write(rotation);
  if(armed & 0x01) digitalWrite(8, HIGH);
  else digitalWrite(8, LOW);
  if(armed & 0x02) digitalWrite(9, HIGH);
  else digitalWrite(9, LOW);
  if(armed & 0x04) digitalWrite(10, HIGH);
  else digitalWrite(10, LOW);
  if(armed & 0x08) digitalWrite(11, HIGH);
  else digitalWrite(11, LOW);
  
}