週刊中ロボ104 A地点～B地点まで人類補間計画その2

続きに
今度は自動で途中のモーションを補完してみます。補間のパターンは色々あるのですが、ArduiroidOneのソフトウェアはサーボモータの角度で姿勢を決めているので、一番簡単な等間隔に補間する場合の関数を作ってみます。

ソースコード
この関数は単純に指定されたポジションAからポジションBを５分割して動かす関数です。

// Set 8 ServoMoter PositionA to B
void set8PosAToB(int positionA, int positionB)
{
  for(int8_t i=0; i<5; i++) //5分割 
  {
    Serial.print(i);

    for(int8_t j=0; j<8; j++)  // 8個のサーボモータ
    {
      angle[j] = servoCenter[j] +((basicPosition[positionA][j]
        + ((basicPosition[positionB][j]-basicPosition[positionA][j])/4.0*i)
          )* servoDir[j]); 
      Serial.print(',');
      Serial.print(angle[j]);
     }
     Serial.println();
 
    // servo Move
    set8Angle( angle[0], angle[1], angle[2], angle[3], angle[4], angle[5], angle[6], angle[7] );
   delay(SERVO_DELAY/5);
  
  }
}

使い方
あらかじめbasicPosion配列に格納された、A地点からB地点までを等間隔に補完して動かします。
　set8PosAToB(1,2);

これはポジション１からポジション２へ補間をしながらサーボモータを動かします。

実装例
　例えばシリアルで 5を指示されたら、最初に一気に動かし、次に補間で動かすなら、こんなコードになるでしょう。

case '5': // ToDo
　 set8Pos(0);
　 set8Pos(1);
　 set8Pos(0);
　 set8PosAToB(0,1);
　 set8PosAToB(1,0);
break;

この例の様に手を動かすくらいの動きが小さい場合は、set8Pos()で指示したばあいとあまり変わりありません。

f:id:nakarobo:20200711051725j:plain f:id:nakarobo:20200711051818j:plain