Arduino sketch for nanabo

varspeedservoslave.ino

#include <VarSpeedServoSam.h> 

VarSpeedServoSam servos[4];
int MAX_SPEED = 50;
int PIN_NO[] = {2, 3, 4, 5};
int MAX_DEGREE[] = {110, 110, 120, 100};
int MIN_DEGREE[] = {70, 90, 80, 80};
int INITIAL_DEGREE[] = {90, 90, 90, 90};

int flag, sid, degree, speed;

void setup() {
  Serial.begin(57600);
  while (!Serial) {
  }

  // initial position
  for (int i = 0; i<4; i++) {
    servos[i].attach(PIN_NO[i]);
    servos[i].write(INITIAL_DEGREE[i]);
  }
}

void loop() {
  int n = Serial.available();
  while (n > 0) {
//    Serial.println(n);
    int flag = Serial.read();
    n--;
    if (flag == 0xFE) {
      if (n < 3) {
        break;
      }
      sid = Serial.read();
      degree = Serial.read();
      speed = Serial.read();
      speed = min(MAX_SPEED, speed);
      degree = constrain(degree, MIN_DEGREE[sid], MAX_DEGREE[sid]);
      servos[sid].write(degree, speed, false);
    }
  }
  delay(20);
}