４足歩行ロボットの制作 - 高林の雑記ブログ

どうも、こんちゃーす。
今回は、４足歩行ロボットを作ったことについて書こうと思います。
素人なので、あまりわかってないですが…
毎度のことながら、口調は定まっていません。

作ろうと思った理由

４足歩行ロボットを作ろうと思ったきっかけは、ボストンダイナミクスのBigDogという奇妙な歩き方をするロボットを作りたいと思ったからである。
バランスを崩しても自分で修正する姿は、まるで本物の動物のようでとても興味を惹かれた。
そこで、物は試しということでまずは試作機を作ろうと思った。
今回は、歩く仕組みや制御などをすべて自分でやりたいと思ったので、すべて一人で作ることにした。
機体設計、回路設計、加工などもすべて一からやった。

開発環境

まず、使ったマイコンはSTM32F446REという、ST社が出している32bitのマイコンである。
開発環境はSystem Workbench for STM32で、CubeMXというマイコンの回路などの設定をするツールで設定して、それをEclipseに吐き出してEclipse上でプログラミングをした。
使った言語はC。

アルゴリズムについて

実際に歩くアルゴリズムには種類がたくさんある。
例えば、遊脚となるのが一脚だけの「ウォーク」、対角の脚が同期する「トロット」、前脚と後脚がほぼ同期する「ギャロップ」などがあり、それぞれで安定性が異なる。
今回は重心が安定しそうな「ウォーク」を採用した。
実際にロボットを動かすにはサーボモータ(mg996r)を用いることにした。例えば前足を前に10cm前進させようとするとプログラムでサーボモータを何度分回転させるかということに変換して考える必要がある。この方法には、一般的には逆運動学という理論が用いられる。
逆運動学とは、主にロボットの関節を制御する方法として知られるもので、指先やつま先の位置から関節の角度が求まる。
その計算のために必要なものとして、腕の長さ、関節の地面からの高さが必要なので、制御に距離センサーを追加した。
（注意）
後の結果でも述べているが、この逆運動学を用いたアルゴリズムは成功しなかった。
なぜなら、そもそもトルクが足りずにモータが思うような角度を出力できなかったからである。
逆運動学のソースコードを期待している方には申し訳ないですが、これ以降でそのコードは出てきません。

３DCAD

CADはinventor professionalという有料のソフトを用いる。ただし、学生は無料である。

大まかな手順としては、①パーツを作る②組み合わせる➂設計図を印刷する
である。

CADを作る意味はいくつかあるが、大きな意味として、干渉解析をすることができるからである。
実際に作る前に、どのパーツとどのパーツが重なり合ってしまうのかなどを確認できる。
また、穴の位置がずれていないかなども確認できる。
私も、さんざんやらかしてしまい、とてもお世話になった。
f:id:kakyouim:20190523165639p:plain

今回は、サーボモータを複数使うので、設計が楽になるように、サーボブラケットを３Dプリンターで用意した。
３Dプリンター用のデータもinventorで作ることができる。
f:id:kakyouim:20190523170308p:plain

部品の選定

DCモータ…電圧を制御することで動かす。PWMという矩形波の Duty比（ON、OFFの割合）で速度を制御。

　　　　　ステッピングモーターより速い

ステッピングモータ…回転角度をパルス数で指定して、そのぶんだけ回転させる。

　　　　　扱いやすい？

サーボモーター…小型のものが多く、トルクはあまり大きくない。

　　　　　PWM波のパルス幅（ ONの時間）によって角度を指定する

今回はサーボモータを使う。
サーボモータはサーボモータにはエンコーダと呼ばれる、回転を測る部品が内蔵されていて、
PID制御がされており、加減速が効率よく行われている

マイコンの選定

f:id:kakyouim:20190522212026p:plain — 図１　マイコン

回路

回路図は「kiCAD」という回路設計用のCADを用いた。
以下に、回路図を一つずつ載せる。
f:id:kakyouim:20190522213407p:plain

今回は、サーボのPWM信号線が１７本、ジャイロセンサーの信号線、デイスプレイの信号線、LSIの信号線、距離センサーの信号線を繋ぎます。
センサー系は、データシート通りに配線します。
サーボのPWMは、PWM波が出力できるピンに配線します。
左上の部分はLチカ（LEDちかちか）させる部分です。
STM32F446RE NUCLEOのデータシートを見ると、電源供給するには、VINに７～１２Vを、V５には５Vを供給してくださいと書いてあるので、今回はVINに8.4Vを供給します。

f:id:kakyouim:20190522213454p:plain
今回は、マイコンとサーボ、センサー類をすべて一つの電池から供給します。
その際、誤動作をしたとき用に緊急停止スイッチを作ります。
単純に、間にスイッチを挟むだけでもいいように思えますが、それだとONのときに流れる電流が大きい場合、スイッチが焼けて壊れてしまいます。
なので、今回はNチャンネルのMOSFETを挟んだにします。スイッチをONにすると、ゲートに電圧がかかって、ドレイン～ソース間に電流が流れるようになります。

f:id:kakyouim:20190522213513p:plain
LSIは５V駆動のため、レギュレータで８Vを５Vに下げてあげる必要があります。それ以外は、データシート通りに配線します。

f:id:kakyouim:20190522213532p:plain
一番上の可変抵抗で、音量を調整します。

（追記）注意！これでは正しく動作しません！
よく考えてみると当たり前なんですが、この回路ではいろんな音を鳴らせるわけないです。
スピーカーとは信号を音の振動に変えて出力するものです。その時にどんな音を出すのかは、信号の情報によります。
つまり、交流でないといけません。直流だと音を表現できないです。

今回の場合は、信号（交流の電圧）をスピーカーにつなげばよいのですが、スピーカーに入力として必要な電力を用意する必要があります。
そのため、パワーアンプ回路という、電力を増幅する回路を間に入れる必要があります。

以下の図はパワーアンプ回路の一例です。
f:id:kakyouim:20190523165504p:plain

f:id:kakyouim:20190522213558p:plain
これも５V駆動です。
データシート通りに配線します。

f:id:kakyouim:20190522213614p:plain
これは、８Vを５Vに下げる回路です。上のダイオードは、逆電圧がかかった場合に、レギュレータに流れる電流をダイオードに変わりに流して、レギュレータを保護します。

f:id:kakyouim:20190522213636p:plain
これも５V駆動です。
データシート通りに配線します

電源には以下を用いた。

f:id:kakyouim:20190522213741p:plain

実際の回路は次のようになった。

f:id:kakyouim:20190522213657p:plain

なお、ロボットに配線する際には、配線がわかりやすくなるようにするために、以下のようにした。
しかし、これは後からわかったことであるが、こうすることで損失抵抗が多くなり、電圧降下が2Vほど見られた。
サーボモータを同時に動かすために大電流を流しているからである。
f:id:kakyouim:20190522213715p:plain

CubeMXの設定について

（追記）最近CubeIDEというものがリリースされましたが、これはそれより前のSW4STM32の設定の方法です。
スライドを作成したので、これを見てほしい。

f:id:kakyouim:20190523171749p:plain

f:id:kakyouim:20190523171806p:plain

50Hz = 1秒に50回振動する = 1回の振動には20msかかる

「20msのうち、1.5ｍｓだけONにする」＝「Duty比100のうち、Duty比7.5のPWM波」
　➡50HzでDuty比が7.5のPWM波を作ればよい！

f:id:kakyouim:20190523171849p:plain

f:id:kakyouim:20190523171905p:plain

f:id:kakyouim:20190523171923p:plain

f:id:kakyouim:20190523171937p:plain

f:id:kakyouim:20190523171949p:plain

f:id:kakyouim:20190523172001p:plain

f:id:kakyouim:20190523172018p:plain

ソースコード

毎度のことながら、乱暴で煩雑なコードです。
このコードは夜中に一日で書いたので、もっと簡潔に書くべきですが、ごり押ししています。（次の日風邪ひいた）

CubeMXのおかげで、細かい設定などのプログラムは自動で生成されるので、ユーザーとして付け足す部分について述べる。
まず、サーボモータの初期位置を記述するために以下のようにした。
また、map関数はDuty比を角度に変換する自作の関数である。

参考文献としては、
blog.livedoor.jp
のしろうさぎさんの記事がとても役に立つので、参考にしてください。

#define SERVO996_LOW 500
#define SERVO996_HIGH 1000
#define SERVO3003_LOW 750
#define SERVO3003_HIGH 950
int8_t PULSE996;
int8_t PULSE3003;
uint16_t Angle;

int8_t PULSEA1 = 0;
int8_t PULSEA2 = 0;
int8_t PULSEA3 = -30;
int8_t PULSEA4 = 0;
int8_t PULSEA5 = 0;

int8_t PULSEB1 = 0;
int8_t PULSEB2 = 0;
int8_t PULSEB3 = -20;
int8_t PULSEB4 = 0;

int8_t PULSEC1 = 0;
int8_t PULSEC2 = 0;
int8_t PULSEC3 = -10;
int8_t PULSEC4 = -10;

int8_t PULSED1 = 0;
int8_t PULSED2 = -5;
int8_t PULSED3 = -20;
int8_t PULSED4 = -10;

long map(long x, long in_min,long in_max,long out_min,long out_max){
		return (x-in_min)*(out_max-out_min)/(in_max-in_min)+out_min;
}

以下は、whileの中に書く部分である。こちらもごちゃごちゃなので、ほぼ参考にしないでください…

  /* USER CODE BEGIN WHILE */
  while (1)
  {

	  //walking step1 before
	    Angle=map(PULSEC3 -10,0,120,SERVO996_LOW,SERVO996_HIGH);
	       __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	             HAL_Delay(200);


	    Angle=map(PULSEC2 +15,0,120,SERVO996_LOW,SERVO996_HIGH);
	          __HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,Angle);

	    Angle=map(PULSEC3 ,0,120,SERVO996_LOW,SERVO996_HIGH);
	      __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	        HAL_Delay(300);

	    Angle=map(PULSED3 -10,0,120,SERVO996_LOW,SERVO996_HIGH);
	     __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,Angle);
	         HAL_Delay(200);

	    Angle=map(PULSED2 +15,0,120,SERVO996_LOW,SERVO996_HIGH);
	          __HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,Angle);

	    Angle=map(PULSED3,0,120,SERVO996_LOW,SERVO996_HIGH);
	      __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,Angle);
	            HAL_Delay(1000);

	  //walking step1
	    Angle=map(PULSEA3 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
	          __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,Angle);
	          HAL_Delay(300);
	    Angle=map(PULSEA4 +40,0,120,SERVO996_LOW,SERVO996_HIGH);
	          __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_2,Angle);
	          HAL_Delay(300);
	    Angle=map(PULSEA3,0,120,SERVO996_LOW,SERVO996_HIGH);
	          __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,Angle);
	          HAL_Delay(1000);
	  //walking step1 after
	          Angle=map(PULSEC3 -15,0,120,SERVO996_LOW,SERVO996_HIGH);
	             __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	                   HAL_Delay(200);


	          Angle=map(PULSEC2,0,120,SERVO996_LOW,SERVO996_HIGH);
	                __HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,Angle);

	          Angle=map(PULSEC3 ,0,120,SERVO996_LOW,SERVO996_HIGH);
	            __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	              HAL_Delay(300);

	          Angle=map(PULSED3 -15,0,120,SERVO996_LOW,SERVO996_HIGH);
	           __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,Angle);
	               HAL_Delay(200);

	          Angle=map(PULSED2,0,120,SERVO996_LOW,SERVO996_HIGH);
	                __HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,Angle);

	          Angle=map(PULSED3,0,120,SERVO996_LOW,SERVO996_HIGH);
	            __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,Angle);
	                  HAL_Delay(1000);


	          //walking move1
	    Angle=map(PULSEC4 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
	          __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,Angle);
	                 //HAL_Delay(1000);
	    Angle=map(PULSED4 +40,0,120,SERVO996_LOW,SERVO996_HIGH);
	           __HAL_TIM_SET_COMPARE(&htim13,TIM_CHANNEL_1,Angle);
	                 //HAL_Delay(1000);
	     Angle=map(PULSEB4 +30,0,120,SERVO996_LOW,SERVO996_HIGH);
	           __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,Angle);
	                 //HAL_Delay(1000);
	  //walking set2(to first)
	  /*   //B
	     Angle=map(PULSEB3 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
	        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,Angle);
	              HAL_Delay(300);
	     Angle=map(PULSEB4 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
	        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,Angle);
	              HAL_Delay(300);
	     Angle=map(PULSEB3,0,120,SERVO996_LOW,SERVO996_HIGH);
	            __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,Angle);
	              HAL_Delay(1000);
	  */
	      //C
	     Angle=map(PULSEC3 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
	        __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	              HAL_Delay(300);
	     Angle=map(PULSEC4 +40,0,120,SERVO996_LOW,SERVO996_HIGH);
	        __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,Angle);
	              HAL_Delay(300);
	     Angle=map(PULSEC3,0,120,SERVO996_LOW,SERVO996_HIGH);
	            __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	              HAL_Delay(1000);
	      //before for D
	          //before A
	     Angle=map(PULSEA3 -10,0,120,SERVO996_LOW,SERVO996_HIGH);
	        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,Angle);

	    Angle=map(PULSEA2 +15,0,120,SERVO996_LOW,SERVO996_HIGH);
	        __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,Angle);
	        HAL_Delay(200);
	     Angle=map(PULSEA3 ,0,120,SERVO996_LOW,SERVO996_HIGH);
	       __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,Angle);
	          HAL_Delay(300);
	          //before B
	     Angle=map(PULSEB3 -10,0,120,SERVO996_LOW,SERVO996_HIGH);
	        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,Angle);

	    Angle=map(PULSEB2 +15,0,120,SERVO996_LOW,SERVO996_HIGH);
	       __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_3,Angle);
	       HAL_Delay(200);
	     Angle=map(PULSEB3,0,120,SERVO996_LOW,SERVO996_HIGH);
	        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,Angle);
	        HAL_Delay(1000);


	      //D
	      Angle=map(PULSED3 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
	        __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,Angle);
	              HAL_Delay(300);
	     Angle=map(PULSED4 -40,0,120,SERVO996_LOW,SERVO996_HIGH);
	        __HAL_TIM_SET_COMPARE(&htim13,TIM_CHANNEL_1,Angle);
	              HAL_Delay(300);
	     Angle=map(PULSED3,0,120,SERVO996_LOW,SERVO996_HIGH);
	            __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,Angle);
	              HAL_Delay(1000);
	              //after for D
	                  //after A
	             Angle=map(PULSEA3 -10,0,120,SERVO996_LOW,SERVO996_HIGH);
	                __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,Angle);

	            Angle=map(PULSEA2 ,0,120,SERVO996_LOW,SERVO996_HIGH);
	                __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,Angle);
	                HAL_Delay(200);
	             Angle=map(PULSEA3 ,0,120,SERVO996_LOW,SERVO996_HIGH);
	               __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,Angle);
	                  HAL_Delay(500);
	                  //after B
	             Angle=map(PULSEB3 -10,0,120,SERVO996_LOW,SERVO996_HIGH);
	                __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,Angle);

	            Angle=map(PULSEB2 ,0,120,SERVO996_LOW,SERVO996_HIGH);
	               __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_3,Angle);
	               HAL_Delay(200);
	             Angle=map(PULSEB3,0,120,SERVO996_LOW,SERVO996_HIGH);
	                __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,Angle);
	                HAL_Delay(1000);

	  //walking step2 before
	              Angle=map(PULSEC3 -10,0,120,SERVO996_LOW,SERVO996_HIGH);
	                 __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	                       HAL_Delay(200);


	              Angle=map(PULSEC2 +15,0,120,SERVO996_LOW,SERVO996_HIGH);
	                    __HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,Angle);

	              Angle=map(PULSEC3 ,0,120,SERVO996_LOW,SERVO996_HIGH);
	                __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	                  HAL_Delay(300);

	              Angle=map(PULSED3 -10,0,120,SERVO996_LOW,SERVO996_HIGH);
	               __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,Angle);
	                   HAL_Delay(200);

	              Angle=map(PULSED2 +15,0,120,SERVO996_LOW,SERVO996_HIGH);
	                    __HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,Angle);

	              Angle=map(PULSED3,0,120,SERVO996_LOW,SERVO996_HIGH);
	                __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,Angle);
	                      HAL_Delay(1000);

	  //walking step2
	     Angle=map(PULSEB3 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
	          __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,Angle);
	            HAL_Delay(300);
	     Angle=map(PULSEB4 -40,0,120,SERVO996_LOW,SERVO996_HIGH);
	             __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,Angle);
	                HAL_Delay(300);
	     Angle=map(PULSEB3,0,120,SERVO996_LOW,SERVO996_HIGH);
	             __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,Angle);
	                HAL_Delay(1000);
	  //walking step2 after
	                Angle=map(PULSEC3 -10,0,120,SERVO996_LOW,SERVO996_HIGH);
	                   __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	                         HAL_Delay(200);


	                Angle=map(PULSEC2,0,120,SERVO996_LOW,SERVO996_HIGH);
	                      __HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,Angle);

	                Angle=map(PULSEC3 ,0,120,SERVO996_LOW,SERVO996_HIGH);
	                  __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	                    HAL_Delay(300);

	                Angle=map(PULSED3 -10,0,120,SERVO996_LOW,SERVO996_HIGH);
	                 __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,Angle);
	                     HAL_Delay(200);

	                Angle=map(PULSED2,0,120,SERVO996_LOW,SERVO996_HIGH);
	                      __HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,Angle);

	                Angle=map(PULSED3,0,120,SERVO996_LOW,SERVO996_HIGH);
	                  __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,Angle);
	                        HAL_Delay(1000);

	  //walking move2
	     Angle=map(PULSEA4 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
	             __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_2,Angle);
	              //  HAL_Delay(1000);
	     Angle=map(PULSEC4 -40,0,120,SERVO996_LOW,SERVO996_HIGH);
	              __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,Angle);
	              //   HAL_Delay(1000);
	      Angle=map(PULSED4 +30,0,120,SERVO996_LOW,SERVO996_HIGH);
	              __HAL_TIM_SET_COMPARE(&htim13,TIM_CHANNEL_1,Angle);
	              //   HAL_Delay(1000);
	  //walking set3(to first)
	  /*    Angle=map(-40,0,120,SERVO996_LOW,SERVO996_HIGH);
	             __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	                HAL_Delay(1000);
	      Angle=map(-40,0,120,SERVO996_LOW,SERVO996_HIGH);
	             __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,Angle);
	                 HAL_Delay(1000);
	      Angle=map(0,0,120,SERVO996_LOW,SERVO996_HIGH);
	             __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	                 HAL_Delay(1000);
	   */

	              //A forward
	              Angle=map(PULSEA3 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
	                 __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,Angle);
	    //                   HAL_Delay(300);
	              Angle=map(PULSEA4 +40,0,120,SERVO996_LOW,SERVO996_HIGH);
	                 __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_2,Angle);
	                       HAL_Delay(300);
	              Angle=map(PULSEA3,0,120,SERVO996_LOW,SERVO996_HIGH);
	                     __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,Angle);
	                       HAL_Delay(500);
	               //C forward
	              Angle=map(PULSEC3 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
	                 __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	   //                    HAL_Delay(300);
	              Angle=map(PULSEC4 +40,0,120,SERVO996_LOW,SERVO996_HIGH);
	                 __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,Angle);
	                       HAL_Delay(300);
	              Angle=map(PULSEC3,0,120,SERVO996_LOW,SERVO996_HIGH);
	                     __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,Angle);
	                       HAL_Delay(500);
	                //B,D  back
          	     Angle=map(PULSEB4 +30,0,120,SERVO996_LOW,SERVO996_HIGH);
         	           __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,Angle);
       	  	    Angle=map(PULSED4 +30,0,120,SERVO996_LOW,SERVO996_HIGH);
      	  	           __HAL_TIM_SET_COMPARE(&htim13,TIM_CHANNEL_1,Angle);
                       HAL_Delay(500);


                 //B forward
	                Angle=map(PULSEB3 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
	                   __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,Angle);
	       //                  HAL_Delay(300);
	                Angle=map(PULSEB4 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
	                        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,Angle);
	                     HAL_Delay(300);
	                Angle=map(PULSEB3,0,120,SERVO996_LOW,SERVO996_HIGH);
	                     __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,Angle);
	                    HAL_Delay(500);

	               //D forward
	               Angle=map(PULSED3 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
	                 __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,Angle);
	       //                HAL_Delay(300);
	              Angle=map(PULSED4 -40,0,120,SERVO996_LOW,SERVO996_HIGH);
	                 __HAL_TIM_SET_COMPARE(&htim13,TIM_CHANNEL_1,Angle);
	                       HAL_Delay(300);
	              Angle=map(PULSED3,0,120,SERVO996_LOW,SERVO996_HIGH);
	                     __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,Angle);
	                       HAL_Delay(500);
	                //A,C back
            	     Angle=map(PULSEA4 -30,0,120,SERVO996_LOW,SERVO996_HIGH);
          	             __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_2,Angle);
          	  	     Angle=map(PULSEC4 -40,0,120,SERVO996_LOW,SERVO996_HIGH);
          	  	              __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,Angle);
   	                       HAL_Delay(500);

動画

youtu.be

結果

動画をみると明らかであるが、うまくいかなかった。機体が重すぎて、サーボモータの出力では足りず、ストールしているのがわかる。
つまり、サーボモータは60度を出力していても、実際にはストールして動かず０度のままである、ということが頻繁に起きてしまったのである。
このプログラムは、あらかじめ歩行パターンを作成しておき、それに基づいて歩くように書いたプログラムである。機体の姿勢によって出力できる角度とできない角度があり、それを一つずつ確認していくともっとうまくいくかもしれない。
しかし、それをするのも時間の無駄なきがするので、今回はここで諦めた。
敗因としては、安いサーボモータを使ったことで重量オーバーしてしまったことがあげられるので、今度は重量とトルクには細心の注意を払いたい。
また、音声合成 LSIを使ってしゃべらせようとしていたが、それをする余裕がなくなったため、次回に回すことにした。
次回は逆運動学の基づいて歩くように設計したい。

まとめ

４足歩行ロボットを作るのは難しい