#include <SoftwareSerial.h>

// Pin 6 is to receive from the controller and does not need to be connected.
SoftwareSerial servoController(6, 7);

int cur = 9;

void setup() {
  Serial.begin(9600);
  servoController.begin(9600);
}

void loop() {
  Serial.println(cur);

  for(int i=0; i<4; i++) {
    setPosition((byte)i, (byte)cur);
  }
  
  cur++;
  if(cur == 255) {
    cur = 0;
  }

  //delay(2);
}

void setPosition(byte servo, byte pos) {
  servoController.write(0xFF);
  servoController.write(servo);
  servoController.write(pos);
}