Arduino Quadruped

必要なツールとマシン

>

はんだごて（汎用）

アプリとオンラインサービス

>

Arduino IDE

このプロジェクトについて

Arduinoベースの四足動物！ Quadrupedは4本足のボットの略で、基本的には4本足のクモのように見えるので、クモがどのように歩くかを学び、Arduinoで複製してみましょう。

備品：

ステップ1：必要なコンポーネント

• 1 X ArduinoMegaまたはArduinoUno
• 1XドリルドPCB
• 12 Xサーボモーター（9g）
• 1 XHC-SR04超音波センサー
• 4 X RGB LED
• 段ボール

ステップ2：CGを維持する

歩行中の主な要因は重心（CG）です。重心が体の中心にとどまり、一定の限界でCGが中心から外れるとバランスが崩れ、転倒します

では、歩きながらCGを維持する方法を見てみましょう。

すべての脚が45度の場合、CGは完全に中央にメインになりますが、いずれかの脚を動かすと、cgがその側に移動するため、その側に落下します。

したがって、これを回避するために、どちらかの端の脚はボットのサイズに基づいて45度を超える角度に維持されるため、3つの脚は三角形を形成し、CGはその内側にあり、4番目の脚は自由に移動してCGになります。三角形の内側に残ります。

ステップ3：ウォーキング手順

• これは開始位置で、片側に2本の脚（C、D）が伸び、他の2本の脚（A、B）が内側に引っ張られています。
• 右上の脚（B）が持ち上げられ、ロボットのはるか前方に手を伸ばします。
• すべての脚が後方に移動し、体が前方に移動します。
• 左後脚（D）が持ち上げられ、体に沿って前に進みます。この位置は開始位置の鏡像です。
• 左上の脚（B）が持ち上げられ、ロボットのはるか前方に手を伸ばします。
• ここでも、すべての脚が後方に移動し、体が前方に移動します。
• 右後脚が持ち上げられ（B）、体に戻り、開始位置に戻ります。

ステップ4：四足動物の計画

ステップ5：ボディの構築

PDFに従って本体を作成します。

ステップ6：回路接続

要件に応じて独自のシールドを作成します。arduinomegaには15pwmピンがあり、そのうち12個をサーボ接続に使用し、3個をRBG LEDに使用し、任意の2個のピンを超音波センサーに使用します。

ステップ7：サーボの初期化

• プログラムをarduinomegaにアップロードし、写真に従って脚の組み立てを開始します

  #include サーボサーボ[4] [3]; //サーボのポートを定義constintservo_pin [4] [3] ={{10,11,2}、{3,4 、5}、{6,7,8}、{9、12、13}}; void setup（）{//すべてのサーボを初期化for（int i =0; i <4; i ++）{for（int j =0; j <3; j ++）{servo [i] [j] .attach（servo_pin [i] [j]）; delay（20）; }}} void loop（void）{for（int i =0; i <4; i ++）{for（int j =0; j <3; j ++）{servo [i] [j] .write（90）; delay（20）; }}}  

ステップ8：最終ステップ

  / *インクルード----------------------------------------- ------------------------- * /＃include  //サーボを定義および制御する＃include  //すべてのサーボを管理するためのタイマーを設定するには#defineledred 46＃define ledblue 44＃define ledgreen 45 / *サーボ--------------------------- ----------------------------------------- * /// 4つのサーボを12個定義legsServoサーボ[4] [3]; //サーボのポートを定義constintservo_pin [4] [3] ={{2、3、4}、{20、6、7}、{8、9、17}、{ 16、12、13}}; / *ロボットのサイズ------------------------------------ --------------------- * / const float length_a =55; const float length_b =77.5; const float length_c =27.5; const float length_side =71; const float z_absolute =-28; / *移動の定数----------------------------------------- ----------- * / const float z_default =-50、z_up =-30、z_boot =z_absolute; const float x_default =62、x_offset =0; const float y_start =0、y_step =40; const float y_default =x_default; / *移動用の変数---------------- ------------------------------------ * / volatile float site_now [4] [3]; //各legvolatilefloatの終わりのリアルタイム座標site_expect [4] [3]; //各レッグフロートの終わりの予想される座標temp_speed [4] [3]; //各軸の速度。各movementfloatmove_speedの前に再計算する必要があります。 //移動speedfloatspeed_multiple =1; //移動速度multipleconstfloat spot_turn_speed =4; const float leg_move_speed =8; const float body_move_speed =3; const float Stand_seat_speed =1; volatile int rest_counter; // + 1 / 0.02s、自動レスト用//関数のパラメータconst float KEEP =255; //計算用のPIを定義constfloat pi =3.1415926; / *ターンの定数------------- ------------------------------------------- * /// temp lengthconst float temp_a =sqrt（pow（2 * x_default + length_side、2）+ pow（y_step、2））; const float temp_b =2 *（y_start + y_step）+ length_side; const float temp_c =sqrt（pow（2 * x_default + length_side 、2）+ pow（2 * y_start + y_step + length_side、2））; const float temp_alpha =acos（（pow（temp_a、2）+ pow（temp_b、2）-pow（temp_c、2））/ 2 / temp_a / temp_b）; // turnconstfloatのサイトturn_x1 =（temp_a --length_side）/ 2; const float turn_y1 =y_start + y_step / 2; const float turn_x0 =turn_x1-temp_b * cos（temp_alpha）; const float turn_y0 =temp_b * sin （temp_alpha）-turn_y1-length_side; / * ---------------------------------------- ----------------------------------- * // *-セットアップ機能-------- -------------------------------------------------- ----------------- * / void setup（）{pi nMode（ledred、OUTPUT）; pinMode（ledblue、OUTPUT）; pinMode（ledgreen、OUTPUT）; //デバッグ用のシリアルを開始Serial.begin（115200）; Serial.println（ "ロボットが初期化を開始します"）; //デフォルトパラメータを初期化しますset_site（0、x_default --x_offset、y_start + y_step、z_boot）; set_site（1、x_default-x_offset、y_start + y_step、z_boot）; set_site（2、x_default + x_offset、y_start、z_boot）; set_site（3、x_default + x_offset、y_start、z_boot）; for（int i =0; i <4; i ++）{for（int j =0; j <3; j ++）{site_now [i] [j] =site_expect [i] [j]; }} //サーボサービスを開始するFlexiTimer2 ::set（20、servo_service）; FlexiTimer2 ::start（）; Serial.println（ "サーボサービスが開始されました"）; //サーボを初期化しますservo_attach（）; Serial.println（ "サーボが初期化されました"）; Serial.println（ "ロボットの初期化が完了しました"）;} voidservo_attach（void）{for（int i =0; i <4; i ++）{for（int j =0; j <3; j ++）{servo [i] [j] .attach（servo_pin [i] [j]）; delay（100）; }}} voidservo_detach（void）{for（int i =0; i <4; i ++）{for（int j =0; j <3; j ++）{servo [i] [j] .detach（）; delay（100）; }}} / *-ループ関数------------------------------------------ --------------------------------- * / void loop（）{analogWrite（ledred、255）; Serial.println（ "スタンド"）;台（）; delay（2000）; analogWrite（ledred、0）; analogWrite（ledblue、255）; Serial.println（ "ステップフォワード"）; step_forward（5）; delay（2000）; analogWrite（ledblue、0）; analogWrite（ledgreen、255）; Serial.println（ "ステップバック"）; step_back（5）; delay（2000）; analogWrite（ledgreen、0）; analogWrite（ledred、255）; analogWrite（ledblue、255）; Serial.println（ "左折"）; turn_left（5）; delay（2000）; analogWrite（ledgreen、255）; analogWrite（ledred、0）; analogWrite（ledblue、255）; Serial.println（ "右折"）; turn_right（5）; delay（2000）; analogWrite（ledgreen、255）; analogWrite（ledred、255）; analogWrite（ledblue、0）; Serial.println（ "ハンドウェーブ"）; hand_wave（3）; delay（2000）; Serial.println（ "ハンドウェーブ"）; hand_shake（3）; delay（2000）; int x =100; for（int i =0; i <5; i ++）{analogWrite（ledgreen、255）; analogWrite（ledred、255）; //白analogWrite（ledblue、255）; delay（x）; analogWrite（ledgreen、255）; //黄色analogWrite（ledred、255）; analogWrite（ledblue、0）; delay（x）; analogWrite（ledgreen、255）; //シアンanalogWrite（ledred、0）; analogWrite（ledblue、255）; delay（x）; analogWrite（ledgreen、0）; analogWrite（ledred、255）; //紫analogWrite（ledblue、255）; delay（x）; analogWrite（ledgreen、0）; analogWrite（ledred、255）; // red analogWrite（ledblue、0）; delay（x）; analogWrite（ledgreen、0）; //青analogWrite（ledred、0）; analogWrite（ledblue、255）; delay（x）; analogWrite（ledgreen、255）; analogWrite（ledred、0）; analogWrite（ledblue、0）; //緑のdelay（x）; } analogWrite（ledgreen、0）; analogWrite（ledred、0）; analogWrite（ledblue、0）; //Serial.println（ "ボディダンス"）; // body_dance（10）; // delay（2000）; //Serial.println("Sit "）; // sit（）; delay（1000）;} / *-sit-ブロッキング関数------------------------------------- -------------------------------------- * / void sit（void）{move_speed =Stand_seat_speed; for（int leg =0; leg <4; leg ++）{set_site（leg、KEEP、KEEP、z_boot）; } wait_all_reach（）;} / *-スタンド-ブロッキング関数------------------------------------- -------------------------------------- * / void Stand（void）{move_speed =Stand_seat_speed; for（int leg =0; leg <4; leg ++）{set_site（leg、KEEP、KEEP、z_default）; } wait_all_reach（）;} / *-左に曲がるスポット-ブロッキング関数-回したいパラメータのステップステップ--------------------------- ------------------------------------------------ * / void turn_left（unsigned int step）{move_speed =spot_turn_speed; while（step-> 0）{if（site_now [3] [1] ==y_start）{//レッグ3＆1 move set_site（3、x_default + x_offset、y_start、z_up）; wait_all_reach（）; set_site（0、turn_x1-x_offset、turn_y1、z_default）; set_site（1、turn_x0-x_offset、turn_y0、z_default）; set_site（2、turn_x1 + x_offset、turn_y1、z_default）; set_site（3、turn_x0 + x_offset、turn_y0、z_up）; wait_all_reach（）; set_site（3、turn_x0 + x_offset、turn_y0、z_default）; wait_all_reach（）; set_site（0、turn_x1 + x_offset、turn_y1、z_default）; set_site（1、turn_x0 + x_offset、turn_y0、z_default）; set_site（2、turn_x1-x_offset、turn_y1、z_default）; set_site（3、turn_x0-x_offset、turn_y0、z_default）; wait_all_reach（）; set_site（1、turn_x0 + x_offset、turn_y0、z_up）; wait_all_reach（）; set_site（0、x_default + x_offset、y_start、z_default）; set_site（1、x_default + x_offset、y_start、z_up）; set_site（2、x_default-x_offset、y_start + y_step、z_default）; set_site（3、x_default-x_offset、y_start + y_step、z_default）; wait_all_reach（）; set_site（1、x_default + x_offset、y_start、z_default）; wait_all_reach（）; } else {//レッグ0＆2 move set_site（0、x_default + x_offset、y_start、z_up）; wait_all_reach（）; set_site（0、turn_x0 + x_offset、turn_y0、z_up）; set_site（1、turn_x1 + x_offset、turn_y1、z_default）; set_site（2、turn_x0-x_offset、turn_y0、z_default）; set_site（3、turn_x1-x_offset、turn_y1、z_default）; wait_all_reach（）; set_site（0、turn_x0 + x_offset、turn_y0、z_default）; wait_all_reach（）; set_site（0、turn_x0-x_offset、turn_y0、z_default）; set_site（1、turn_x1-x_offset、turn_y1、z_default）; set_site（2、turn_x0 + x_offset、turn_y0、z_default）; set_site（3、turn_x1 + x_offset、turn_y1、z_default）; wait_all_reach（）; set_site（2、turn_x0 + x_offset、turn_y0、z_up）; wait_all_reach（）; set_site（0、x_default-x_offset、y_start + y_step、z_default）; set_site（1、x_default-x_offset、y_start + y_step、z_default）; set_site（2、x_default + x_offset、y_start、z_up）; set_site（3、x_default + x_offset、y_start、z_default）; wait_all_reach（）; set_site（2、x_default + x_offset、y_start、z_default）; wait_all_reach（）; }}} / *-右に曲がるスポット-ブロック機能-回したいパラメータのステップステップ------------------------------ --------------------------------------------- * / void turn_right（ unsigned int step）{move_speed =spot_turn_speed; while（step-> 0）{if（site_now [2] [1] ==y_start）{// leg 2＆0 move set_site（2、x_default + x_offset、y_start、z_up）; wait_all_reach（）; set_site（0、turn_x0-x_offset、turn_y0、z_default）; set_site（1、turn_x1-x_offset、turn_y1、z_default）; set_site（2、turn_x0 + x_offset、turn_y0、z_up）; set_site（3、turn_x1 + x_offset、turn_y1、z_default）; wait_all_reach（）; set_site（2、turn_x0 + x_offset、turn_y0、z_default）; wait_all_reach（）; set_site（0、turn_x0 + x_offset、turn_y0、z_default）; set_site（1、turn_x1 + x_offset、turn_y1、z_default）; set_site（2、turn_x0-x_offset、turn_y0、z_default）; set_site（3、turn_x1-x_offset、turn_y1、z_default）; wait_all_reach（）; set_site（0、turn_x0 + x_offset、turn_y0、z_up）; wait_all_reach（）; set_site（0、x_default + x_offset、y_start、z_up）; set_site（1、x_default + x_offset、y_start、z_default）; set_site（2、x_default-x_offset、y_start + y_step、z_default）; set_site（3、x_default-x_offset、y_start + y_step、z_default）; wait_all_reach（）; set_site（0、x_default + x_offset、y_start、z_default）; wait_all_reach（）; } else {//レッグ1＆3 move set_site（1、x_default + x_offset、y_start、z_up）; wait_all_reach（）; set_site（0、turn_x1 + x_offset、turn_y1、z_default）; set_site（1、turn_x0 + x_offset、turn_y0、z_up）; set_site（2、turn_x1-x_offset、turn_y1、z_default）; set_site（3、turn_x0-x_offset、turn_y0、z_default）; wait_all_reach（）; set_site（1、turn_x0 + x_offset、turn_y0、z_default）; wait_all_reach（）; set_site（0、turn_x1-x_offset、turn_y1、z_default）; set_site（1、turn_x0-x_offset、turn_y0、z_default）; set_site（2、turn_x1 + x_offset、turn_y1、z_default）; set_site（3、turn_x0 + x_offset、turn_y0、z_default）; wait_all_reach（）; set_site（3、turn_x0 + x_offset、turn_y0、z_up）; wait_all_reach（）; set_site（0、x_default-x_offset、y_start + y_step、z_default）; set_site（1、x_default-x_offset、y_start + y_step、z_default）; set_site（2、x_default + x_offset、y_start、z_default）; set_site（3、x_default + x_offset、y_start、z_up）; wait_all_reach（）; set_site（3、x_default + x_offset、y_start、z_default）; wait_all_reach（）; }}} / *-進む-ブロック機能-パラメータのステップステップが進みたい-------------------------------- ------------------------------------------- * / void step_forward（unsigned int step）{move_speed =leg_move_speed; while（step-> 0）{if（site_now [2] [1] ==y_start）{//レッグ2＆1 move set_site（2、x_default + x_offset、y_start、z_up）; wait_all_reach（）; set_site（2、x_default + x_offset、y_start + 2 * y_step、z_up）; wait_all_reach（）; set_site（2、x_default + x_offset、y_start + 2 * y_step、z_default）; wait_all_reach（）; move_speed =body_move_speed; set_site（0、x_default + x_offset、y_start、z_default）; set_site（1、x_default + x_offset、y_start + 2 * y_step、z_default）; set_site（2、x_default-x_offset、y_start + y_step、z_default）; set_site（3、x_default-x_offset、y_start + y_step、z_default）; wait_all_reach（）; move_speed =leg_move_speed; set_site（1、x_default + x_offset、y_start + 2 * y_step、z_up）; wait_all_reach（）; set_site（1、x_default + x_offset、y_start、z_up）; wait_all_reach（）; set_site（1、x_default + x_offset、y_start、z_default）; wait_all_reach（）; } else {//レッグ0＆3 move set_site（0、x_default + x_offset、y_start、z_up）; wait_all_reach（）; set_site（0、x_default + x_offset、y_start + 2 * y_step、z_up）; wait_all_reach（）; set_site（0、x_default + x_offset、y_start + 2 * y_step、z_default）; wait_all_reach（）; move_speed =body_move_speed; set_site（0、x_default-x_offset、y_start + y_step、z_default）; set_site（1、x_default-x_offset、y_start + y_step、z_default）; set_site（2、x_default + x_offset、y_start、z_default）; set_site（3、x_default + x_offset、y_start + 2 * y_step、z_default）; wait_all_reach（）; move_speed =leg_move_speed; set_site（3、x_default + x_offset、y_start + 2 * y_step、z_up）; wait_all_reach（）; set_site（3、x_default + x_offset、y_start、z_up）; wait_all_reach（）; set_site（3、x_default + x_offset、y_start、z_default）; wait_all_reach（）; }}} / *-戻る-ブロック機能-パラメータのステップステップを実行したい-------------------------------- ------------------------------------------- * / void step_back（unsigned int step）{move_speed =leg_move_speed; while（step-> 0）{if（site_now [3] [1] ==y_start）{// leg 3＆0 move set_site（3、x_default + x_offset、y_start、z_up）; wait_all_reach（）; set_site（3、x_default + x_offset、y_start + 2 * y_step、z_up）; wait_all_reach（）; set_site（3、x_default + x_offset、y_start + 2 * y_step、z_default）; wait_all_reach（）; move_speed =body_move_speed; set_site（0、x_default + x_offset、y_start + 2 * y_step、z_default）; set_site（1、x_default + x_offset、y_start、z_default）; set_site（2、x_default-x_offset、y_start + y_step、z_default）; set_site（3、x_default-x_offset、y_start + y_step、z_default）; wait_all_reach（）; move_speed =leg_move_speed; set_site（0、x_default + x_offset、y_start + 2 * y_step、z_up）; wait_all_reach（）; set_site（0、x_default + x_offset、y_start、z_up）; wait_all_reach（）; set_site（0、x_default + x_offset、y_start、z_default）; wait_all_reach（）; } else {//レッグ1＆2 move set_site（1、x_default + x_offset、y_start、z_up）; wait_all_reach（）; set_site（1、x_default + x_offset、y_start + 2 * y_step、z_up）; wait_all_reach（）; set_site（1、x_default + x_offset、y_start + 2 * y_step、z_default）; wait_all_reach（）; move_speed =body_move_speed; set_site（0、x_default-x_offset、y_start + y_step、z_default）; set_site（1、x_default-x_offset、y_start + y_step、z_default）; set_site（2、x_default + x_offset、y_start + 2 * y_step、z_default）; set_site（3、x_default + x_offset、y_start、z_default）; wait_all_reach（）; move_speed =leg_move_speed; set_site（2、x_default + x_offset、y_start + 2 * y_step、z_up）; wait_all_reach（）; set_site（2、x_default + x_offset、y_start、z_up）; wait_all_reach（）; set_site（2、x_default + x_offset、y_start、z_default）; wait_all_reach（）; }}} // RegisHsuvoidによって追加body_left（int i）{set_site（0、site_now [0] [0] + i、KEEP、KEEP）; set_site（1、site_now [1] [0] + i、KEEP、KEEP）; set_site（2、site_now [2] [0] -i、KEEP、KEEP）; set_site（3、site_now [3] [0] -i、KEEP、KEEP）; wait_all_reach（）;} void body_right（int i）{set_site（0、site_now [0] [0] -i、KEEP、KEEP）; set_site（1、site_now [1] [0] -i、KEEP、KEEP）; set_site（2、site_now [2] [0] + i、KEEP、KEEP）; set_site（3、site_now [3] [0] + i、KEEP、KEEP）; wait_all_reach（）;} void hand_wave（int i）{float x_tmp; float y_tmp; float z_tmp; move_speed =1; if（site_now [3] [1] ==y_start）{body_right（15）; x_tmp =site_now [2] [0]; y_tmp =site_now [2] [1]; z_tmp =site_now [2] [2]; move_speed =body_move_speed; for（int j =0; j  i / 4）move_speed =body_dance_speed * 2; if（j> i / 2）move_speed =body_dance_speed * 3; set_site（0、KEEP、y_default-20、KEEP）; set_site（1、KEEP、y_default + 20、KEEP）; set_site（2、KEEP、y_default-20、KEEP）; set_site（3、KEEP、y_default + 20、KEEP）; wait_all_reach（）; set_site（0、KEEP、y_default + 20、KEEP）; set_site（1、KEEP、y_default-20、KEEP）; set_site（2、KEEP、y_default + 20、KEEP）; set_site（3、KEEP、y_default-20、KEEP）; wait_all_reach（）; } move_speed =body_dance_speed; head_down（30）;} / *-マイクロサーボサービス/タイマー割り込み関数/ 50Hz-サイトが期待される場合、この関数は終点を直線で移動します-temp_speed [4] [3]は期待されるサイトを設定する前に設定する必要があります、終点が一直線に動くことを確認し、移動速度を決定します。 -------------------------------------------------- ------------------------- * / voidservo_service（void）{sei（）;静的フロートアルファ、ベータ、ガンマ; for（int i =0; i <4; i ++）{for（int j =0; j <3; j ++）{if（abs（site_now [i] [j] --site_expect [i] [j]）> =abs（temp_speed [i] [j]））site_now [i] [j] + =temp_speed [i] [j]; else site_now [i] [j] =site_expect [i] [j]; } cartesian_to_polar（alpha、beta、gamma、site_now [i] [0]、site_now [i] [1]、site_now [i] [2]）; Polar_to_servo（i、アルファ、ベータ、ガンマ）; } rest_counter ++;} / *-エンドポイントの期待サイトの1つを設定します-この機能は同時にtemp_speed [4] [3]を設定します-非ブロッキング関数--------------- -------------------------------------------------- ---------- * / void set_site（int leg、float x、float y、float z）{float length_x =0、length_y =0、length_z =0; if（x！=KEEP）length_x =x --site_now [leg] [0]; if（y！=KEEP）length_y =y --site_now [leg] [1]; if（z！=KEEP）length_z =z --site_now [leg] [2]; float length =sqrt（pow（length_x、2）+ pow（length_y、2）+ pow（length_z、2））; temp_speed [leg] [0] =length_x / length * move_speed * speed_multiple; temp_speed [leg] [1] =length_y / length * move_speed * speed_multiple; temp_speed [leg] [2] =length_z / length * move_speed * speed_multiple; if（x！=KEEP）site_expect [leg] [0] =x; if（y！=KEEP）site_expect [leg] [1] =y; if（z！=KEEP）site_expect [leg] [2] =z;} / *-エンドポイントの1つがサイトを予期するように移動するのを待ちます-ブロッキング関数----------------- -------------------------------------------------- -------- * / void wait_reach（int leg）{while（1）if（site_now [leg] [0] ==site_expect [leg] [0]）if（site_now [leg] [1] ==site_expect [leg] [1]）if（site_now [leg] [2] ==site_expect [leg] [2]）break;} / *-すべてのエンドポイントが移動してサイトを期待するのを待つ-ブロッキング関数---- -------------------------------------------------- --------------------- * / void wait_all_reach（void）{for（int i =0; i <4; i ++）wait_reach（i）;} / * -デカルトから極へのトランスサイト-数学モデル2/2 ------------------------------------- -------------------------------------- * / void cartesian_to_polar（volatile float＆alpha、volatile float＆beta 、volatile float＆gamma、volatile float x、volatile float y、volatile float z）{// wz度を計算するfloatv、w; w =（x> =0？1：-1）*（sqrt（pow（x、2）+ pow（y、2）））; v =w --length_c; alpha =atan2（z、v）+ acos（（pow（length_a、2）-pow（length_b、2）+ pow（v、2）+ pow（z、2））/ 2 / length_a / sqrt（pow（v 、2）+ pow（z、2）））; beta =acos（（pow（length_a、2）+ pow（length_b、2）-pow（v、2）-pow（z、2））/ 2 / length_a / length_b）; // x-y-z次数を計算するガンマ=（w> =0）？ atan2（y、x）：atan2（-y、-x）; //トランス度pi-> 180 alpha =alpha / pi * 180;ベータ=ベータ/パイ* 180; gamma =gamma / pi * 180;} / *-極からマイクロサーボへのトランスサイト-事実への数学モデルマップ-eepromに保存されたエラーが追加されます----------------- -------------------------------------------------- -------- * / void Polar_to_servo（int leg、float alpha、float beta、float gamma）{if（leg ==0）{alpha =90-alpha;ベータ=ベータ;ガンマ+ =90; } else if（leg ==1）{alpha + =90;ベータ=180-ベータ;ガンマ=90-ガンマ; } else if（leg ==2）{alpha + =90;ベータ=180-ベータ;ガンマ=90-ガンマ; } else if（leg ==3）{alpha =90-alpha;ベータ=ベータ;ガンマ+ =90; }サーボ[レッグ] [0] .write（アルファ）;サーボ[レッグ] [1] .write（ベータ）;サーボ[レッグ] [2] .write（ガンマ）;}  

LEDピンを接続します

• これで、四足動物の準備が整いました。
• プログラムをアップロードします。
• プログラムで定義されているピンに従ってサーボを接続します。

コード

• スパイダー
• spider_fix.ino

スパイダー Arduino

 / *含む-------------------------------------------- ---------------------- * /＃include  //サーボを定義および制御するには#include  //設定するにはすべてのサーボを管理するためのタイマー#defineledred 46＃define ledblue 44＃define ledgreen 45 / *サーボ------------------------------ -------------------------------------- * /// 4本の脚に12個のサーボを定義サーボサーボ[ 4] [3]; //サーボのポートを定義constintservo_pin [4] [3] ={{2、3、4}、{20、6、7}、{8、9、17}、{16、12 、13}}; / *ロボットのサイズ--------------------------------------- ------------------ * / const float length_a =55; const float length_b =77.5; const float length_c =27.5; const float length_side =71; const float z_absolute =-28; / *移動の定数-------------------------------------------- -------- * / const float z_default =-50、z_up =-30、z_boot =z_absolute; const float x_default =62、x_offset =0; const float y_start =0、y_step =40; const float y_default =x_default; / *移動用の変数---------------------- ------------------------------ * / volatile float site_now [4] [3]; //各legvolatilefloatの終わりのリアルタイム座標site_expect [4] [3]; //各レッグフロートの終わりの予想される座標temp_speed [4] [3]; //各軸の速度。各movementfloatmove_speedの前に再計算する必要があります。 //移動speedfloatspeed_multiple =1; //移動速度multipleconstfloat spot_turn_speed =4; const float leg_move_speed =8; const float body_move_speed =3; const float Stand_seat_speed =1; volatile int rest_counter; // + 1 / 0.02s、自動レスト用//関数のパラメータconst float KEEP =255; //計算用のPIを定義constfloat pi =3.1415926; / *ターンの定数------------- ------------------------------------------- * /// temp lengthconst float temp_a =sqrt（pow（2 * x_default + length_side、2）+ pow（y_step、2））; const float temp_b =2 *（y_start + y_step）+ length_side; const float temp_c =sqrt（pow（2 * x_default + length_side 、2）+ pow（2 * y_start + y_step + length_side、2））; const float temp_alpha =acos（（pow（temp_a、2）+ pow（temp_b、2）-pow（temp_c、2））/ 2 / temp_a / temp_b）; // turnconstfloatのサイトturn_x1 =（temp_a --length_side）/ 2; const float turn_y1 =y_start + y_step / 2; const float turn_x0 =turn_x1-temp_b * cos（temp_alpha）; const float turn_y0 =temp_b * sin （temp_alpha）-turn_y1-length_side; / * ---------------------------------------- ----------------------------------- * // *-セットアップ機能-------- -------------------------------------------------- ----------------- * / void setup（）{pi nMode（ledred、OUTPUT）; pinMode（ledblue、OUTPUT）; pinMode（ledgreen、OUTPUT）; //start serial for debug Serial.begin(115200); Serial.println("Robot starts initialization"); //initialize default parameter set_site(0, x_default - x_offset, y_start + y_step, z_boot); set_site(1, x_default - x_offset, y_start + y_step, z_boot); set_site(2, x_default + x_offset, y_start, z_boot); set_site(3, x_default + x_offset, y_start, z_boot); for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { site_now[i][j] =site_expect[i][j]; } } //start servo service FlexiTimer2::set(20, servo_service); FlexiTimer2::start(); Serial.println("Servo service started"); //initialize servos servo_attach(); Serial.println("Servos initialized"); Serial.println("Robot initialization Complete");}void servo_attach(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].attach(servo_pin[i][j]); delay(100); } }}void servo_detach(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].detach(); delay(100); } }}/* - loop function ---------------------------------------------------------------------------*/void loop(){ analogWrite(ledred,255); Serial.println("Stand"); stand(); delay(2000); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Step forward"); step_forward(5); delay(2000); analogWrite(ledblue,0); analogWrite(ledgreen,255); Serial.println("Step back"); step_back(5); delay(2000); analogWrite(ledgreen,0); analogWrite(ledred,255); analogWrite(ledblue,255); Serial.println("Turn left"); turn_left(5); delay(2000); analogWrite(ledgreen,255); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Turn right"); turn_right(5); delay(2000); analogWrite(ledgreen,255); analogWrite(ledred,255); analogWrite(ledblue,0); Serial.println("Hand wave"); hand_wave(3); delay(2000); Serial.println("Hand wave"); hand_shake(3); delay(2000); int x=100; for(int i=0;i<5;i++) { analogWrite(ledgreen,255); analogWrite(ledred,255);//white analogWrite(ledblue,255); delay(x); analogWrite(ledgreen,255);//yellow analogWrite(ledred,255); analogWrite(ledblue,0); delay(x); analogWrite(ledgreen,255);//cyan analogWrite(ledred,0); analogWrite(ledblue,255); delay(x); analogWrite(ledgreen,0); analogWrite(ledred,255);//purple analogWrite(ledblue,255); delay(x); analogWrite(ledgreen,0); analogWrite(ledred,255);//red analogWrite(ledblue,0); delay(x); analogWrite(ledgreen,0);//blue analogWrite(ledred,0); analogWrite(ledblue,255); delay(x); analogWrite(ledgreen,255); analogWrite(ledred,0); analogWrite(ledblue,0); //green delay(x); } analogWrite(ledgreen,0); analogWrite(ledred,0); analogWrite(ledblue,0); //Serial.println("Body dance"); //body_dance(10); // delay(2000); //Serial.println("Sit"); // sit(); delay(1000);}/* - sit - blocking function ---------------------------------------------------------------------------*/void sit(void){ move_speed =stand_seat_speed; for (int leg =0; leg <4; leg++) { set_site(leg, KEEP, KEEP, z_boot); } wait_all_reach();}/* - stand - blocking function ---------------------------------------------------------------------------*/void stand(void){ move_speed =stand_seat_speed; for (int leg =0; leg <4; leg++) { set_site(leg, KEEP, KEEP, z_default); } wait_all_reach();}/* - spot turn to left - blocking function - parameter step steps wanted to turn ---------------------------------------------------------------------------*/void turn_left(unsigned int step){ move_speed =spot_turn_speed; while (step--> 0) { if (site_now[3][1] ==y_start) { //leg 3&1 move set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x1 - x_offset, turn_y1, z_default); set_site(1, turn_x0 - x_offset, turn_y0, z_default); set_site(2, turn_x1 + x_offset, turn_y1, z_default); set_site(3, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(3, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x1 + x_offset, turn_y1, z_default); set_site(1, turn_x0 + x_offset, turn_y0, z_default); set_site(2, turn_x1 - x_offset, turn_y1, z_default); set_site(3, turn_x0 - x_offset, turn_y0, z_default); wait_all_reach(); set_site(1, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); set_site(1, x_default + x_offset, y_start, z_up); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 0&2 move set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_up); set_site(1, turn_x1 + x_offset, turn_y1, z_default); set_site(2, turn_x0 - x_offset, turn_y0, z_default); set_site(3, turn_x1 - x_offset, turn_y1, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x0 - x_offset, turn_y0, z_default); set_site(1, turn_x1 - x_offset, turn_y1, z_default); set_site(2, turn_x0 + x_offset, turn_y0, z_default); set_site(3, turn_x1 + x_offset, turn_y1, z_default); wait_all_reach(); set_site(2, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_up); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}/* - spot turn to right - blocking function - parameter step steps wanted to turn ---------------------------------------------------------------------------*/void turn_right(unsigned int step){ move_speed =spot_turn_speed; while (step--> 0) { if (site_now[2][1] ==y_start) { //leg 2&0 move set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x0 - x_offset, turn_y0, z_default); set_site(1, turn_x1 - x_offset, turn_y1, z_default); set_site(2, turn_x0 + x_offset, turn_y0, z_up); set_site(3, turn_x1 + x_offset, turn_y1, z_default); wait_all_reach(); set_site(2, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_default); set_site(1, turn_x1 + x_offset, turn_y1, z_default); set_site(2, turn_x0 - x_offset, turn_y0, z_default); set_site(3, turn_x1 - x_offset, turn_y1, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_up); set_site(1, x_default + x_offset, y_start, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 1&3 move set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x1 + x_offset, turn_y1, z_default); set_site(1, turn_x0 + x_offset, turn_y0, z_up); set_site(2, turn_x1 - x_offset, turn_y1, z_default); set_site(3, turn_x0 - x_offset, turn_y0, z_default); wait_all_reach(); set_site(1, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x1 - x_offset, turn_y1, z_default); set_site(1, turn_x0 - x_offset, turn_y0, z_default); set_site(2, turn_x1 + x_offset, turn_y1, z_default); set_site(3, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(3, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_default); set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}/* - go forward - blocking function - parameter step steps wanted to go ---------------------------------------------------------------------------*/void step_forward(unsigned int step){ move_speed =leg_move_speed; while (step--> 0) { if (site_now[2][1] ==y_start) { //leg 2&1 move set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =body_move_speed; set_site(0, x_default + x_offset, y_start, z_default); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); move_speed =leg_move_speed; set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 0&3 move set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =body_move_speed; set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_default); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =leg_move_speed; set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}/* - go back - blocking function - parameter step steps wanted to go ---------------------------------------------------------------------------*/void step_back(unsigned int step){ move_speed =leg_move_speed; while (step--> 0) { if (site_now[3][1] ==y_start) { //leg 3&0 move set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =body_move_speed; set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(1, x_default + x_offset, y_start, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); move_speed =leg_move_speed; set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 1&2 move set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =body_move_speed; set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); move_speed =leg_move_speed; set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}// add by RegisHsuvoid body_left(int i){ set_site(0, site_now[0][0] + i, KEEP, KEEP); set_site(1, site_now[1][0] + i, KEEP, KEEP); set_site(2, site_now[2][0] - i, KEEP, KEEP); set_site(3, site_now[3][0] - i, KEEP, KEEP); wait_all_reach();}void body_right(int i){ set_site(0, site_now[0][0] - i, KEEP, KEEP); set_site(1, site_now[1][0] - i, KEEP, KEEP); set_site(2, site_now[2][0] + i, KEEP, KEEP); set_site(3, site_now[3][0] + i, KEEP, KEEP); wait_all_reach();}void hand_wave(int i){ float x_tmp; float y_tmp; float z_tmp; move_speed =1; if (site_now[3][1] ==y_start) { body_right(15); x_tmp =site_now[2][0]; y_tmp =site_now[2][1]; z_tmp =site_now[2][2]; move_speed =body_move_speed; for (int j =0; j  i / 4) move_speed =body_dance_speed * 2; if (j> i / 2) move_speed =body_dance_speed * 3; set_site(0, KEEP, y_default - 20, KEEP); set_site(1, KEEP, y_default + 20, KEEP); set_site(2, KEEP, y_default - 20, KEEP); set_site(3, KEEP, y_default + 20, KEEP); wait_all_reach(); set_site(0, KEEP, y_default + 20, KEEP); set_site(1, KEEP, y_default - 20, KEEP); set_site(2, KEEP, y_default + 20, KEEP); set_site(3, KEEP, y_default - 20, KEEP); wait_all_reach(); } move_speed =body_dance_speed; head_down(30);}/* - microservos service /timer interrupt function/50Hz - when set site expected,this function move the end point to it in a straight line - temp_speed[4][3] should be set before set expect site,it make sure the end point move in a straight line,and decide move speed. ---------------------------------------------------------------------------*/void servo_service(void){ sei(); static float alpha, beta, gamma; for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { if (abs(site_now[i][j] - site_expect[i][j])>=abs(temp_speed[i][j])) site_now[i][j] +=temp_speed[i][j]; else site_now[i][j] =site_expect[i][j]; } cartesian_to_polar(alpha, beta, gamma, site_now[i][0], site_now[i][1], site_now[i][2]); polar_to_servo(i, alpha, beta, gamma); } rest_counter++;}/* - set one of end points' expect site - this founction will set temp_speed[4][3] at same time - non - blocking function ---------------------------------------------------------------------------*/void set_site(int leg, float x, float y, float z){ float length_x =0, length_y =0, length_z =0; if (x !=KEEP) length_x =x - site_now[leg][0]; if (y !=KEEP) length_y =y - site_now[leg][1]; if (z !=KEEP) length_z =z - site_now[leg][2]; float length =sqrt(pow(length_x, 2) + pow(length_y, 2) + pow(length_z, 2)); temp_speed[leg][0] =length_x / length * move_speed * speed_multiple; temp_speed[leg][1] =length_y / length * move_speed * speed_multiple; temp_speed[leg][2] =length_z / length * move_speed * speed_multiple; if (x !=KEEP) site_expect[leg][0] =x; if (y !=KEEP) site_expect[leg][1] =y; if (z !=KEEP) site_expect[leg][2] =z;}/* - wait one of end points move to expect site - blocking function ---------------------------------------------------------------------------*/void wait_reach(int leg){ while (1) if (site_now[leg][0] ==site_expect[leg][0]) if (site_now[leg][1] ==site_expect[leg][1]) if (site_now[leg][2] ==site_expect[leg][2]) break;}/* - wait all of end points move to expect site - blocking function ---------------------------------------------------------------------------*/void wait_all_reach(void){ for (int i =0; i <4; i++) wait_reach(i);}/* - trans site from cartesian to polar - mathematical model 2/2 ---------------------------------------------------------------------------*/void cartesian_to_polar(volatile float &alpha, volatile float &beta, volatile float &gamma, volatile float x, volatile float y, volatile float z){ //calculate w-z degree float v, w; w =(x>=0 ? 1 :-1) * (sqrt(pow(x, 2) + pow(y, 2))); v =w - length_c; alpha =atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2) + pow(z, 2)) / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2))); beta =acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) / 2 / length_a / length_b); //calculate x-y-z degree gamma =(w>=0) ? atan2(y, x) :atan2(-y, -x); //trans degree pi->180 alpha =alpha / pi * 180; beta =beta / pi * 180; gamma =gamma / pi * 180;}/* - trans site from polar to microservos - mathematical model map to fact - the errors saved in eeprom will be add ---------------------------------------------------------------------------*/void polar_to_servo(int leg, float alpha, float beta, float gamma){ if (leg ==0) { alpha =90 - alpha; beta =beta; gamma +=90; } else if (leg ==1) { alpha +=90; beta =180 - beta; gamma =90 - gamma; } else if (leg ==2) { alpha +=90; beta =180 - beta; gamma =90 - gamma; } else if (leg ==3) { alpha =90 - alpha; beta =beta; gamma +=90; } servo[leg][0].write(alpha); servo[leg][1].write(beta); servo[leg][2].write(gamma);}

spider_fix.inoArduino

// Locate the initial position of legs // RegisHsu 2015-09-09#include  Servo servo[4][3];//define servos' portsconst int servo_pin[4][3] ={ {10,11,2}, {3,4,5}, {6,7,8}, {9, 12, 13} };void setup(){ //initialize all servos for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].attach(servo_pin[i][j]); delay(20); } }}void loop(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].write(90); delay(20); } }}

カスタムパーツとエンクロージャー

回路図