     advanceFrame(_actionIndex == _idleAction);
   }
 
-  const auto& frame = kActions[_actionIndex].frames[_frameIndex];
+  auto& frame = kActions[_actionIndex].frames[_frameIndex];
 
-  for (int servoIndex = 0; servoIndex < kServoCenter.size(); servoIndex++)
+  for (int servoIndex = 0; servoIndex < kServoSpecs.size(); servoIndex++)
   {
-    int pulseLen = (float(frame.boneAngles[servoIndex]) / 360) * (kServoMax - kServoMin) + kServoMin;
+    auto& servoConfig = kServoSpecs[servoIndex];
+    auto& spec = servoConfig.spec;
+    float angle = frame.boneAngles[servoIndex] + servoConfig.center;
+    angle = servoConfig.flip ? (180 - angle) : angle;
+    int pulseLen = (angle / 180) * (spec.pulseMax - spec.pulseMin) + spec.pulseMin;
     _servoMux.setPWM(servoIndex, 0, pulseLen);
   }
 

int _frameIndex = 0;
int _frameCount = 1;
int _lastFrameMillis = 0;

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


auto _servoMux = Adafruit_PWMServoDriver();

struct ServoSpec
{
  int pulseMin;
  int pulseMax;
};
const ServoSpec kServoA0090 = { 150, 650 };
const ServoSpec kServoKy66 = { 150, 520 };

struct ServoConfig
{
  ServoSpec spec;
  int center;
  bool flip;
};

// 150 (0) -> 400 (90) -> 650 (180)
//const vector<short> kServoCenter = { 0, 320, 320, 345, 290, 330, 320, 345, 290 };
const vector<ServoConfig> kServoSpecs = { 
  ServoConfig(),
  { kServoKy66, 0, false}, { kServoKy66, 90, true}, { kServoKy66, 90, false}, { kServoA0090, 0, false},
  { kServoKy66, 0, false}, { kServoKy66, 90, true}, { kServoKy66, 90, false}, { kServoA0090, 0, false}
};
LatchButton leftButton(4, InputPinMode::PullDown);
LatchButton rightButton(5, InputPinMode::PullDown);

