#include <Servo.h>
#include "flashFL.h"
#include "flashBL.h"
#include "flashFR.h"
#include "flashBR.h"
#include "flashHL.h"
#include "flashHR.h"
#include "flashTL.h"
#include "flashTR.h"

Servo servoFL; 
Servo servoBL;
Servo servoFR; 
Servo servoBR;
Servo servoHL;
Servo servoHR;
Servo servoTL;
Servo servoTR;

int angleFL = 110;  // 110
int angleBL = 70; // 70
int angleFR = 92; // 70
int angleBR = 136; // 110
int angleHL = 135;
int angleHR = 155;
int angleTL = 45;
int angleTR = 25;
volatile uint16_t i;
long pulse, inches;

void setup()
{
  servoFL.attach(6);
  servoBL.attach(7);
  servoFR.attach(8);
  servoBR.attach(9); 
  servoHL.attach(10);
  servoHR.attach(11);
  servoTL.attach(12);
  servoTR.attach(13);
  servoFL.write(angleFL);
  servoBL.write(angleBL);
  servoFR.write(angleFR);
  servoBR.write(angleBR);
  servoHL.write(angleHL);
  servoHR.write(angleHR);
  servoTL.write(angleTL);
  servoTR.write(angleTR);
  delay(5000); 
  Serial.begin(9600);
  pinMode(5, INPUT);
}

void loop()
{
  for (i=0; i<160; i+=1) 
  { 
    angleFL = pgm_read_word(&flashFL[i]);
    angleBL = pgm_read_word(&flashBL[i]);
    angleFR = pgm_read_word(&flashFR[i]);
    angleBR = pgm_read_word(&flashBR[i]);
    angleHL = pgm_read_word(&flashHL[i]);
    angleHR = pgm_read_word(&flashHR[i]); 
    angleTL = pgm_read_word(&flashTL[i]);
    angleTR = pgm_read_word(&flashTR[i]);       
    servoFL.write(angleFL);
    servoBL.write(angleBL);
    servoFR.write(angleFR);
    servoBR.write(angleBR);
    servoHL.write(angleHL);
    servoHR.write(angleHR);
    servoTL.write(angleTL);
    servoTR.write(angleTR);
    if (i%40==0)
    {
      pulse = pulseIn(5, HIGH);
      inches = pulse/147;
      Serial.print(inches);
      while (inches<10)
      {
        delay(5000);
        pulse = pulseIn(5, HIGH);
        inches = pulse/147;
      }
    }
    delay(20); // 20       
  }
}