Dynamixel AX-12での歩行モーションでH8の限界が見えた

逆運動学ちっくにXYZから各関節角をもとめるプログラムを書いて、軌跡から歩行モーションのパターンを生成するプログラムを書いた。

ただ、これだとどうも、AX-12の剛性が弱すぎて、体重移動したあと片足を浮かせることができない。
よって、体重移動に合わせて、軸足のロール軸だけを体重移動を戻す方向に回転させることで、負荷をかけ片足を上げたときにトルクがつりあうような補正をかけることで、足踏みモーションを作った。

そこから、正弦波を用いた軌跡を関数でリアルタイムにパターンを作って実行する。

プログラムは下に書いてみた。
たぶん、コメントがないのでよく分からないと思うし、構造的に移植することはできないと思うが、問題なのは実行速度。

H8/3052だとこれを実行したとき1秒に４ループくらいしか実行してくれない。
double型の三角関数と逆三角関数をなんども計算するためだろうか・・・。
AX-12のボーレートをを56kでとっているので、その速度の性もあるかもしれないが、それでも遅いだろう。
計算なしで値だけ送ると1秒を２０分割くらいは出来るので・・・。

となると、計算速度。
パイプライン処理できるRISCのマイコンは必須だろうか・・・。
ATmega128が1枚あるが・・・、それを使うか、とうとうSH2に手を出すか・・・。

sinテーブルを100倍したint型で配列で処理するという手もないこともないが、いつか限界が来ることを考えると、出来れば実行速度を上げたいところだと思う・・・。

あと、AX-12についてだが、内部温度エラーでトルクオフした際、マニュアルではTorque Enableを１にすればOKと書いてますが、上手くいかなかったのでベステクさんにメールしたところ

運用的に従来の方法（アラーム停止→最指令→復帰→連続運用）では
あえて復帰しない様にしてあります。
同時にトルクリミット値が0になりますのでアラーム要因が解消してい
れば再設定する事で動作します。

とのことでした。

以下が例のプログラム
Z方向の軌跡は正弦波の上半分だけ。
原点は腰が(0,0,0)で、前方にX軸、右方向にY軸、下向きにZ軸の正の方向を取る。

//左足
int new_xyz_r(int x,int y,int z){
  double l,a,b,c,d;
  int leg = 77;
  l = sqrt(x * x + y * y + z * z);
  c = asin(1.0 * x / l) * 57.3;
  d = atan(1.0 * y / z) * 57.3;
  a = acos(0.5 * l / leg) * 57.3;
  b = asin(0.5 * l / leg) * 57.3;
  pos_sav[3] = 819 - 3.413 * b; if(safe(3)) return 1;
  pos_sav[4] = 205 + 3.413 * b; if(safe(4)) return 1;
  pos_sav[1] = pos_sav[6] = 665 - 3.413 * d; if(safe(1)) return 1;
  pos_sav[5] = 512 - 3.413 * ( c + a ); if(safe(5)) return 1;
  pos_sav[2] = 512 - 3.413 * ( c - a ); if(safe(2)) return 1;
  return 0;
}
//左足
int new_xyz_l(int x,int y,int z){
  double l,a,b,c,d;
  int leg = 77;
  l = sqrt(x * x + y * y + z * z);
  c = asin(1.0 * x / l) * 57.3;
  d = atan(1.0 * y / z) * 57.3;
  a = acos(1.0 * l / leg) * 57.3;
  b = asin(1.0 * l / leg) * 57.3;
  pos_sav[9] = 205 + 3.413 * b;if(safe(9)) return 1;
  pos_sav[10] = 819 - 3.413 * b;if(safe(10)) return 1;
  pos_sav[7] = pos_sav[12] = 358 - 3.413 * d; if(safe(7)) return 1; //ここの符号でyの方向
  pos_sav[11] = 512 + 3.413 * ( c + a ); if(safe(11)) return 1;
  pos_sav[8] = 512 + 3.413 * ( c - a ); if(safe(8)) return 1;
  return 0;
}
void sin_g_move(int b,int k,int n,int hr , int hl , int go,int go2,int up ,int inz,int gain){ 
 // b    : 揺れ幅
 // k    : 回数
 // n    : フレーム分割数
 // hr   : 右足の歩幅
 // hl   : 左足の歩幅
 // go   : 前に傾ける値（そのままサーボの値）
 // go2  : 前方への体重移動
 // up   : 上がる足の高さ
 // inz  : 初期の足の長さ
 // gain : 足の付け根のロール軸補正ゲイン
  int i,y,j,rx,lx,rz,lz;
  for (j=0;j＜k;j++){
    for (i=0;i＜n;i++){
      y = 1.0 * b * sin(3.141592 * 2 * i / n);//基準位相
      rx = 1.0 * hr * sin(3.141592 * 2 * i / n - 3.141592 / 2 );//位相90-
      lx = 1.0 * hl * sin(3.141592 * 2 * i / n + 3.141592 / 2 );//位相90+
      rz = inz - 1.0 * up * sin(3.141592 * 2 * i / n );//位相180-
      lz = inz + 1.0 * up * sin(3.141592 * 2 * i / n);//
      //足上げだけZ軸ポジション確定
      if (rz ＞ inz) rz = inz;
      if (lz ＞ inz) lz = inz;
      //go2は体重移動の補正(8がBEST)
      new_xyz_r(rx - go2 ,y,rz);
      new_xyz_l(lx - go2 ,y,lz);
      //左右にゆれたときの腰の加重に関する補正(gain5がBEST)
      if (y ＜ 0) pos_sav[6] = pos_sav[6] - ( -y ) * gain;
      if (y ＞ 0) pos_sav[12]= pos_sav[12]+ (  y ) * gain;
      //前かがみの補正(20BEST)
      pos_sav[5]= pos_sav[5] - go;
      pos_sav[11]= pos_sav[11] + go;
      all_send();
    }
  }
}