*All rights go to Steven Genter the creator of RoboRealm

#include <Servo.h> 

Servo servos[14];
boolean pinModes[32];

unsigned int crc;
unsigned int command;
unsigned int channel;
unsigned int streamAnalog;
unsigned int value;
unsigned long lvalue;
unsigned long streamDigital;
unsigned long lastDigital;
unsigned int lastAnalog[16];

#define ARDUINO_GET_ID 0
#define ARDUINO_SET_SERVO 1
#define ARDUINO_SET_DIGITAL_STREAM 2
#define ARDUINO_SET_DIGITAL_HIGH 3
#define ARDUINO_SET_DIGITAL_LOW 4
#define ARDUINO_SET_ANALOG_STREAM 5
#define ARDUINO_DIGITAL_STREAM 6
#define ARDUINO_ANALOG_STREAM 7

void initialize()
{
  int i;
  for (i=0;i<32;i++) pinModes[i]=-1;
  streamDigital=0;
  streamAnalog=0;
  lastDigital=-1;
  for (i=0;i<16;i++) lastAnalog[i]=-1;
}

void setup() 
{
  Serial.begin(115200);

  initialize();
  
  int i;
  for (i=2;i<14;i++) 
    servos[i].attach(i);
}

void writePacket()
{
  unsigned char buffer[2];  
  buffer[0]=command|128;
  buffer[1]=channel;
  Serial.write(buffer, 2);
}

void writeValuePacket(int value)
{
  unsigned char buffer[5];  
  
  buffer[0]=command|128;
  buffer[1]=channel;
  buffer[2]=value&127;
  buffer[3]=(value>>7)&127;
  buffer[4]=(buffer[0]^buffer[1]^buffer[2]^buffer[3])&127;
  
  Serial.write(buffer, 5);
}

void writeTriValuePacket(unsigned long value)
{
  unsigned char buffer[6];  
  
  buffer[0]=command|128;
  buffer[1]=channel;
  buffer[2]=value&127;
  buffer[3]=(value>>7)&127;
  buffer[4]=(value>>14)&127;
  buffer[5]=(buffer[0]^buffer[1]^buffer[2]^buffer[3]^buffer[4])&127;
  
  Serial.write(buffer, 6);
}

void writeQuadValuePacket(unsigned long value)
{
  unsigned char buffer[8];  
  
  buffer[0]=command|128;
  buffer[1]=channel;
  buffer[2]=value&127;
  buffer[3]=(value>>7)&127;
  buffer[4]=(value>>14)&127;
  buffer[5]=(value>>21)&127;
  buffer[6]=(value>>28)&127;
  buffer[7]=(buffer[0]^buffer[1]^buffer[2]^buffer[3]^buffer[4]^buffer[5]^buffer[6])&127;
  
  Serial.write(buffer, 8);
}

void readPacket()
{
  // get header byte
  // 128 (bit 8) flag indicates a new command packet .. that 
  // means the value bytes can never have 128 set!
  // next 7 bits are the command 0-7
  // next byte is the channel 0-32
  do
  {
    while (Serial.available() <= 0) continue; 
    command = Serial.read();
  }
  while ((command&128)==0);
  
  command^=128;

  while (Serial.available() <= 0) continue; 
  channel = Serial.read();
}

int readValuePacket()
{
  unsigned int valueLow;
  unsigned int valueHigh;
  
  // wait for value low byte    
  while (Serial.available() <= 0) continue; 
  valueLow = Serial.read();
  if (valueLow&128) return 0;

  // wait for value high byte    
  while (Serial.available() <= 0) continue; 
  valueHigh = Serial.read();
  if (valueHigh&128) return 0;
    
  // wait for crc byte    
  while (Serial.available() <= 0) continue; 
  crc = Serial.read();
  if (crc&128) return 0;
  
  if (crc!=(((128|command)^channel^valueLow^valueHigh)&127)) return 0;

  value = valueLow|(valueHigh<<7);
  
  return 1;
}

int readTriValuePacket()
{
  unsigned long valueLow;
  unsigned long valueMed;
  unsigned long valueHigh;
  
  // wait for value low byte    
  while (Serial.available() <= 0) continue; 
  valueLow = Serial.read();
  if (valueLow&128) return 0;

  // wait for value med byte    
  while (Serial.available() <= 0) continue; 
  valueMed = Serial.read();
  if (valueMed&128) return 0;

  // wait for value high byte    
  while (Serial.available() <= 0) continue; 
  valueHigh = Serial.read();
  if (valueHigh&128) return 0;
    
  // wait for crc byte    
  while (Serial.available() <= 0) continue; 
  crc = Serial.read();
  if (crc&128) return 0;
  
  if (crc!=(((128|command)^channel^valueLow^valueMed^valueHigh)&127)) return 0;

  lvalue = valueLow|(valueMed<<7)|(valueHigh<<14);
  
  return 1;
}

int readQuadValuePacket()
{
  unsigned long value1;
  unsigned long value2;
  unsigned long value3;
  unsigned long value4;
  unsigned long value5;
  
  // wait for value 1 byte    
  while (Serial.available() <= 0) continue; 
  value1 = Serial.read();
  if (value1&128) return 0;

  // wait for value 2 byte    
  while (Serial.available() <= 0) continue; 
  value2 = Serial.read();
  if (value2&128) return 0;
    
  // wait for value 3 byte    
  while (Serial.available() <= 0) continue; 
  value3 = Serial.read();
  if (value3&128) return 0;
    
  // wait for value 4 byte    
  while (Serial.available() <= 0) continue; 
  value4 = Serial.read();
  if (value4&128) return 0;
    
  // wait for value 5 byte    
  while (Serial.available() <= 0) continue; 
  value5 = Serial.read();
  if (value5&128) return 0;
    
  // wait for crc byte    
  while (Serial.available() <= 0) continue; 
  crc = Serial.read();
  if (crc&128) return 0;
  
  if (crc!=(((128|command)^channel^value1^value2^value3^value4^value5)&127)) return 0;

  lvalue = value1|(value2<<7)|(value3<<14)|(value4<<21)|(value5<<28);
  
  return 1;
}

void loop() 
{
  
  while (Serial.available()>0)
  {
    readPacket();
    
    switch (command)
    {
      // init
      case  ARDUINO_GET_ID:
        initialize();
        Serial.print("ARDU");
      break;
      // servo
      case  ARDUINO_SET_SERVO:
        if ((channel>=0)&&(channel<14))
        {
          if (readValuePacket())
          {
            servos[channel].writeMicroseconds(value);
            writeValuePacket(value);
          }
        }
      break;
      //digital stream
      case  ARDUINO_SET_DIGITAL_STREAM:
        if (readQuadValuePacket())
        {
          streamDigital = lvalue;
          writeQuadValuePacket(lvalue);
          lastDigital=-1;
        }
      break;
      //set digital high
      case  ARDUINO_SET_DIGITAL_HIGH:
        if ((channel>=0)&&(channel<32))
        {
          if (pinModes[channel]!=OUTPUT)
          {
            pinMode(channel+22, OUTPUT);
            pinModes[channel]=OUTPUT;
            if (streamDigital&((unsigned long)1<<channel))
              streamDigital^=(unsigned long)1<<channel;
          }
          
          digitalWrite(channel+22, HIGH);
          writePacket();
        }
      break;
      //set digital low
      case  ARDUINO_SET_DIGITAL_LOW:
        if ((channel>=0)&&(channel<32))
        {
          if (pinModes[channel]!=OUTPUT)
          {
            pinMode(channel+22, OUTPUT);
            pinModes[channel]=OUTPUT;
            if (streamDigital&((unsigned long)1<<channel))
              streamDigital^=(unsigned long)1<<channel;
          }
          digitalWrite(channel+22, LOW);
          writePacket();
        }
      break;
      //analog stream
      case  ARDUINO_SET_ANALOG_STREAM:
        if (readTriValuePacket())
        {
          streamAnalog = lvalue;
          writeTriValuePacket(lvalue);
          for (channel=0;channel<16;channel++) lastAnalog[channel]=-1;
        }
      break;
    }
  }
  
  for (channel=0;channel<16;channel++)
  {
    if (streamAnalog&(1<<channel))
    {
      value = analogRead(channel);
      // only send value if it has changed
      if (value!=lastAnalog[channel])
      {
        command = ARDUINO_ANALOG_STREAM;
        writeValuePacket(value);
        
        lastAnalog[channel]=value;
      }
    }
  }

  if (streamDigital)
  {
    unsigned long digitalValue=0;
    for (channel=0;channel<32;channel++)
    {
      if (streamDigital&((unsigned long)1<<channel))
      {
        if (pinModes[channel]!=INPUT)
        {
          pinMode(channel+22, INPUT);
          pinModes[channel]=INPUT;
          // pullup
          digitalWrite(channel+22, HIGH); 
        }
        
        digitalValue |= ((unsigned long)digitalRead(channel+22))<<channel;
      }
    }

    // only send value if it has changed
    if (digitalValue!=lastDigital)
    {    
      command = ARDUINO_DIGITAL_STREAM;
      channel = 0;
      writeQuadValuePacket(digitalValue);
      lastDigital=digitalValue;
    }
  }
  
}