// PINKCONNECTION2 Client Program for Helen Henny/Chuck E./Guest Star Cyberamic Servo

#include <Servo.h>

Servo servoChannel[6];
byte onDegrees[8] =  { 90, 0, 0, 0, 45, 135, 0, 0 }; // these are incomplete
byte offDegrees[8] = { 0 , 0, 0, 0, 90, 90 , 0, 0 };
byte mapping[8] =    { 0 , 1, 1, 2, 3 , 3  , 4, 5 };

int byte1 = 0;
int byte2 = 0;

void setup()
{
  servoChannel[0].attach(2);
  servoChannel[1].attach(3);
  servoChannel[2].attach(4);
  servoChannel[3].attach(5);
  servoChannel[4].attach(6);
  servoChannel[5].attach(7);
  for (int i = 0; i < 8; i++) servoChannel[mapping[i]].write(offDegrees[i]);
  Serial.begin(9600);
  Serial.write("PC2,8,Chuck E./Helen/Guest Star,END");
}

void loop()
{
  if (Serial.available() > 1)
  {
    byte1 = Serial.read();
    byte2 = Serial.read();

    if ((byte1 & 64) && (byte2 & 64))
    {
      if (byte1 & 1) servoChannel[mapping[0]].write(onDegrees[0]);
      else servoChannel[mapping[0]].write(offDegrees[0]);

      if (byte1 & 2) servoChannel[mapping[1]].write(onDegrees[1]);
      else servoChannel[mapping[1]].write(offDegrees[1]);

      if (byte1 & 4) servoChannel[mapping[2]].write(onDegrees[2]);
      else servoChannel[mapping[2]].write(offDegrees[2]);

      if (byte1 & 8) servoChannel[mapping[3]].write(onDegrees[3]);
      else servoChannel[mapping[3]].write(offDegrees[3]);


      if (byte2 & 1) servoChannel[mapping[4]].write(onDegrees[4]);
      else servoChannel[mapping[4]].write(offDegrees[4]);

      if (byte2 & 2) servoChannel[mapping[5]].write(onDegrees[5]);
      else servoChannel[mapping[5]].write(offDegrees[5]);

      if (byte2 & 4) servoChannel[mapping[6]].write(onDegrees[6]);
      else servoChannel[mapping[6]].write(offDegrees[6]);

      if (byte2 & 8) servoChannel[mapping[7]].write(onDegrees[7]);
      else servoChannel[mapping[7]].write(offDegrees[7]);
    }
  }
}