Robot keyframe movement.

robot_frames.ino

#include <Servo.h>

// El numero de motores
static const unsigned int numServos = 2;

// Los motores, sus pins, y su posicion actual
static Servo servos[numServos];
static unsigned int servoPins[numServos] = {9,10};
static unsigned int servoPos[numServos] = {0};

// La animacion
static const unsigned int numFrames = 4;
static unsigned int currentFrame = 0;
static unsigned int delays[numFrames] = { 100, 100, 100, 100 };
static unsigned int keyFrames[numServos][numFrames] = {
  {0, 90, 180, 90},
  {0, 90, 180, 90}
};


static int i;

// Mandar señal a los motores
void updateServos() {
  for (i = 0; i < numServos; i++) {
    servos[i].write(servoPos[i]);
  }
}

// Mover los motores hacia la posicion deseada
byte stepKeyFrame() {
  byte frameReached = 1;
  
  for (i = 0; i < numServos; i++) {
    int target = keyFrames[i][currentFrame];
    int delta = servoPos[i] - target;
    
    if (abs(delta) <= 1) {

      servoPos[i] = target;

    } else {

      int scaledDelta = delta * 98 / 100;

      if (!scaledDelta)
        scaledDelta = delta > 0 ? 1 : -1;
      
      servoPos[i] = target + scaledDelta;

      frameReached = 0;
    }
  }
  
  updateServos();
  int dt = (int)(delays[currentFrame] * ((float)(100-98)/98.0));
  delay(dt);
  return frameReached;
}

// Conectar los motores
void setup() {
  for (i = 0; i < numServos; i++) {
    servos[i].attach(servoPins[i]);
  }
  
  updateServos();
}

// Main loop
void loop() {
  if (stepKeyFrame()) {
    currentFrame = (currentFrame + 1) % numFrames;
  }
}