６日でできる！二足歩行ロボット自作　SW編　Day4（下書き）

はじめに

指定のポーズを取らせます

今日はロボットにあらかじめ記憶させておいたポーズ取らせるようにするコマンドを追加します。

お約束

HW編の　"おわりに"　にあるソースコードに順次機能を追加して行きます。

SW編はHW編の”おわりに”で調整できているものとします。

Arduinoのスケッチは大きく分けると、定義、初期設定、メインループの三つから構成されています。

HWの最後のソースコードの定義、初期設定、メインループにそれぞれソースコードを追加すると機能が追加されます。

では早速ソースコードを見て行きます。

basicPosition配列に８つのサーボモーターの位置を角度で指定して行きます。

おわりにのソースコードで、先頭が０と１の二行が入っていますが、その間にSコマンドやAコマンドで確認したポーズを入力して行きます。

// 定義

int8_t basicPosition[][MAX_SERVO_NUM+1] =

{

{0,0,0,0,0,0,0,0,0}, // 立つ

{1,99,99,99,99,99,99,99,99} // 脱力

}

とりあえず20個分領域を追加して、この例では、１に”中腰”と２に”しゃがむ"を追加しました。

ゼロは起動時に使うので、常にすべてゼロにしておいて下さい。

この配列に記憶されたポーズを今から実装する”Bコマンド”で呼び出すと、ロボットはそのボーズを取ろうとします。

立つ→中腰→しゃがむ→中腰→立つを繰り返すと、ロボットが映像の様にスクワットをする訳です。


// 定義
int8_t basicPosition[][MAX_SERVO_NUM+1] =
 {
 {0,0,0,0,0,0,0,0,0}, // 立つ
 {1,-45,45,-45,45,0,0,0,0}, // 中腰
 {2,-70,90,-70,90,0,-75,-75,0}, // しゃがむ
 {3,0,0,0,0,0,0,0,0}, // 予約
 {4,0,0,0,0,0,0,0,0}, // 予約
 {5,0,0,0,0,0,0,0,0}, // 予約
 {6,0,0,0,0,0,0,0,0}, // 予約
 {7,0,0,0,0,0,0,0,0}, // 予約
 {8,0,0,0,0,0,0,0,0}, // 予約
 {9,0,0,0,0,0,0,0,0}, // 予約
 {10,0,0,0,0,0,0,0,0}, // 予約
 {11,0,0,0,0,0,0,0,0}, // 予約
 {12,0,0,0,0,0,0,0,0}, // 予約
 {13,0,0,0,0,0,0,0,0}, // 予約
 {14,0,0,0,0,0,0,0,0}, // 予約
 {15,0,0,0,0,0,0,0,0}, // 予約
 {16,0,0,0,0,0,0,0,0}, // 予約
 {17,0,0,0,0,0,0,0,0}, // 予約
 {18,0,0,0,0,0,0,0,0}, // 予約
 {19,99,99,99,99,99,99,99,99} // 脱力
}

// 初期設定
// 特になし

// メインループ

    // BasicPosition
    if ( command == 'B' || command == 'b' ) // 'B'is a Basic Step Command "B positionNo" for BasicPosition[positionNo][0～7]
    {
      Serial.print(command);
      Serial.print(',');

      int motion_no = Serial.parseInt();

      Serial.print(motion_no);
      Serial.print('\n');

      // check Value マイナスなら軸を左右で反転させる
      int derection = 0;
      if(motion_no < 0){
        derection = 1;
        motion_no = abs(motion_no);
      }
      if( motion_no >= 20) motion_no = 0;

      // Attach
      for (int i = 0; i < MAX_SERVO_NUM; i++)
      {
        servo[i].attach(i + 2);
      }

      // Move
      int movePosition[MAX_SERVO_NUM];
      
      if(derection == 0)
      {
        movePosition[0] = basicPosition[motion_no][1];
        movePosition[1] = basicPosition[motion_no][2];
        movePosition[2] = basicPosition[motion_no][3];
        movePosition[3] = basicPosition[motion_no][4];
        movePosition[4] = basicPosition[motion_no][5];
        movePosition[5] = basicPosition[motion_no][6];
        movePosition[6] = basicPosition[motion_no][7];
        movePosition[7] = basicPosition[motion_no][8];
      }
      else　// 左右反転
      {
        movePosition[0] = basicPosition[motion_no][3];
        movePosition[1] = basicPosition[motion_no][4];
        movePosition[2] = basicPosition[motion_no][1];
        movePosition[3] = basicPosition[motion_no][2];
        movePosition[4] = -basicPosition[motion_no][5];
        movePosition[5] = basicPosition[motion_no][7];
        movePosition[6] = basicPosition[motion_no][6];
        movePosition[7] = -basicPosition[motion_no][8];
      }

      Serial.print('b');
      for (int i = 0; i < MAX_SERVO_NUM; i++)
      {
         servo[i].write(90+servoOffset[i] + (movePosition[i] * servoDir[i]));
         Serial.print(',');
         Serial.print(movePosition[i]);
     }
      delay(250); // delay time (msec)

      Serial.print('\n');

      // Detach
      for (int i = 0; i < MAX_SERVO_NUM; i++)
      {
        servo[i].detach();
      }

    }
    //BasicPosition End.