クアッドクローラーの歩行を理解する

組み込み
スポンサーリンク

クアッドクローラーの歩行はquadCrawler_Walk関数で制御されています。

void quadCrawler_Walk(uint16_t speed, uint8_t com)

最初の引数は「動作速度」です。
デフォルトでは「quadCrawler_fast」が設定されています。
テストや動作確認を行う場合は低速にした方が分かりやすいと思います。
「com」は動作する方向です。

//動作速度定義
enum {
  quadCrawler_sslow = 2000,
  quadCrawler_slow = 1000,
  quadCrawler_typical = 500,
  quadCrawler_fast = 200,
  quadCrawler_high = 100,
};

前進動作

制御方式としては「motion」に値を設定し「set_servo_motion」関数で実際に動かしています。

else if (com == fw) {
  //Serial.println ("-fw");
  static const uint8_t motion[4][8] = {
  {-1, RFC_F, -1, RRC_R, -1, LFC_R, -1, LRC_F},
  {RFK_D, -1, RRK_U, -1, LFK_U, -1, LRK_D, -1, },
  {-1, RFC_R, -1, RRC_F, -1, LFC_F, -1, LRC_R},
  {RFK_U, -1, RRK_D, -1, LFK_D, -1, LRK_U, -1, }};
  set_servo_motion(motion, ServoRepeat0);
}

設定されている定数については、quadCrawler.cppで定義されています。
8個のモーターにそれぞれ値が設定されていますが、[Crach」が基盤に直結されているモーターとなり、「Knee」が関節を曲げるモーターになります。
「-1」はスキップされるので、定数が設定されているものが対象となります。
実際の足の動きについては「つくるっち」のサイトを見ると分かりやすいです。

ロボットを歩かせる

対角線となる前後の足を地面につけ、もう一方の対角線を浮かします。
浮かした足を前に出しつつ、自慢につけた足で前方に押し出しています。

モーター制御について

実際にモーターを動かす処理はset_servo_deg関数で行われています。
可動域を超えないように処理が入っています。
組み立てを間違えなければプログラミングでちょっと間違えても可動域を超えることは無いと思います。

static void set_servo_deg(uint8_t id, unsigned int deg) {
  const uint8_t channel_table[] = {RFK, RFC, RRK, RRC, LFK, LFC, LRK, LRC};
  uint8_t channel = channel_table[id];
  unsigned int setdata;
  setdata = (unsigned int)((deg - deg_min) * (servo_max - servo_min) /  (deg_max - deg_min)) + servo_min;
  if (setdata <= servo_max) {
    if (setdata >= servo_min) {
      pwm.setPWM(channel, 0, setdata);
      delay(20);
    }
  }
}

ちょっと計算してみます。
右足を前に押し出す処理の場合、定数は以下のようになっています。

define servo_min 200 // Min pulse length out of 4096
define servo_max 400 // Max pulse length out of 4096
define deg_max 130 // Max deg
define deg_min 20 // Min deg

define RFC_F 90

ちょっと計算してみましょう。
まず、以下の部分で1度ずらすためのパルス数を計算しています。

(servo_max - servo_min) / (deg_max - deg_min)
= (400 - 200) / (130 - 20)
= 200 / 110
= 1.8

以下で移動させる角度を計算しています。
初期値が20度の位置になっていると思うので、90度動かすためには70度移動させることになります。

(deg - deg_min) = 90 - 20 = 70

ということで、移動角度に1度当たりのパルス数をかけて最小値(初期値)を加えると90度動かすためのパルス数が計算できます。

70 * 1.8 + 200 = 326

計算した値を使用してサーボを動かしています。

pwm.setPWM(channel, 0, setdata);

コメント

タイトルとURLをコピーしました