Arduinoスパイダーロボット（四足動物）

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

やあみんな！これは、この種の非常に素晴らしい電子プロジェクトを作成しながら、ステップバイステップでガイドする新しいチュートリアルです。これは、「スパイダーロボット」または「四足歩行ロボット」とも呼ばれる「クローラーロボット」です。

ロボット工学技術の急速な進化に誰もが気づいたので、私たちはあなたたちをロボット工学とロボット製作のより高いレベルに連れて行くことにしました。私たちは少し前に、いくつかの基本的な電子プロジェクトとPICTO92のような基本的なロボットをラインフォロワーロボットにすることから始めました。これは、電子的なものに少し慣れて、自分でプロジェクトを発明できるようにするためです。

別のレベルに移動して、コンセプトの基本的なロボットであるこのロボットから始めましたが、プログラムを深く理解すると少し複雑になります。また、これらのガジェットはウェブストアでは非常に高価であるため、このステップバイステップのガイダンスを提供します 自分でスパイダーボットを作成するためのガイド 。

このプロジェクトは、ロボットの外観を改善するためにJLCPCBに注文したカスタマイズされたPCBを入手した後、特別に作成するのに非常に便利です。また、このガイドには、クローラーを簡単に作成できる十分なドキュメントとコードがあります。

このプロジェクトはわずか7日で作成され、ハードウェアの作成と組み立てが完了するまでに2日、コードとAndroidアプリの準備に5日かかりました。それを通してロボットを制御するために。始める前にまず見てみましょう

学習内容：

• プロジェクトの機能に応じて適切なコンポーネントを選択する

• 選択したすべてのコンポーネントを接続する回路を作成する

• すべてのプロジェクトパーツを組み立てます

• ロボットバランスのスケーリング

• Androidアプリの使用。 Bluetooth経由で接続し、システムの操作を開始するには

ステップ1：「スパイダーロボット」とは何ですか？

その名前が示すように、私たちのロボットは、sipderの動きの基本的な表現ですが、8本の足ではなく4本の足しか使用していないため、まったく同じ体の動きを実行しません。

四足動物とも呼ばれます ロボットは4本の脚を持ち、これらの脚を使って動きます。各脚の動きは、ロボットの体の位置を特定し、ロボットの体のバランスを制御するために、他の脚に関連付けられています。

脚式ロボットは、車輪付きロボットよりも地形をうまく処理し、さまざまで動物的な方法で移動します。ただし、これにより脚式ロボットはより複雑になり、多くのメーカーがアクセスしにくくなります。また、サーボモーターまたはステッピングモーターをベースにしており、どちらも車輪付きロボットで使用できるDCモーターよりも高価であるため、4倍の全身を作成するためにメーカーが費やす必要のある製造コストと高い費用もかかります。

利点

4本の脚で受動的な安定性、または積極的に位置を調整せずに立ったままでいることができるため、自然界には四足動物が豊富にあります。同じことがロボットにも当てはまります。 4脚ロボットは、脚の多いロボットよりも安価でシンプルですが、それでも安定性を実現できます。

ステップ2：サーボモーターが主なアクチュエーターです

ウィキペディアで定義されているサーボモーターは、角度または線形の位置、速度、および加速度の正確な制御を可能にする回転アクチュエーターまたは線形アクチュエーターです。[1]位置フィードバック用のセンサーに結合された適切なモーターで構成されます。また、比較的洗練されたコントローラーが必要です。多くの場合、サーボモーター専用に設計された専用モジュールが必要です。

サーボモーターという用語は、閉ループ制御システムでの使用に適したモーターを指すためによく使用されますが、サーボモーターは特定のクラスのモーターではありません。

一般的に、制御信号は方形波パルス列です。制御信号の一般的な周波数は、44Hz、50Hz、および400Hzです。正のパルス幅は、サーボ位置を決定するものです。正のパルス幅が約0.5msの場合、サーボホーンは可能な限り左に偏向します（通常、問題のサーボに応じて約45〜90度）。正のパルス幅が約2.5msから3.0msの場合、サーボは可能な限り右に偏向します。パルス幅が約1.5msの場合、サーボはニュートラル位置を0度に保持します。出力高電圧は、一般的に2.5ボルトから10ボルトの間です（通常3V）。出力低電圧の範囲は-40mV〜0Vです。

ステップ3：PCB作成（JLCPCBによって作成）

JLCPCBについて

JLCPCB（Shenzhen JIALICHUANG Electronic Technology Development Co.、Ltd。）は、中国最大のPCBプロトタイプ企業であり、迅速なPCBプロトタイプと小ロットのPCB生産を専門とするハイテクメーカーです。

PCB製造で10年以上の経験を持つJLCPCBは、国内外で200,000を超える顧客を抱えており、1日あたり8,000を超えるPCBプロトタイピングと少量のPCB生産をオンラインで注文しています。年間生産能力は20万平方メートルです。さまざまな1層、2層、または多層PCB用。 JLCは、大規模で設備の整った、厳格な管理と優れた品質を特徴とするプロのPCBメーカーです。

プロジェクトに戻る

PCBを製造するために、私は多くのPCB生産者からの価格を比較し、この回路を注文するためにJLCPCBを最高のPCBサプライヤーと最も安価なPCBプロバイダーに選びました。ガーバーファイルをアップロードし、PCBの厚さの色や数量などのパラメータを設定するためにクリックするだけで、5日後にPCBを取得するためにわずか2ドルを支払うだけです。

関連するスキームの写真を示しているので、Arduino Nanoを使用してシステム全体を制御しました。また、このプロジェクトをさらに改善するためにロボットスパイダーの形状を設計しました。

ここからCircuit（PDF）ファイルを入手できます。上の写真でわかるように、PCBは非常によく製造されており、私たちが設計したのと同じPCBスパイダーの形状を持っており、すべてのラベルとロゴがはんだ付けの手順をガイドするためにあります。

同じ回路設計を注文したい場合は、ここからこの回路のガーバーファイルをダウンロードすることもできます。

ステップ4：材料

次に、このプロジェクトに必要なコンポーネントを確認しましょう。前述したように、Arduino Nanoを使用して、ロボットの4本の脚の12個のサーボモーターすべてを実行しています。このプロジェクトには、Cozmoの顔を表示するOLEDディスプレイと、Androidアプリを介してロボットを制御するBluetoothモジュールも含まれています。

この種のプロジェクトを作成するには、次のものが必要です：

• -JLCPCBに注文したPCB

• -各レッグに3つのサーボを覚えている12個のサーボモーター：https：//amzn.to/2B25XbG

• -1つのArduinoNano：https：//amzn.to/2MmZsVg

• -HC-06 Bluetoothモジュール：https：//amzn.to/2B1Z3CY

• -1つのOLEDディスプレイ画面：https：//amzn.to/2OySnyn

• -5mm RGB LED：https：//amzn.to/2B56hq3

• -いくつかのヘッダー接続：https：//amzn.to/2nyZg7i

• -そして、ロボット本体は、3Dプリンターを使用して印刷する必要があることを和らげます

ステップ5：ロボットの組み立て

これでPCBの準備が整い、すべてのコンポーネントが非常によくはんだ付けされました。その後、ロボット本体を組み立てる必要があります。手順はとても簡単なので、私が示す手順に従うだけで、最初に各脚を側面に準備して作成する必要があります。 1つは、ジョイント用に2つのサーボモーターと、この小さな取り付けパーツを備えたCoxa、Femur、およびTibiaのプリントパーツが必要です。

ロボットのボディピースについては、ここからSTLファイルをダウンロードできます。

最初のサーボから始めて、それをソケットに入れ、ネジで保持します。その後、アタッチ用のネジを配置せずにサーボの斧を180°に回し、大腿骨が脛骨に接続する次の部分に移動します最初のサーボジョイントアックスとアタッチピースを使用します。脚を完成させる最後のステップは、2番目のジョイントを配置することです。つまり、2番目のサーボが脚の3番目の部分であるCoxaピースを保持します。

次に、すべての脚に対して同じことを繰り返して、4本の脚を準備します。その後、上部シャーシを取り、残りのサーボをソケットに配置してから、各レッグを適切なサーボに接続します。最後に印刷された部分は、回路基板を配置する下部のロボットシャーシだけです。

ステップ6：Androidアプリ

アンドロイドについて話すと、それが可能になります

Bluetoothを介してロボットに接続し、前後に移動したり、左右に回転したりできます。このカラーホイールから目的の色を選択することで、ロボットのライトの色をリアルタイムで制御することもできます。

このリンクからAndroidアプリを無料でダウンロードできます：ここ

ステップ7：Arduinoコードとテストの検証

これでロボットの実行準備がほぼ整いましたが、最初に関節角度を設定する必要があるため、サーボを90度に取り付けて各サーボを正しい位置に配置できるセットアップコードをアップロードします。7Vを接続することを忘れないでください。ロボットを動かすためのDCバッテリー。

次に、Androidアプリを使用してロボットを制御するためのメインプログラムをアップロードする必要があります。これらのリンクからダウンロードできる両方のプログラム：

-サーボコードのスケーリング：ダウンロードリンク-スパイダーロボットメインプログラム：ダウンロードリンク

コードをアップロードした後、メインコードで作成したCozmoロボットの笑顔を表示するために、OLEDディスプレイを接続しました。

上の写真でわかるように、ロボットは私のスマートフォンから送信されたすべての指示と、それをさらに改善するために実行するその他のいくつかの改善に従います。

コード

• スパイダーロボットのメインコード

スパイダーロボットのメインコード Arduino

 / ********************************************** ************************************************** ************************************************** ********************** *-作成者：BELKHIR Mohamed * *-職業:(電気Ingineer）MEGADAS所有者**-主な目的：産業用アプリケーション* * -著作権（c）所有者：無断転載禁止* *-ライセンス：BSD2-条項ライセンス**-日付：2017年4月20日* * ****************** ************************************************** ************************************************** **************************** ********************* / / *************************** ******** ノート **************************************///以下の条件が満たされている場合、変更の有無にかかわらず、ソースおよびバイナリ形式での再配布および使用が許可されます。//*ソースコードの再配布では、上記の著作権表示、この//条件のリスト、および次の免責事項を保持する必要があります。 .// *バイナリ形式での再配布では、上記の著作権表示、//この条件のリスト、および//配布に付属するドキュメント//および/またはその他の資料の次の免責事項を複製する必要があります。//このソフトウェアは著作権所有者によって提供されますおよび寄稿者は「現状有姿」//および//商品性および特定の目的への適合性の黙示の保証を含むがこれに限定されない明示的または黙示的な保証は否認されます/ * * /＃include  //サーボを定義および制御する＃include  //すべてのサーボを管理するタイマーを設定する#include  #include  #include  // OLEDディスプレイTWIアドレス#defineOLED_ADDR 0x3CAdafruit_SSD1306 display（-1）; / *サーボ--------------------------- ----------------------------------------- * /// 4つのサーボを12個定義legsServoサーボ[4] [3]; //サーボのポートを定義constintservo_pin [4] [3] ={{11、12、13}、{2、4、7}、{14、15、16}、{ 8、9、10}}; / *ロボットのサイズ------------------------------------ --------------------- * / const float length_a =50; const float length_b =77.1; 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、自動レスト用//関数のparameterconst 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; const int lightR =3; const int lightG =5; const int lightB =6; int LedR =0; int LedG =0; int LedB =0; char SerialData; //この変数を使用して、シリアルportString data =""; void setup（）{Serial.begin（9600）;を介して受信した各文字を読み取ります。 display.begin（SSD1306_SWITCHCAPVCC、OLED_ADDR）; display.clearDisplay（）; display.display（）; delay（10000）; 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（）; //サーボを初期化しますservo_attach（）; Stand（）; // delay（3000）; // sit（）; // delay（3000）; // Stand（）; // delay（3000）;ハッピー（）;遅延（ランダム（500、1000））; cierra（）;遅延（150）; enfado（）;遅延（ランダム（1000、3000））; cierra（）;遅延（150）; entorna（）;遅延（ランダム（1000、3000））; cierra（）;遅延（150）; enfado1（）;遅延（ランダム（1000、3000））; cierra（）;遅延（150）; triste（）;遅延（ランダム（1000、3000））; cierra（）;遅延（150）; abre（）;遅延（ランダム（500、3000））; cierra（）;遅延（150）;ハッピー（）; delay（random（500、1000））;} void loop（）{while（Serial.available（））//シリアルデータが利用可能である間、それを保存します{delay（10）; SerialData =Serial.read（）; if（SerialData =='b'）LedR =Serial.parseInt（）; else if（SerialData =='g'）LedG =Serial.parseInt（）; else if（SerialData =='r'）LedB =Serial.parseInt（）; else data + =SerialData; } if（data =="f"）//保存されたデータが前方移動の場合{cierra（）;遅延（150）;ハッピー（）; step_forward（1）; } if（data =="p"）//保存されたデータが後方移動の場合{cierra（）;遅延（150）; triste（）; step_back（1）; } if（data =="l"）//保存されたデータを左折する場合は車{cierra（）;遅延（150）; enfado1（）; turn_left（1）; } if（data =="m"）//保存されたデータが右折する場合車{cierra（）;遅延（150）; enfado（）; turn_right（5）; } data =""; analogWrite（lightR、LedR）; analogWrite（lightG、LedG）; analogWrite（lightB、LedB）;} 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 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 - 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);}void abre() { display.clearDisplay(); display.fillCircle (50, 15, 12, WHITE); //ojo izquierdo grande display.fillCircle (82, 20, 7, WHITE); //ojo derecho pequeo display.display();}void cierra() { display.clearDisplay(); display.drawFastHLine(40, 15, 20, WHITE); display.drawFastHLine(72, 20, 15, WHITE); display.display();}void entorna() { display.clearDisplay(); display.fillCircle (42, 10, 20, WHITE); //ojo izquierdo grande display.fillCircle (82, 10, 15, WHITE); //ojo derecho pequeo display.fillRect (0, 0, 128, 15, BLACK); //ceja superior display.fillRect (0, 40, 128, 15, BLACK); //ceja inferior display.display();}void triste() { display.clearDisplay(); display.fillCircle (42, 10, 17, WHITE); //ojo izquierdo grande display.fillCircle (82, 10, 17, WHITE); //ojo derecho pequeo display.fillTriangle (0, 0, 0, 35, 78, 0, BLACK); //ceja superior display.fillTriangle (50, 0, 128, 35, 128, 0, BLACK); //ceja superior display.display();}void happy() { display.clearDisplay(); display.fillCircle (42, 25, 15, WHITE); //ojo izquierdo grande display.fillCircle (82, 25, 15, WHITE); //ojo derecho pequeo display.fillCircle (42, 33, 20, BLACK); //ojo izquierdo grande display.fillCircle (82, 33, 20, BLACK); //ojo derecho pequeo display.display();}void enfado() { display.clearDisplay(); display.fillCircle (42, 10, 18, WHITE); //ojo izquierdo grande display.fillCircle (82, 10, 12, WHITE); //ojo derecho pequeo display.fillTriangle (0, 0, 54, 26, 118, 0, BLACK); //ceja superior display.display();}void enfado1() { display.clearDisplay(); display.fillCircle (42, 10, 18, WHITE); //ojo izquierdo grande display.fillCircle (82, 10, 12, WHITE); //ojo derecho pequeo display.fillTriangle (0, 0, 65, 15, 120, 0, BLACK); //ceja superior display.display();}

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

回路図