●基準姿勢を調整する


ロボットを組み立て終わりましたらいよいよ電源を入れて立たせてみます。
"mm03"のプログラムを保存した方はもう一度開いて"mm05"として保存します。loop()の中を削除します。

void setup() {
  // put your setup code here, to run once:
  Wire.begin(21,22);
  Wire.setClock(100000);

  init_pca9685();
  set_angle();
  
  pinMode(19, OUTPUT);
  digitalWrite(19,LOW);
  Serial.begin(9600);  //追加
}

void loop() {
  
}

この状態でロボットに転送して実行しますが、倒れないようにロボットは寝かせておきます。腕は割とラフで構いませんが、両足は前から見て、横から見て、まっすぐになるようにソフト的にこれを修正します。下記のように修正用の変数"offset[12]"(名前はなんでも良い)を使います。
一応各関節(サーボ)に名前をつけました。

0 LHR Left Hip Roll 左股関節左右
1 LHP Left Hip Pitch 左股関節前後
2 LAP Left Ankle Pitch 左足首前後
3 LAR Left Ankle Roll 左足首左右
4 LSP Left Shoulder Pitch 左肩前後
5 LSR Left Shoulder Roll 左肩左右
6 RSR Right Shoulder Roll 右肩左右
7 RSP Right Shoulder Pitch 右肩前後
8 RAR Right Ankle Roll 右足首左右
9 RAP Right Ankle Pitch 右足首前後
10 RHP Right Hip Pitch 右股関節前後
11 RHR Right Hip Roll 右股関節左右
#include <Wire.h>
//                        LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR 
int16_t tempAngles[12] = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
int16_t offset[12] =     { 0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};

void init_pca9685() {
  .................................
}

void set_angle() {
  int angle;
  Wire.beginTransmission(0x40);
  Wire.write(6);
  for(int i=0;i<12;i++){
    angle = 400 + (tempAngles[i] + offset[i] - 90)*2.8;
    Wire.write(0x0);
    Wire.write(0x0>>8);
    Wire.write(angle);
    Wire.write(angle>>8);
  }
  Wire.endTransmission();
}

void setup() {
  ..................................
}

void loop() {
  // put your main code here, to run repeatedly:
}

サーボの角度指定のところに補正値を足して実際に出力される角度を修正しています。offset[]の値をいろいろ変えてロボットを立たせてみてほどよい値を見つけてください。サーボの回転方向はこちらの図で確認してください。最初から大きくずれてしまっている場合(30°以上)はサーボホーンを1歯ずらしてみてください。
だいたい合ったところで電池を背負った状態で後傾になっていないかどうか確認します。もし後ろに体重がかかっていると歩行で倒れやすくなりますので、足首の関節で調節します。
非常に根気のいる作業です。落ち着いて頑張りましょう。下図の矢印は各サーボの+の回転方向を示しています。
mm_joints

●歩行させる

調整が終わりましたら歩かせてみます。とりあえず下記の変更部分を追加して動かしてみましょう。

#include <Wire.h>
//                        LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR 
int16_t tempAngles[12] = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
int16_t offset[12] = { 3,  5, -3, 15,  0,  0,  0,  0,  0,  3,  0,  5};//自機に合わせて変更

int16_t fwrdAngles[8][13] = {
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 80, 90, 90, 80, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100,100, 90, 90,100,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}  //左足前
};
uint8_t divCounter;
uint8_t keyFrame;
uint8_t nextKeyFrame;

void init_pca9685() {
  ............................................
}

void set_angle() {
  ............................................
}

void setup() {
  // put your setup code here, to run once:
  Wire.begin(21,22);
  Wire.setClock(100000);

  init_pca9685();
  set_angle();
  
  pinMode(19, OUTPUT);
  digitalWrite(19,LOW);
  Serial.begin(9600);  //追加

  divCounter = 0;
  keyFrame = 0;
  nextKeyFrame = 1;
}

void loop() {
  // put your main code here, to run repeatedly:
  divCounter++;
  if(divCounter >= fwrdAngles[nextKeyFrame][12]) {
    divCounter = 0;
    keyFrame = nextKeyFrame;
    nextKeyFrame++;
    if(nextKeyFrame > 7) nextKeyFrame = 0;
  }
  for(int i=0; i<12; i++) {
    tempAngles[i] = fwrdAngles[keyFrame][i] +
              int8_t((fwrdAngles[nextKeyFrame][i] - fwrdAngles[keyFrame][i])
               * divCounter / fwrdAngles[nextKeyFrame][12]);
  }
  set_angle();
  delay(30);
}

いかがでしょうか。倒れずに前に進めば合格です。左右で動きが違う等の問題はイニシャル姿勢のオフセット調整で修正します。

●プログラムの説明
int16_t fwrdAngles[8][13] = {
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 80, 90, 90, 80, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100,100, 90, 90,100,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}  //左足前
};
uint8_t divCounter;
uint8_t keyFrame;
uint8_t nextKeyFrame;

歩行動作の各関節の角度を配列で与えます。配列の行(全体の姿勢)を順番に送ってアニメーションさせます。データ更新の時間間隔を短くすれば滑らかな動きになりますが、逆にデータ量が増えてしまいます。このため大まかな姿勢データのみを与え(キーフレームと呼ぶ)、その間を細かい時間で補間する、という方法をとります。サーボのデータ更新は30msecごとに行うこととして(delay(30))、配列右端の数値でフレーム間を分割して角度が徐々に変化するようにします。

  divCounter++;
  if(divCounter >= fwrdAngles[nextKeyFrame][12]) {
    divCounter = 0;
    keyFrame = nextKeyFrame;
    nextKeyFrame++;
    if(nextKeyFrame > 7) nextKeyFrame = 0;
  }
  for(int i=0; i<12; i++) {
    tempAngles[i] = fwrdAngles[keyFrame][i] +
              int8_t((fwrdAngles[nextKeyFrame][i] - fwrdAngles[keyFrame][i])
               * divCounter / fwrdAngles[nextKeyFrame][12]);
  }
  set_angle();
  delay(30);

角度の計算は、
現在角度 = 今回キーフレーム角度 + (次回キーフレーム角度 - 今回キーフレーム角度) * 分割カウンター / 分割数
という感じで、分割カウンターは毎回+1して分割数に達したら0に戻します。そのタイミングで今回キーフレームと次回キーフレームを次の行に更新します。

●練習問題

後ろに歩かせてください。

#include <Wire.h>

int16_t tempAngles[12] = {90,90,90,90,90,90,90,90,90,90,90,90};
int16_t offset[12] = {3,5,-3,15,0,0,0,0,0,3,0,5};//自機に合わせて変更

int16_t bwrdAngles[8][13] = { //後退
// 順番を逆にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90,100, 90, 90, 90, 4}, //左足前
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90, 80, 80,100, 90, 90, 90, 90,100, 90, 90, 90, 8}, //右重心
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 90, 90, 80, 90, 90, 90, 90, 90, 80, 80, 90, 4}, //右足前
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 90, 90, 90, 90, 80,100,100, 90, 8}, //左重心
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8}
};
int16_t fwrdAngles[8][13] = {
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 80, 90, 90, 80, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100,100, 90, 90,100,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}  //左足前
};

uint8_t divCounter;
uint8_t keyFrame;
uint8_t nextKeyFrame;

void init_pca9685() {
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x80);   //reset device
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x10);   //sleep mode
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0xFE);
  Wire.write(0x65); //set prescaler to 60Hz
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x80);   //back to prev mode
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0xa0);   //enable auto-increment
  Wire.endTransmission();
}

void set_angle() {
  int angle;
  Wire.beginTransmission(0x40);
  Wire.write(6);
  for(int i=0;i<12;i++){
    angle = 400 + (tempAngles[i] + offset[i] - 90)*2.8;
    Wire.write(0x0);
    Wire.write(0x0>>8);
    Wire.write(angle);
    Wire.write(angle>>8);
  }
  Wire.endTransmission();
}

void setup() {
  // put your setup code here, to run once:
  Wire.begin(21,22);
  Wire.setClock(100000);

  init_pca9685();
  set_angle();
  
  pinMode(19, OUTPUT);
  digitalWrite(19,LOW);
  Serial.begin(9600);  //追加

  divCounter = 0;
  keyFrame = 0;
  nextKeyFrame = 1;
}

void loop() {
  // put your main code here, to run repeatedly:
  divCounter++;
  if(divCounter >= bwrdAngles[nextKeyFrame][12]) {
    divCounter = 0;
    keyFrame = nextKeyFrame;
    nextKeyFrame++;
    if(nextKeyFrame > 7) nextKeyFrame = 0;
  }
  for(int i=0; i<12; i++) {
    tempAngles[i] = bwrdAngles[keyFrame][i] +
              int8_t((bwrdAngles[nextKeyFrame][i] - bwrdAngles[keyFrame][i])
               * divCounter / bwrdAngles[nextKeyFrame][12]);
  }
  set_angle();
  delay(30);
}
●練習問題

左右旋回させてください。

・左旋回

#include <Wire.h>

int16_t tempAngles[12] = {90,90,90,90,90,90,90,90,90,90,90,90};
int16_t offset[12] = {3,5,-3,15,0,0,0,0,0,3,0,5};//自機に合わせて変更

int16_t ltrnAngles[8][13] = {
// 常に左重心にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 90, 90, 90, 90, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 90, 90, 90, 90, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80, 90, 90, 90, 90, 90,100, 90, 90, 90, 8}, //左重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90, 90, 90, 90, 90,100, 90, 90, 90, 4}  //左足前
};
int16_t bwrdAngles[8][13] = { //後退
// 順番を逆にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90,100, 90, 90, 90, 4}, //左足前
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90, 80, 80,100, 90, 90, 90, 90,100, 90, 90, 90, 8}, //右重心
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 90, 90, 80, 90, 90, 90, 90, 90, 80, 80, 90, 4}, //右足前
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 90, 90, 90, 90, 80,100,100, 90, 8}, //左重心
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8}
};
int16_t fwrdAngles[8][13] = {
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 80, 90, 90, 80, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100,100, 90, 90,100,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}  //左足前
};
uint8_t divCounter;
uint8_t keyFrame;
uint8_t nextKeyFrame;

void init_pca9685() {
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x80);   //reset device
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x10);   //sleep mode
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0xFE);
  Wire.write(0x65); //set prescaler to 60Hz
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x80);   //back to prev mode
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0xa0);   //enable auto-increment
  Wire.endTransmission();
}

void set_angle() {
  int angle;
  Wire.beginTransmission(0x40);
  Wire.write(6);
  for(int i=0;i<12;i++){
    angle = 400 + (tempAngles[i] + offset[i] - 90)*2.8;
    Wire.write(0x0);
    Wire.write(0x0>>8);
    Wire.write(angle);
    Wire.write(angle>>8);
  }
  Wire.endTransmission();
}

void setup() {
  // put your setup code here, to run once:
  Wire.begin(21,22);
  Wire.setClock(100000);

  init_pca9685();
  set_angle();
  
  pinMode(19, OUTPUT);
  digitalWrite(19,LOW);
  Serial.begin(9600);  //追加

  divCounter = 0;
  keyFrame = 0;
  nextKeyFrame = 1;
}

void loop() {
  // put your main code here, to run repeatedly:
  divCounter++;
  if(divCounter >= ltrnAngles[nextKeyFrame][12]) {
    divCounter = 0;
    keyFrame = nextKeyFrame;
    nextKeyFrame++;
    if(nextKeyFrame > 7) nextKeyFrame = 0;
  }
  for(int i=0; i<12; i++) {
    tempAngles[i] = ltrnAngles[keyFrame][i] +
              int8_t((ltrnAngles[nextKeyFrame][i] - ltrnAngles[keyFrame][i])
               * divCounter / ltrnAngles[nextKeyFrame][12]);
  }
  set_angle();
  delay(30);
}

・右旋回

#include <Wire.h>

int16_t tempAngles[12] = {90,90,90,90,90,90,90,90,90,90,90,90};
int16_t offset[12] = {3,5,-3,15,0,0,0,0,0,3,0,5};//自機に合わせて変更

int16_t rtrnAngles[8][13] = {
// 常に右重心にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90,100, 90, 90, 90, 90, 90,100,100, 90, 8}, //右重心
  {90, 90, 90,100, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90,100, 90, 90, 90, 90, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100, 90, 90, 90, 90,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90, 90, 90, 90, 90,100, 90, 90, 90, 4}  //左足前
};
int16_t ltrnAngles[8][13] = {
// 常に左重心にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 90, 90, 90, 90, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 90, 90, 90, 90, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80, 90, 90, 90, 90, 90,100, 90, 90, 90, 8}, //左重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90, 90, 90, 90, 90,100, 90, 90, 90, 4}  //左足前
};
int16_t bwrdAngles[8][13] = { //後退
// 順番を逆にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90,100, 90, 90, 90, 4}, //左足前
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90, 80, 80,100, 90, 90, 90, 90,100, 90, 90, 90, 8}, //右重心
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 90, 90, 80, 90, 90, 90, 90, 90, 80, 80, 90, 4}, //右足前
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 90, 90, 90, 90, 80,100,100, 90, 8}, //左重心
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8}
};
int16_t fwrdAngles[8][13] = {
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 80, 90, 90, 80, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100,100, 90, 90,100,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}  //左足前
};
uint8_t divCounter;
uint8_t keyFrame;
uint8_t nextKeyFrame;

void init_pca9685() {
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x80);   //reset device
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x10);   //sleep mode
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0xFE);
  Wire.write(0x65); //set prescaler to 60Hz
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x80);   //back to prev mode
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0xa0);   //enable auto-increment
  Wire.endTransmission();
}

void set_angle() {
  int angle;
  Wire.beginTransmission(0x40);
  Wire.write(6);
  for(int i=0;i<12;i++){
    angle = 400 + (tempAngles[i] + offset[i] - 90)*2.8;
    Wire.write(0x0);
    Wire.write(0x0>>8);
    Wire.write(angle);
    Wire.write(angle>>8);
  }
  Wire.endTransmission();
}

void setup() {
  // put your setup code here, to run once:
  Wire.begin(21,22);
  Wire.setClock(100000);

  init_pca9685();
  set_angle();
  
  pinMode(19, OUTPUT);
  digitalWrite(19,LOW);
  Serial.begin(9600);  //追加

  divCounter = 0;
  keyFrame = 0;
  nextKeyFrame = 1;
}

void loop() {
  // put your main code here, to run repeatedly:
  divCounter++;
  if(divCounter >= rtrnAngles[nextKeyFrame][12]) {
    divCounter = 0;
    keyFrame = nextKeyFrame;
    nextKeyFrame++;
    if(nextKeyFrame > 7) nextKeyFrame = 0;
  }
  for(int i=0; i<12; i++) {
    tempAngles[i] = rtrnAngles[keyFrame][i] +
              int8_t((rtrnAngles[nextKeyFrame][i] - rtrnAngles[keyFrame][i])
               * divCounter / rtrnAngles[nextKeyFrame][12]);
  }
  set_angle();
  delay(30);
}
●練習問題

横歩きさせてください。
・右横歩き

#include <Wire.h>

int16_t tempAngles[12] = {90,90,90,90,90,90,90,90,90,90,90,90};
int16_t offset[12] = {3,5,-3,15,0,0,0,0,0,3,0,5};//自機に合わせて変更

int16_t rghtAngles[6][13] = {
// LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time 
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 90, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 80, 8},
  {100, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  { 90, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8}
};
int16_t rtrnAngles[8][13] = {
// 常に右重心にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90,100, 90, 90, 90, 90, 90,100,100, 90, 8}, //右重心
  {90, 90, 90,100, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90,100, 90, 90, 90, 90, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100, 90, 90, 90, 90,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90, 90, 90, 90, 90,100, 90, 90, 90, 4}  //左足前
};
int16_t ltrnAngles[8][13] = {
// 常に左重心にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 90, 90, 90, 90, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 90, 90, 90, 90, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80, 90, 90, 90, 90, 90,100, 90, 90, 90, 8}, //左重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90, 90, 90, 90, 90,100, 90, 90, 90, 4}  //左足前
};
int16_t bwrdAngles[8][13] = { //後退
// 順番を逆にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90,100, 90, 90, 90, 4}, //左足前
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90, 80, 80,100, 90, 90, 90, 90,100, 90, 90, 90, 8}, //右重心
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 90, 90, 80, 90, 90, 90, 90, 90, 80, 80, 90, 4}, //右足前
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 90, 90, 90, 90, 80,100,100, 90, 8}, //左重心
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8}
};
int16_t fwrdAngles[8][13] = {
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 80, 90, 90, 80, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100,100, 90, 90,100,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}  //左足前
};
uint8_t divCounter;
uint8_t keyFrame;
uint8_t nextKeyFrame;

void init_pca9685() {
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x80);   //reset device
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x10);   //sleep mode
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0xFE);
  Wire.write(0x65); //set prescaler to 60Hz
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x80);   //back to prev mode
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0xa0);   //enable auto-increment
  Wire.endTransmission();
}

void set_angle() {
  int angle;
  Wire.beginTransmission(0x40);
  Wire.write(6);
  for(int i=0;i<12;i++){
    angle = 400 + (tempAngles[i] + offset[i] - 90)*2.8;
    Wire.write(0x0);
    Wire.write(0x0>>8);
    Wire.write(angle);
    Wire.write(angle>>8);
  }
  Wire.endTransmission();
}

void setup() {
  // put your setup code here, to run once:
  Wire.begin(21,22);
  Wire.setClock(100000);

  init_pca9685();
  set_angle();
  
  pinMode(19, OUTPUT);
  digitalWrite(19,LOW);
  Serial.begin(9600);  //追加

  divCounter = 0;
  keyFrame = 0;
  nextKeyFrame = 1;
}

void loop() {
  // put your main code here, to run repeatedly:
  divCounter++;
  if(divCounter >= rghtAngles[nextKeyFrame][12]) {
    divCounter = 0;
    keyFrame = nextKeyFrame;
    nextKeyFrame++;
    if(nextKeyFrame > 5) nextKeyFrame = 0;
  }
  for(int i=0; i<12; i++) {
    tempAngles[i] = rghtAngles[keyFrame][i] +
              int8_t((rghtAngles[nextKeyFrame][i] - rghtAngles[keyFrame][i])
               * divCounter / rghtAngles[nextKeyFrame][12]);
  }
  set_angle();
  delay(30);
}

・左横歩き

#include <Wire.h>

int16_t tempAngles[12] = {90,90,90,90,90,90,90,90,90,90,90,90};
int16_t offset[12] = {3,5,-3,15,0,0,0,0,0,3,0,5};//自機に合わせて変更

int16_t leftAngles[6][13] = {
// LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time 
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8},
  { 90, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  {100, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 80, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 90, 8},
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8}
};
int16_t rghtAngles[6][13] = {
// LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time 
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 90, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 80, 8},
  {100, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  { 90, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8}
};
int16_t rtrnAngles[8][13] = {
// 常に右重心にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90,100, 90, 90, 90, 90, 90,100,100, 90, 8}, //右重心
  {90, 90, 90,100, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90,100, 90, 90, 90, 90, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100, 90, 90, 90, 90,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90, 90, 90, 90, 90,100, 90, 90, 90, 4}  //左足前
};
int16_t ltrnAngles[8][13] = {
// 常に左重心にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 90, 90, 90, 90, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 90, 90, 90, 90, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80, 90, 90, 90, 90, 90,100, 90, 90, 90, 8}, //左重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90, 90, 90, 90, 90,100, 90, 90, 90, 4}  //左足前
};
int16_t bwrdAngles[8][13] = { //後退
// 順番を逆にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90,100, 90, 90, 90, 4}, //左足前
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90, 80, 80,100, 90, 90, 90, 90,100, 90, 90, 90, 8}, //右重心
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 90, 90, 80, 90, 90, 90, 90, 90, 80, 80, 90, 4}, //右足前
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 90, 90, 90, 90, 80,100,100, 90, 8}, //左重心
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8}
};
int16_t fwrdAngles[8][13] = {
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 80, 90, 90, 80, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100,100, 90, 90,100,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}  //左足前
};
uint8_t divCounter;
uint8_t keyFrame;
uint8_t nextKeyFrame;

void init_pca9685() {
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x80);   //reset device
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x10);   //sleep mode
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0xFE);
  Wire.write(0x65); //set prescaler to 60Hz
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x80);   //back to prev mode
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0xa0);   //enable auto-increment
  Wire.endTransmission();
}

void set_angle() {
  int angle;
  Wire.beginTransmission(0x40);
  Wire.write(6);
  for(int i=0;i<12;i++){
    angle = 400 + (tempAngles[i] + offset[i] - 90)*2.8;
    Wire.write(0x0);
    Wire.write(0x0>>8);
    Wire.write(angle);
    Wire.write(angle>>8);
  }
  Wire.endTransmission();
}

void setup() {
  // put your setup code here, to run once:
  Wire.begin(21,22);
  Wire.setClock(100000);

  init_pca9685();
  set_angle();
  
  pinMode(19, OUTPUT);
  digitalWrite(19,LOW);
  Serial.begin(9600);  //追加

  divCounter = 0;
  keyFrame = 0;
  nextKeyFrame = 1;
}

void loop() {
  // put your main code here, to run repeatedly:
  divCounter++;
  if(divCounter >= leftAngles[nextKeyFrame][12]) {
    divCounter = 0;
    keyFrame = nextKeyFrame;
    nextKeyFrame++;
    if(nextKeyFrame > 5) nextKeyFrame = 0;
  }
  for(int i=0; i<12; i++) {
    tempAngles[i] = leftAngles[keyFrame][i] +
              int8_t((leftAngles[nextKeyFrame][i] - leftAngles[keyFrame][i])
               * divCounter / leftAngles[nextKeyFrame][12]);
  }
  set_angle();
  delay(30);
}

<= マイクロマシーン チュートリアル(4)
チュートリアルベース
マイクロマシーン チュートリアル(6) =>