char val; // Data received from the serial port

// Store servo position
// int angle = 0;
// 1 ms = 0 degrees, 2 ms = 180 degrees
// 50 hz wave -> 20 ms high + low time 
// int high_time = 1000 + angle*1000/180;
// int low_time = 20000 - high_time;

int h_time = 1500; // head angle starts at 90 degrees (straight)
int n_time = 1500; // neck angle starts at 90 degrees (straight)
int t_time = 1500; // tail angle starts at 90 degrees (straight)
int e_time = 1500; // end angle starts at 90 degrees (straight)

void setup() {
  // Begin serial communication at baud rate 9600
  Serial.begin(9600);
  establishContact();  // send a byte to establish contact until receiver responds 
  pinMode(15, OUTPUT); // servo at pin 15 - Head
  pinMode(14, OUTPUT); // servo at pin 14 - Neck
  pinMode(9, OUTPUT); // servo at pin 9 - Tail
  pinMode(8, OUTPUT); // servo at pin 8 - End
}

 void loop() {
   if (Serial.available() > 0) { // If data is available to read,
    val = Serial.read(); // read it and store it in val
   }
   if (val == '1') 
   { // If 1 was received
     int h_time = h_time - 100; // head moves left
     digitalWrite(15, HIGH);
     delayMicroseconds(h_time);
     digitalWrite(15, LOW); 
     delayMicroseconds(20000-h_time);}
     
    if (val == '2') 
   { // If 2 was received
     int h_time = h_time + 100; // head moves right
     digitalWrite(15, HIGH);
     delayMicroseconds(h_time);
     digitalWrite(15, LOW); 
     delayMicroseconds(20000-h_time);}

     if (val == '3') 
   { // If 3 was received
     int h_time = h_time - 100; // neck moves left
     digitalWrite(14, HIGH);
     delayMicroseconds(n_time);
     digitalWrite(14, LOW); 
     delayMicroseconds(20000-n_time);}

     if (val == '4') 
   { // If 4 was received
     int n_time = n_time + 100; // neck moves right
     digitalWrite(14, HIGH);
     delayMicroseconds(n_time);
     digitalWrite(14, LOW); 
     delayMicroseconds(20000-n_time);}

     if (val == '5') 
   { // If 5 was received
     int t_time = t_time - 200; // tail moves left
     digitalWrite(9, HIGH);
     delayMicroseconds(t_time);
     digitalWrite(9, LOW); 
     delayMicroseconds(20000-t_time);}
     
    if (val == '6') 
   { // If 6 was received
     int t_time = t_time + 100; // tail moves right
     digitalWrite(9, HIGH);
     delayMicroseconds(t_time);
     digitalWrite(9, LOW); 
     delayMicroseconds(20000-t_time);}

     if (val == '7') 
   { // If 7 was received
     int e_time = e_time - 100; // end moves left
     digitalWrite(8, HIGH);
     delayMicroseconds(e_time);
     digitalWrite(8, LOW); 
     delayMicroseconds(20000-e_time);}
     
    if (val == '8') 
   { // If 8 was received
     int e_time = e_time + 100; // end moves right
     digitalWrite(8, HIGH);
     delayMicroseconds(e_time);
     digitalWrite(8, LOW); 
     delayMicroseconds(20000-e_time);}
     
     else {
     // 
   }
   delay(10); // Wait 10 milliseconds for next reading
}