The head - firmware beta release

This is the beta version of the firmware insde the Head-Arduino

[code]
 /*
P.R.U. Personal Robotic Unit
A robot with cat's personality
------------------------------
Head firmware:
Head Pan & tilt
2 ears servo rotating
2 eyes 4 led

*/
#include <Servo.h>
#include <ModbusSlave.h>

/* First step MBS: create an instance */
ModbusSlave mbs;
Servo EarLeft;
Servo EarRight;
Servo Pan;
Servo Tilt;

/* slave registers */
enum {      
        MB_PAN,    
        MB_TILT,
        MB_R_EYE,
        MB_L_EYE,
        MB_R_EAR,
        MB_L_EAR,
        MB_REGS /* total number of registers on slave */
};

int regs[MB_REGS];
unsigned long wdog = 0;         /* watchdog */
unsigned long tprev = 0;         /* previous time*/

void set_eye(char eye, boolean l1, boolean l2, boolean l3, boolean l4);

void setup()
{
  /* the Modbus slave configuration parameters */
  const unsigned char SLAVE = 10;
  const long BAUD = 19200;
  const char PARITY = 'e';
  const char TXENPIN = 7;

  /* Second step MBS: configure */
  mbs.configure(SLAVE,BAUD,PARITY,TXENPIN);

  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(A4, OUTPUT);
  pinMode(A5, OUTPUT);

  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);

  pinMode(A0, OUTPUT);
  pinMode(A1, OUTPUT);
  pinMode(A2, OUTPUT);
  pinMode(A3, OUTPUT);

  Pan.attach(9);
  Tilt.attach(10);
  EarLeft.attach(A4);
  EarRight.attach(A5);
}

void loop()
{
  /* Third and las step MBS: update in loop*/
  if(mbs.update(regs, MB_REGS))
    wdog = millis();

  if ((millis() - wdog) > 5000)  {      /* no comms in 5 sec */
    // to do
  }      

  /* the values in regs are set by the modbus master */
  // Test Pan
  if(regs[MB_PAN] != Pan.read())
    Pan.write(regs[MB_PAN]);

  // Test Tilt
  if(regs[MB_TILT] != Tilt.read())
    Tilt.write(regs[MB_TILT]);

  // Test Ear Left
  if(regs[MB_L_EAR] != EarLeft.read())
    EarLeft.write(regs[MB_L_EAR]);

  // Test Ear Right
  if(regs[MB_R_EAR] != EarRight.read())
    EarRight.write(regs[MB_R_EAR]);
 
  // Test Left Eye
  switch(regs[MB_L_EYE]) {
    case 0: // Ronf
      set_eye(1, LOW, LOW, LOW, LOW);
      break;
    case 10: // Mmmmmaaaoooo
      set_eye(1, LOW, HIGH, HIGH, LOW);
      break;
    case 11: // Look R
      set_eye(1, LOW, HIGH, LOW, LOW);
      break;
    case 12: // Look R
      set_eye(1, LOW, LOW, HIGH, LOW);
      break;
    case 13: // Look Up
      set_eye(1, HIGH, LOW, LOW, LOW);
      break;
    case 14: // Looh D
      set_eye(1, LOW, LOW, LOW, HIGH);
      break;
    case 15: // OMG
      set_eye(1, HIGH, HIGH, HIGH, HIGH);
      break;
  }

  // Test Right Eye
  switch(regs[MB_R_EYE]) {
    case 0: // Ronf
      set_eye(0, LOW, LOW, LOW, LOW);
      break;
    case 10: // Mmmmmaaaoooo
      set_eye(0, LOW, HIGH, HIGH, LOW);
      break;
    case 11: // Look R
      set_eye(0, LOW, HIGH, LOW, LOW);
      break;
    case 12: // Look R
      set_eye(0, LOW, LOW, HIGH, LOW);
      break;
    case 13: // Look Up
      set_eye(0, HIGH, LOW, LOW, LOW);
      break;
    case 14: // Looh D
      set_eye(0, LOW, LOW, LOW, HIGH);
      break;
    case 15: // OMG
      set_eye(0, HIGH, HIGH, HIGH, HIGH);
      break;
  }
}

void set_eye(char eye, boolean l1, boolean l2, boolean l3, boolean l4)
{
  if (eye==0) {
      digitalWrite(A0, l1);
      digitalWrite(A1, l2);
      digitalWrite(A2, l3);
      digitalWrite(A3, l4);
  }
  if (eye==1) {
      digitalWrite(2, l1);
      digitalWrite(3, l2);
      digitalWrite(4, l3);
      digitalWrite(5, l4);
  }
}
[/code]