MobBob：Androidスマートフォンで制御されるDIY Arduino Robot

必要なツールとマシン

>	はんだごて（汎用）
	3Dプリンター（汎用）

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

>

Arduino IDE

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

ウェブサイトの指示に従ってこのロボットを作りました。ArduinoとAndroidの共生のための素晴らしいアイデアです。

MobBobはスマートフォン制御のロボットです。 MobBobは、スマートフォンのパワーを活用することで、音声認識とコンピュータービジョンを備えた歩行型の会話型ロボットです。

3Dプリントされたパーツ：https：//www.thingiverse.com/thing：715688

Android.apkファイル：https：//apkpure.com/mobbob/com.cevinius.MobBob

コードでは、次のことを行います。

-ビルドに一致するようにピン変数を更新します

-サーボセンター、最小値、最大値を微調整します

-ヒップサーボの取り付け方法に応じて、「FRONT_JOINT_HIPS」を1または-1に設定します。 MobBobの前面にサーボアクセルを取り付けます。この構成では、この値を1に設定します。

コード

• コード

コード Arduino

 / * * =============================================================* MobBob制御プログラム-ソフトウェアシリアルBluetoothバージョン* Kevin Chan（別名Cevinius）* =============================================================**このプログラムシリアルコマンドを使用してMobBobを制御できるようにします。このバージョンのコードでは、*コマンドはソフトウェアのシリアルポートを介して受信され、ピンは上部近くの#defineで定義されています。 *これは、Arduino互換ボードを使用して、Bluetoothカードをソフトウェアシリアル用に設定されたピンに接続できることを意味します。 （DFRobotのBlunoボード用に設計された他のバージョンとは対照的です。）* *このプログラムは長く、スムーズなサーボアニメーションプログラムとシリアル*コマンドパーサープログラムの2つの主要コンポーネントが含まれています。 **アニメーションシステム* ================*アニメーションプログラムは、サーボキーフレームアレイをスムーズにアニメーション化するように設計されています。コードは、使いやすいように*最善を尽くそうとします。 **アニメーションシステムは1つのコマンドのみをキューに入れます。つまり、1つのコマンドを実行でき、* 1つのコマンドをキューに入れることができます。さらにコマンドを送信すると、キューに入れられたコマンドが上書きされます。 * *アニメーションシステムは、デフォルトで現在のアニメーションの終了を待ってから次のアニメーションを開始します。これは、アニメーションデータがロボットをベースポーズにして終了すると、スムーズに結合することを意味します。これをサポートするために、アニメーションシステムには、アニメーションに「終了シーケンス」を設定して、ロボットをベースポーズに戻す機能もあります。この機能は、ウォークフォワード/バックワードアニメーションに使用されます。 *これらのアニメーションには、ロボットをベースポーズに戻す最終シーケンスがあります。 * *アニメーションの再生が終了すると、アニメーションシステムはシリアルポートに応答文字列を出力します。 *これにより、発信者は、要求したアニメーションの再生がいつ終了したかを知ることができます。これは、*ユーザーがアニメーションをシーケンスするのに便利です。アニメーションが終了するのを待ってから、別のアニメーションを開始します。 * *アニメーションコードには、物事を微調整できるようにするための多くの変数があります。例えば。更新頻度、arduinoピンなど。**アニメーションデータ配列形式も手作業で簡単に編集できるように設計されています。 **コマンドパーサー* ==============*このシステムは、シリアル経由で受信したコマンドを解析し、処理します。コマンドには、サーボ位置を直接設定するためのコマンドと、事前定義されたアニメーションおよびウォークをトリガーするためのコマンドが含まれます。 * *したがって、歩行の詳細を気にしたくないユーザーは、事前定義された歩行/アニメーションを使用できます。 *また、サーボを完全に制御したい（その場で新しいアニメーションを作成したい）ユーザーもそれを行うことができます。 * *上記のように、これらのコマンドはArduinoシリアルモニターからインタラクティブに使用できます。 * Bluetooth LEを使用して送信することもできます（Blunoを使用している場合）。電話アプリは、BluetoothLEを介して* Blunoにコマンドを送信します。 * *一般的なコマンド：* ----------------- *準備完了/ OKチェック： *ステータスチェック。コントローラが機能しているかどうかを確認するために、応答がすぐに返されます。 * *サーボの設定： * time-指定された角度にトゥイーンする時間、0はすぐに角度にジャンプします* leftHip-中心からマイクロ秒。 -veはヒップイン、+ veはヒップアウト* leftFoot-フラットからマイクロ秒。 -veは足を下に、+ veは足を上に* rightHip-中心からマイクロ秒。 -veはヒップイン、+ veはヒップアウト* rightFoot-フラットからマイクロ秒。 -veはフットダウン、+ veはフットアップ*このコマンドは、サーボを完全に制御するために使用されます。 *現在のポーズから指定されたポーズまで、指定された期間にわたってロボットをトゥイーンできます。 * *停止/リセット： *現在のアニメーションの後にロボットを停止します。ループ*に設定されたアニメーションを無期限に停止するために使用できます。これは、ロボットを基本ポーズ（直立）にするためにも使用できます。* *即時停止： *現在のアニメーションが完了するのを待たずに、ロボットをすぐに停止します。これは*ロボットの現在のアニメーションを中断します。ロボットがアニメーションの途中*で不安定なポーズになる可能性があるため、これを使用するときは注意してください。 * *標準ウォークコマンド：* ----------------------- *転送：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 *後方：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 *左に曲がる：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 *右折：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 * *楽しいアニメーションコマンド：* ----------------------- *シェイクヘッド：、-1は連続、0またはパラメータなしを意味します1回と同じです。 * *バウンス：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 * *ウォブル：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 *左のぐらつき：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 * Wobble Right：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 * *タップフィート：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 *左足をタップ：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 *右足をタップ：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 * *シェイクレッグ：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 *左足を振る：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 *右脚を振る：、-1は連続を意味し、0またはパラメータなしは1回と同じです。 * *また、コマンドの実行が終了すると、コードはシリアル経由で応答文字列を送り返します。 * *コマンドが正常に終了した場合、応答文字列はパラメータのないコマンドコードです。例えば。前進が終了すると、応答「」を送信します。 * *コマンドがで中断された場合、現在のアニメーションが途中で停止した可能性があります。 *この場合、ロボットが奇妙な途中のポーズになっている可能性があり、finishAnimsが再生されていない可能性があります。これが発生したことをユーザーに知らせるために、応答文字列には*パラメーター-1が含まれます。たとえば、を使用して歩行が途中で停止した場合、応答文字列は* になり、歩行が停止したが途中で停止したことを示します。 *（注：を使用して停止すると、現在のアニメーションサイクルが完了するのを待ってから*停止します。そのため、その場合、アニメーションは途中で停止しません。）**応答はアニメーションが完了すると、コマンド送信者は*応答文字列を探して、ロボットが新しいコマンドの準備ができたことを判断できます。 *例：コマンドを使用すると、3つのステップ*（およびfinish anim）がすべて完了するまで応答文字列は送信されません。したがって、コマンド送信者は、ロボットに次のことを行うように指示する前に、応答*文字列を待つことができます。 * / #include  #include  // -------------------------------- -------------------------------------------------- //シリアル通信の速度-シリアル（Bluetooth）カードにこれを設定します。//------------------------------- -------------------------------------------------- -// Bluetoothボードとのシリアル通信速度。//一部のボードのデフォルトは9600です。私が使用しているボードのデフォルト値は115200です。＃defineSERIAL_SPEED 115200 //これらのピンにソフトウェアシリアルポートを設定します。constintrxPin=11; // dataconstの受信に使用されるピンinttxPin =12; // dataSoftwareSerial softwareSerial（rxPin、txPin）; // ----------------------------------の送信に使用されるピン------------------------------------------------ // Arduinoピンのセットアップ-特定のロボット用にこれらを設定します。//------------------------------------- --------------------------------------------- const int SERVO_LEFT_HIP =5; const int SERVO_LEFT_FOOT =2; const int SERVO_RIGHT_HIP =3; const int SERVO_RIGHT_FOOT =4; //このコードをすべての4サーボバイペッドで使用できるようにしたい！ （Bob、MobBobのように）//一部のビルドがヒップサーボをMobBobの方法とは異なる方向に向けてマウントしていることに気付きました。この設定により、どちらのビルドスタイルにもコードを構成できます。//MobBobスタイルには1前向きのヒップ（前向きのジョイント）// -1（ボブスタイルの後ろ向きのヒップ（後ろ向きのジョイント））＃define FRONT_JOINT_HIPS 1 // ------------------- -------------------------------------------------- ------------- //サーボの最大/最小/中心定数-特定のロボットに合わせて設定します。//------------------ -------------------------------------------------- -------------- const int LEFT_HIP_CENTRE =1580; const int LEFT_HIP_MIN =LEFT_HIP_CENTRE-500; const int LEFT_HIP_MAX =LEFT_HIP_CENTRE + 500; const int LEFT_FOOT_CENTRE =1410; const int LEFT_FOOT_ const int LEFT_FOOT_MAX =LEFT_FOOT_CENTRE + 500; const int RIGHT_HIP_CENTRE =1500; const int RIGHT_HIP_MIN =RIGHT_HIP_CENTRE-500; const int RIGHT_HIP_MAX =RIGHT_HIP_CENTRE + 500; const int RI GHT_FOOT_CENTRE =1465; const int RIGHT_FOOT_MIN =RIGHT_FOOT_CENTRE-500; const int RIGHT_FOOT_MAX =RIGHT_FOOT_CENTRE + 500; // ---------------------------- -------------------------------------------------- //よりユーザーフレンドリーな方法でジョイント値を計算するのに役立つヘルパー関数//サーボが別の方法で設定されている場合は、ここで符号を調整できます//ここで更新すると、アニメーションデータを変更する必要がなくなります//サーボの設定が異なる場合//（例：元のボブのヒップサーボはモブボブのものとは逆になっています。）////（また、//左右のヒップと左/右の足で異なるため、各サーボに使用する記号を覚えるのは難しいと思います。） // ------------------------------------------------ ------------------------------ int LeftHipCentre（）{return LEFT_HIP_CENTRE; } int LeftHipIn（intミリ秒）{return LEFT_HIP_CENTRE +（FRONT_JOINT_HIPS *ミリ秒）; } int LeftHipOut（intミリ秒）{return LEFT_HIP_CENTRE-（FRONT_JOINT_HIPS *ミリ秒）; } int RightHipCentre（）{return RIGHT_HIP_CENTRE; } int RightHipIn（intミリ秒）{return RIGHT_HIP_CENTRE-（FRONT_JOINT_HIPS *ミリ秒）; } int RightHipOut（intミリ秒）{return RIGHT_HIP_CENTRE +（FRONT_JOINT_HIPS *ミリ秒）; } int LeftFootFlat（）{return LEFT_FOOT_CENTRE; } int LeftFootUp（intミリ秒）{returnLEFT_FOOT_CENTRE-ミリ秒; } int LeftFootDown（intミリ秒）{return LEFT_FOOT_CENTRE +ミリ秒; } int RightFootFlat（）{return RIGHT_FOOT_CENTRE; } int RightFootUp（intミリ秒）{return RIGHT_FOOT_CENTRE +ミリ秒; } int RightFootDown（intミリ秒）{returnRIGHT_FOOT_CENTRE-ミリ秒; } // ----------------------------------------------- ----------------------------------- //標準的な歩行歩行およびその他のサーボアニメーションのキーフレームアニメーションデータ// //形式は{、、、、} //ミリ秒-このキーフレームの位置にトゥイーンする時間。例えば。 500は、このフレームの開始時のロボットの位置からこのフレームで指定された位置に移動するのに500msかかることを意味します// leftHipMicros-サーボマイクロ秒単位の左股関節の位置// leftFootMicros-サーボ内の左股関節の位置microsecs.//rightHipMicros-サーボマイクロ秒単位の左股関節の位置// rightFootMicros-サーボマイクロ秒単位の左股関節の位置////サーボマイクロ値は、-1の特別な値をサポートします。この値が指定されている場合、このキーフレームでこのサーボを無視するようにアニメーションコードに指示します。つまり、そのサーボは//このキーフレームの開始時の位置に留まります。////また、アニメーションデータの最初の要素は特別です。これはメタデータ要素です。//最初の要素は{、0、0、0、0}で、アニメーションのフレーム数//を示します。したがって、最初の実際のキーフレームはanimData [1]にあり、最後のキーフレーム//はanimData []にあります。 （ここで、はanimData [0] [0]の値です。）// ----------------------------- -------------------------------------------------- --- //キーフレーム配列へのアクセスをより人間が読みやすくするための定数constint TWEEN_TIME_VALUE =0; const int LEFT_HIP_VALUE =1; const int LEFT_FOOT_VALUE =2; const int RIGHT_HIP_VALUE =3; const int RIGHT_FOOT_VALUE =4; //使用される定数歩行歩行アニメーションdata.constint FOOT_DELTA =150; const int HIP_DELTA =FRONT_JOINT_HIPS * 120; //デフォルトの直立姿勢になります。 stopAnim（）。intによって使用されますstandStraightAnim [] [5] ={//メタデータ。最初の要素はフレーム数です。 {1、0、0、0、0}、//扁平足、偶数足{300、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}}; //これの前に、ロボットを扁平足、偶数足（つまり、standStraightAnim）.int walkForwardAnim [] [5] ={//メタデータ。最初の要素はフレーム数です。 {8、0、0、0、0}、//左に傾ける、足を均等にする{300、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FOOT_DELTA）}、//左に傾ける、右足forward {300、LeftHipIn（HIP_DELTA）、LeftFootUp（FOOT_DELTA）、RightHipOut（HIP_DELTA）、RightFootDown（FOOT_DELTA）}、//足が平ら、右足が前方{300、LeftHipIn（HIP_DELTA）、LeftFootFlat（）、RightHipOut（HIP_DELTA）、 RightFootFlat（）}、//右に傾け、右足を前に{300、LeftHipIn（HIP_DELTA）、LeftFootDown（FOOT_DELTA）、RightHipOut（HIP_DELTA）、RightFootUp（FOOT_DELTA）}、//右に傾け、足を均等に{300、LeftHipCentre （）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾け、左足を前方に{300、LeftHipOut（HIP_DELTA）、LeftFootDown（FOOT_DELTA）、RightHipIn（HIP_DELTA）、RightFootUp（FOOT_DELTA）} 、//足を平らにし、左足を前方に{300、LeftHipOut（HIP_DELTA）、LeftFootFlat（）、RightHipIn（HIP_DELTA）、RightFootFlat（）} 、//左に傾け、左足を前に{300、LeftHipOut（HIP_DELTA）、LeftFootUp（FOOT_DELTA）、RightHipIn（HIP_DELTA）、RightFootDown（FOOT_DELTA）}}; //これの前に、ロボットを足を平らにし、足を均等にします（すなわち、 StandStraightAnim）.int walkBackwardAnim [] [5] ={//メタデータ。最初の要素はフレーム数です。 {8、0、0、0、0}、//左に傾ける、足を均等にする{300、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FOOT_DELTA）}、//左に傾ける、左足forward {300、LeftHipOut（HIP_DELTA）、LeftFootUp（FOOT_DELTA）、RightHipIn（HIP_DELTA）、RightFootDown（FOOT_DELTA）}、//足が平ら、左足が前方{300、LeftHipOut（HIP_DELTA）、LeftFootFlat（）、RightHipIn（HIP_DELTA）、 RightFootFlat（）}、//右に傾け、左足を前に{300、LeftHipOut（HIP_DELTA）、LeftFootDown（FOOT_DELTA）、RightHipIn（HIP_DELTA）、RightFootUp（FOOT_DELTA）}、//右に傾け、足を均等に{300、LeftHipCentre （）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾け、右足を前方に{300、LeftHipIn（HIP_DELTA）、LeftFootDown（FOOT_DELTA）、RightHipOut（HIP_DELTA）、RightFootUp（FOOT_DELTA）} 、//足が平ら、右足が前方{300、LeftHipIn（HIP_DELTA）、LeftFootFlat（）、RightHipOut（HIP_DELTA）、RightFootFlat（）}、 //左に傾け、右足を前に{300、LeftHipIn（HIP_DELTA）、LeftFootUp（FOOT_DELTA）、RightHipOut（HIP_DELTA）、RightFootDown（FOOT_DELTA）}}; //終了ウォークアニメーションは、walkForwardAnim / walkBackwardAnimの最後からロボットを戻しますto StandStraightAnim.int walkEndAnim [] [5] ={//メタデータ。最初の要素はフレーム数です。 {2、0、0、0、0}、//左に傾ける、足を均等にする{300、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FOOT_DELTA）}、//足を平らにする、足を均等にする{ 300、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}}; //これの前に、ロボットを足を平らにし、足を均等にします（つまり、standStraightAnim）.int turnLeftAnim [] [5] ={/ /メタデータ。最初の要素はフレーム数です。 {6、0、0、0、0}、//左に傾ける、足を均等にする{300、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FOOT_DELTA）}、//左に傾ける、左に曲がるヒップ、右に曲がるヒップ{300、LeftHipIn（HIP_DELTA）、LeftFootUp（FOOT_DELTA）、RightHipIn（HIP_DELTA）、RightFootDown（FOOT_DELTA）}、//足を平らにする、左に曲がるヒップ、右に曲がるヒップ{300、LeftHipIn（HIP_DELTA）、LeftFootFlat （）、RightHipIn（HIP_DELTA）、RightFootFlat（）}、//右に傾ける、左ヒップを回す、右ヒップを回す{300、LeftHipIn（HIP_DELTA）、LeftFootDown（FOOT_DELTA）、RightHipIn（HIP_DELTA）、RightFootUp（FOOT_DELTA）}、 //右に傾ける、足を均等にする{300、LeftHipCentre（）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//足を平らにする、足を均等にする{300、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（） ）、RightFootFlat（）}}; //これの前に、ロボットを足を平らにし、足を均等にします（つまり、standStraightAnim）.int turnRightAnim [] [5] ={//メタデータ。最初の要素はフレーム数です。 {6、0、0、0、0}、//右に傾ける、足を均等にする{300、LeftHipCentre（）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾ける、左に曲がるヒップ、右に曲がるヒップ{300、LeftHipIn（HIP_DELTA）、LeftFootDown（FOOT_DELTA）、RightHipIn（HIP_DELTA）、RightFootUp（FOOT_DELTA）}、//足を平らにする、左に曲がるヒップ、右に曲がるヒップ{300、LeftHipIn（HIP_DELTA）、LeftFootFlat （）、RightHipIn（HIP_DELTA）、RightFootFlat（）}、//左に傾ける、左ヒップを回す、右ヒップを回す{300、LeftHipIn（HIP_DELTA）、LeftFootUp（FOOT_DELTA）、RightHipIn（HIP_DELTA）、RightFootDown（FOOT_DELTA）}、 //左に傾ける、足を均等にする{300、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FOOT_DELTA）}、//足を平らにする、足を均等にする{300、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（） ）、RightFootFlat（）}}; //頭のアニメーションを振る。揺れているhead.intshakeHeadAnim [] [5] ={//メタデータをエミュレートするためにすばやく左右に移動します。最初の要素はフレーム数です。 {4、0、0、0、0}、//足を平ら、左にひねる{150、LeftHipOut（HIP_DELTA）、LeftFootFlat（）、RightHipIn（HIP_DELTA）、RightFootFlat（）}、//足を平ら、足を偶数{150 、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}、//フィートフラット、右ツイスト{150、LeftHipIn（HIP_DELTA）、LeftFootFlat（）、RightHipOut（HIP_DELTA）、RightFootFlat（）}、//フィートフラット、足も{150、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}}; //ウォブルアニメーション。左右に傾けて楽しいwobble.intwobbleAnim [] [5] ={//メタデータを実行します。最初の要素はフレーム数です。 {4、0、0、0、0}、//左に傾ける、足を均等にする{300、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FOOT_DELTA）}、//足を平らにする、足を均等にする{300 、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}、//右に傾ける、足を均等にする{300、LeftHipCentre（）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、// Feetフラット、足も{300、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}}; //左アニメーションをぐらつきます。左に傾けてback.intwobbleLeftAnim [] [5] ={//メタデータ。最初の要素はフレーム数です。 {2、0、0、0、0}、//左に傾ける、足を均等にする{300、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FOOT_DELTA）}、//足を平らにする、足を均等にする{300 、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}、}; //右アニメーションをぐらつきます。右に傾けてback.intwobbleRightAnim [] [5] ={//メタデータ。最初の要素はフレーム数です。 {2、0、0、0、0}、//右に傾ける、足を均等にする{300、LeftHipCentre（）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//足を平らにする、足を均等にする{300 、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}}; //足のアニメーションをタップします。両方のfeet.intをタップします。tapFeetAnim[] [5] ={//メタデータ。最初の要素はフレーム数です。 {2、0、0、0、0}、//両足を上げる、足を均等にする{500、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//足を平らにする、足を均等にする{ 500、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}、}; //左足をタップanim.inttapLeftFootAnim [] [5] ={//メタデータ。最初の要素はフレーム数です。 {2、0、0、0、0}、//左足を上げる、足を均等にする{500、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootFlat（）}、//足を平らにする、足を均等にする{500 、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}、}; //右足をタップanim.inttapRightFootAnim [] [5] ={//メタデータ。最初の要素はフレーム数です。 {2、0、0、0、0}、//右足を上げる、足を均等にする{500、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//足を平らにする、足を均等にする{500 、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}、}; //上下にバウンスするanim.intbounceAnim [] [5] ={//メタデータ。最初の要素はフレーム数です。 {2、0、0、0、0}、//両足を上げる、足を均等にする{500、LeftHipCentre（）、LeftFootDown（300）、RightHipCentre（）、RightFootDown（300）}、//足を平らにする、足を均等にする{ 500、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}、}; //シェイクレッグAnimation.intshakeLegsAnim [] [5] ={//メタデータ。最初の要素はフレーム数です。 {14、0、0、0、0}、//左に傾け、足を均等に{300、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FOOT_DELTA）}、//左に傾け、右ヒップを{ 100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipIn（HIP_DELTA）、RightFootDown（FOOT_DELTA）}、//左に傾ける、足を均等にする{100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FO_ 、//左に傾ける、右ヒップアウト{100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipOut（HIP_DELTA）、RightFootDown（FOOT_DELTA）}、//左に傾ける、足を均等にする{100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA） 、RightHipCentre（）、RightFootDown（FOOT_DELTA）}、//左に傾ける、右ヒップを{100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipIn（HIP_DELTA）、RightFootDown（FOOT_DELTA）}、//左に傾ける、足を偶数{ 100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FOOT_DELTA）}、//足が平ら、足が偶数{300、LeftHipCen tre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}、//右に傾ける、足を均等に{300、LeftHipCentre（）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾ける、左ヒップ{100、LeftHipIn（HIP_DELTA）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾ける、足を均等にする{100、LeftHipCentre（）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、 RightFootUp（FOOT_DELTA）}、//右に傾ける、左ヒップアウト{100、LeftHipOut（HIP_DELTA）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾ける、足を均等にする{100、LeftHipCentre（） 、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//足が平ら、足が偶数{300、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}}; //左足のアニメーションを振る.int shakeLeftLegAnim [] [5] ={//メタデータ。最初の要素はフレーム数です。 {12、0、0、0、0}、//右に傾け、足を均等に{300、LeftHipCentre（）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾け、左ヒップを{ 100、LeftHipIn（HIP_DELTA）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾ける、足を均等にする{100、LeftHipCentre（）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FO 、//右に傾ける、左ヒップアウト{100、LeftHipOut（HIP_DELTA）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾ける、足を均等にする{100、LeftHipCentre（）、LeftFootDown（FOOT_DELTA） 、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾ける、左ヒップを{100、LeftHipIn（HIP_DELTA）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾ける、足も{ 100、LeftHipCentre（）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾ける、左ヒップアウト{100、Le ftHipOut（HIP_DELTA）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾ける、足を均等にする{100、LeftHipCentre（）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT） /右に傾ける、左ヒップを{100、LeftHipIn（HIP_DELTA）、LeftFootDown（FOOT_DELTA）、RightHipCentre（）、RightFootUp（FOOT_DELTA）}、//右に傾ける、足を均等にする{100、LeftHipCentre（）、LeftFootDown（FOOT_DELTA）、RightHipCentre （）、RightFootUp（FOOT_DELTA）}、//足が平ら、足が偶数{300、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}}; //右足を振るAnimation.intshakeRightLegAnim [] [5 ] ={//メタデータ。最初の要素はフレーム数です。 {12、0、0、0、0}、//左に傾け、足を均等に{300、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FOOT_DELTA）}、//左に傾け、右ヒップを{ 100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipIn（HIP_DELTA）、RightFootDown（FOOT_DELTA）}、//左に傾ける、足を均等にする{100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FO_ 、//左に傾ける、右ヒップアウト{100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipOut（HIP_DELTA）、RightFootDown（FOOT_DELTA）}、//左に傾ける、足を均等にする{100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA） 、RightHipCentre（）、RightFootDown（FOOT_DELTA）}、//左に傾ける、右ヒップを{100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipIn（HIP_DELTA）、RightFootDown（FOOT_DELTA）}、//左に傾ける、足を偶数{ 100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FOOT_DELTA）}、//左に傾ける、右ヒップアウト{100、LeftHi pCentre（）、LeftFootUp（FOOT_DELTA）、RightHipOut（HIP_DELTA）、RightFootDown（FOOT_DELTA）}、//左に傾ける、足を均等にする{100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre（）、RightFootDown（FOOT_DELTA）} /左に傾ける、右ヒップを{100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipIn（HIP_DELTA）、RightFootDown（FOOT_DELTA）}、//左に傾ける、足を均等にする{100、LeftHipCentre（）、LeftFootUp（FOOT_DELTA）、RightHipCentre （）、RightFootDown（FOOT_DELTA）}、//フィートフラット、フィート偶数{300、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}、}; // ---------- -------------------------------------------------- ---------------------- //サーボ位置の設定/トゥイーン用の特別なダイナミックアニメーションデータ// ------------- -------------------------------------------------- ------------------- //これらは、SetServos（）関数に使用する2つの特別なアニメーションデータです。彼らは//単一のフレームを持っています。これらは、これらのアニメーションデータのデータを変更し、それらを再生して//サーボを移動します。intsetServosAnim1[] [5] ={//メタデータ。最初の要素はフレーム数です。 {1、0、0、0、0}、//左に傾け、足を偶数{0、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}}; int setServosAnim2 [] [5] ={/ /メタデータ。最初の要素はフレーム数です。 {1、0、0、0、0}、//左に傾ける、足を均等にする{0、LeftHipCentre（）、LeftFootFlat（）、RightHipCentre（）、RightFootFlat（）}}; // -------- -------------------------------------------------- ------------------------ //サーボ変数// -------------------- -------------------------------------------------- ------------サーボservoLeftHip;サーボservoLeftFoot;サーボservoRightHip;サーボservoRightFoot; // ------------------------ -------------------------------------------------- -------- //アニメーションを再生するための状態変数// -------------------------------- -------------------------------------------------- //アニメーションの更新間のミリ秒.constintmillisBetweenAnimUpdate =20; //最後のアニメーションの更新を行った時刻.longtimeAtLastAnimUpdate; //現在再生中のanim.int（* currAnim）[5];に関連//再生中の現在のアニメーション.int（* finishAnim）[5]; // currAnimが終了または停止したときに再生するアニメーション。longtimeAtStartOfFrame; //最後のkeyframeでのmillis（）-私たちがlerpingしているフレームfromint targetFrame; //私たちがlerpingしているフレームtointanimNumLoops; //アニメーションを再生する回数。 -1は、無限ループを意味します。charanimCompleteStr[3] ="-"; //これは2文字の文字列です。アニムが完了すると、//ステータスを「<」+ animComplereStr + ">"として出力します。//アニムキューに関連します。つまり次のanimto play.bool animInProgress; //アニメーションが再生されているかどうかint（* nextAnim）[5]; //これは、現在のアニメーションが完了したら次に再生されるアニメーションです。 //つまり、サイズ1のキューのようなものです！ // currがループしていない場合、現在のアニメーションの最後にこれを再生します。 // currがループしている場合、これは現在のループの最後から始まり、// curranimを置き換えます。 //何も再生されていない場合、これはすぐに開始されます。 int（* nextFinishAnim）[5]; //これはキューに入れられたanimation.intnextAnimNumLoopsの終了アニメーションです。 //アニメーションを再生する回数。 -1は、無限ループを意味します。charnextAnimCompleteStr[3] ="-"; //これは2文字の文字列です。アニムが完了すると、//ステータスを「<」+ animComplereStr + ">"。boolinterruptInProgressAnim;として出力します。 //現在のアニメーションを中断して、すぐにアニメーションを変更するかどうか。 / ------------------------------------------------- ------------------------------ //パーサー変数// -------------- -------------------------------------------------- --------------- //定数区切り記号charsconstchar START_CHAR ='<'; const char END_CHAR ='>'; const char SEP_CHAR ='、'; //定数と変数パーサーstate.constint PARSER_WAITING =0の場合。 // '<'が解析を開始するのを待っています。constintPARSER_COMMAND=1; //コマンド文字列を読み取ります。constintPARSER_PARAM1=2; //パラメータ1を読み取ります。constintPARSER_PARAM2=3; //パラメータ2を読み取ります。constintPARSER_PARAM3=4; // param3.constを読み取りますintPARSER_PARAM4 =5; // param3.constを読み取りますintPARSER_PARAM5 =6; // param3.constを読み取りますintPARSER_EXECUTE =7; //コマンドの解析が終了したので、実行します。//現在のパーサーstate.int currParserState =PARSER_WAITING; //コマンドを保存するための文字列。コマンド用に2文字、 '\ 0'用に1文字。//解析中のコマンドをここに保存します。charcurrCmd[3] ="-"; //コマンド内の文字を追跡します。 int currCmdIndex; //最大コマンド長.constint CMD_LENGTH =2; //現在のパラメーター値。それらを解析した後、ここに格納します。intcurrParam1Val; int currParam2Val; int currParam3Val; int currParam4Val; int currParam5Val; //パラメータで解析している桁を追跡するための変数//これを使用して1桁を変換し直します10進値.intcurrParamIndex; //現在のパラメーターが負であるかどうか。booleancurrParamNegative; //パラメーターの最大長。 this.const int MAX_PARAM_LENGTH =6; // ===================================を超える場合は、解析を停止します。 ============================================// Arduino setup（）およびloop（）.// ===============================================================================void setup（）{//メインシリアルポートソフトウェアシリアルをセットアップします。 begin（SERIAL_SPEED）; //サーボを設定servoLeftHip.attach（SERVO_LEFT_HIP、LEFT_HIP_MIN、LEFT_HIP_MAX）; ServoLeftFoot.attach（SERVO_LEFT_FOOT、LEFT_FOOT_MIN、LEFT_FOOT_MAX）; ServoRightHip.attach（SERVO_RIGHT_HIP、RIGHT_HIP_MIN、RIGHT_HIP_MAX）; ServoRightFoot.attach（SERVO_RIGHT_FOOT、RIGHT_FOOT_MIN、RIGHT_FOOT_MAX）; //パーサー用に設定します。 setup_Parser（）; //アニメーションコードの設定を行います。 setup_Animation（）;} void loop（）{//パーサーを更新します。 loop_Parser（）; //アニメーションを更新します。 loop_Animation（）;} // ===============================================================================//パーサーに関連する// ===============================================================================//パーサーのものをセットアップします。 setup（）で呼び出されます。他の場所では呼び出さないでください。voidsetup_Parser（）{//最初のコマンドを待ちます。 currParserState =PARSER_WAITING; //この応答を出力して、起動して準備ができたことを示します。 softwareSerial.println（ ""）;} //パーサー関連のLoop（）。 loop（）で呼び出されます。他の場所では呼び出さないでください。voidloop_Parser（）{// ------------------------------------- -------------------- // PARSER // //データがある場合は、それを解析して処理します。 // ------------------------------------------------ --------- //ピンのシリアルポートから読み取り、USBポートに書き込みます。 if（softwareSerial.available（）> 0）{char c =softwareSerial.read（）; // WAITING状態の場合は、START_CHARを探します。 if（currParserState ==PARSER_WAITING）{// START_CHARの場合は、この状態から抜け出します... if（c ==START_CHAR）{//コマンドの解析を開始します。 currParserState =PARSER_COMMAND; //解析の準備ができているものをリセットしますcurrCmdIndex =0; currCmd [0] ='-'; currCmd [1] ='-'; currParam1Val =0; currParam2Val =0; currParam3Val =0; currParam4Val =0; currParam5Val =0; } //それ以外の場合は、この状態のままにします。 } //コマンドを探す状態。 else if（currParserState ==PARSER_COMMAND）{//区切り文字の場合は、パラメーター1を解析します。ただし、//空でないことを確認してください。そうでない場合は、解析エラーになります。 if（c ==SEP_CHAR）{if（currCmdIndex ==CMD_LENGTH）{currParserState =PARSER_PARAM1; currParamIndex =0; currParamNegative =false; } else {currParserState =PARSER_WAITING; }} //それ以外の場合、それがend charの場合、パラメーターはないので、//処理する準備ができています。ただし、空でないことを確認してください。それ以外の場合は、解析エラーです。 else if（c ==END_CHAR）{if（currCmdIndex ==CMD_LENGTH）{currParserState =PARSER_EXECUTE; } else {currParserState =PARSER_WAITING; }} //ここに文字が多すぎると、解析エラーが発生します。//破棄して、PARSER_WAITINGに戻ります。elseif（（currCmdIndex> =CMD_LENGTH）||（c <'A'）||（ c> 'Z'））{currParserState =PARSER_WAITING; } //現在の文字を保存します。 else {currCmd [currCmdIndex] =c; currCmdIndex ++; }} //パラメータ1を解析する状態* currParam1Val; } currParserState =PARSER_PARAM2; currParamIndex =0; currParamNegative =false; } //それ以外の場合、それがend charの場合、パラメータはないので、//処理する準備ができています。 else if（c ==END_CHAR）{if（currParamNegative）{currParam1Val =-1 * currParam1Val; } currParserState =PARSER_EXECUTE; } //開始時にネガティブをチェックします。 else if（（currParamIndex ==0）&&（c =='-'））{currParamNegative =true; currParamIndex ++; } //長すぎる場合、または文字が数字でない場合は、//解析エラーなので、放棄してPARSER_WAITINGに戻ります。 else if（（currParamIndex> =MAX_PARAM_LENGTH）||（c <'0'）||（c> '9'））{currParserState =PARSER_WAITING; } //有効な文字なので、処理します。 else {//既存の値をシフトし、下部に新しい数字を追加します。 int currDigitVal =c-'0 '; currParam1Val =（currParam1Val * 10）+ currDigitVal; currParamIndex ++; }} //パラメータ2を解析する状態* currParam2Val; } currParserState =PARSER_PARAM3; currParamIndex =0; currParamNegative =false; } //それ以外の場合、それがend charの場合、パラメータはないので、//処理する準備ができています。 else if（c ==END_CHAR）{if（currParamNegative）{currParam2Val =-1 * currParam2Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam2Val =(currParam2Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 3. else if (currParserState ==PARSER_PARAM3) { // Else if it's a separator, parse parameter 2. if (c ==SEP_CHAR) { if (currParamNegative) { currParam3Val =-1 * currParam3Val; } currParserState =PARSER_PARAM4; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam3Val =-1 * currParam3Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam3Val =(currParam3Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 4. else if (currParserState ==PARSER_PARAM4) { // Else if it's a separator, parse parameter 2. if (c ==SEP_CHAR) { if (currParamNegative) { currParam4Val =-1 * currParam4Val; } currParserState =PARSER_PARAM5; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam4Val =-1 * currParam4Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam4Val =(currParam4Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 5. else if (currParserState ==PARSER_PARAM5) { // If it's the end char, there are no parameters, so we're ready to // process. if (c ==END_CHAR) { if (currParamNegative) { currParam5Val =-1 * currParam5Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam5Val =(currParam5Val * 10) + currDigitVal; currParamIndex++; } } //--------------------------------------------------------- // PARSER CODE HANDLER (Still part of Parser, but section that // processes completed commands) // // If the most recently read char completes a command, // then process the command, and clear the state to // go back to looking for a new command. // // The parsed items are stored in:// currCmd, currParam1Val, currParam2Val, currParam3Val, // currParam4Val, currParam5Val //--------------------------------------------------------- if (currParserState ==PARSER_EXECUTE) { // Ready/OK Check: if ((currCmd[0] =='O') &&(currCmd[1] =='K')) { softwareSerial.println(""); } // Set Servo: // time - time to tween to specified angles // leftHip - microsecs from centre. -ve is hip in, +ve is hip out // leftFoot - microsecs from flat. -ve is foot down, +ve is foot up // rightHip - microsecs from centre. -ve is hip in, +ve is hip out // rightFoot - microsecs from flat. -ve is foot down, +ve is foot up else if ((currCmd[0] =='S') &&(currCmd[1] =='V')) { int tweenTime =currParam1Val; if (currParam1Val <0) { tweenTime =0; } SetServos(tweenTime, currParam2Val, currParam3Val, currParam4Val, currParam5Val, "SV"); } // Stop/Reset:, Stops current anim. Also can be used to put robot into reset position. else if ((currCmd[0] =='S') &&(currCmd[1] =='T')) { StopAnim("ST"); } // Stop Immediate: else if ((currCmd[0] =='S') &&(currCmd[1] =='I')) { StopAnimImmediate("SI"); } // Forward:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='F') &&(currCmd[1] =='W')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(walkForwardAnim, walkEndAnim, numTimes, "FW"); } // Backward:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='B') &&(currCmd[1] =='W')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(walkBackwardAnim, walkEndAnim, numTimes, "BW"); } // Turn Left:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='T')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(turnLeftAnim, NULL, numTimes, "LT"); } // Turn Right:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='R') &&(currCmd[1] =='T')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(turnRightAnim, NULL, numTimes, "RT"); } // Shake Head:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='S') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeHeadAnim, NULL, numTimes, "SX"); } // Bounce:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='B') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(bounceAnim, NULL, numTimes, "BX"); } // Wobble:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='W') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(wobbleAnim, NULL, numTimes, "WX"); } // Wobble Left:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='W') &&(currCmd[1] =='Y')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(wobbleLeftAnim, NULL, numTimes, "WY"); } // Wobble Right:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='W') &&(currCmd[1] =='Z')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(wobbleRightAnim, NULL, numTimes, "WZ"); } // Tap Feet:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='T') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(tapFeetAnim, NULL, numTimes, "TX"); } // Tap Left Foot:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='T') &&(currCmd[1] =='Y')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(tapLeftFootAnim, NULL, numTimes, "TY"); } // Tap Right Foot:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='T') &&(currCmd[1] =='Z')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(tapRightFootAnim, NULL, numTimes, "TZ"); } // Shake Legs:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeLegsAnim, NULL, numTimes, "LX"); } // Shake Left Leg:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='Y')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeLeftLegAnim, NULL, numTimes, "LY"); } // Shake Right Leg:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='Z')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeRightLegAnim, NULL, numTimes, "LZ"); } //-------------------------------------------------- // Clear the state and wait for the next command! // This must be done! //-------------------------------------------------- currParserState =PARSER_WAITING; } }}//===============================================================================// Related to playing servo animations.//===============================================================================// Call this to play the given animation once. Pass in NULL if there is no finishAnim.void PlayAnim(int animToPlay[][5], int finishAnim[][5], const char *completeStr){ // Put this in the queue. PlayAnimNumTimes(animToPlay, finishAnim, 1, completeStr);}// Call this to loop the given animation. Pass in NULL if there is no finishAnim.void LoopAnim(int animToPlay[][5], int finishAnim[][5], const char *completeStr){ // Put this in the queue. PlayAnimNumTimes(animToPlay, finishAnim, -1, completeStr);}// Call this to play the given animation the specified number of times. // -1 number of times will make it loop forever.// Pass in NULL if there is no finishAnim.void PlayAnimNumTimes(int animToPlay[][5], int finishAnim[][5], int numTimes, const char *completeStr){ // Put this in the queue. nextAnim =animToPlay; nextFinishAnim =finishAnim; nextAnimNumLoops =numTimes; // Save the completeStr if (completeStr ==NULL) { nextAnimCompleteStr[0] ='-'; nextAnimCompleteStr[1] ='-'; } else { nextAnimCompleteStr[0] =completeStr[0]; nextAnimCompleteStr[1] =completeStr[1]; }}// Stop after the current animation.void StopAnim(const char *completeStr){ // Put this in the queue. PlayAnimNumTimes(standStraightAnim, NULL, 1, completeStr);}// Stop immediately and lerp robot to zero position, interrupting // any animation that is in progress.void StopAnimImmediate(const char *completeStr){ // Put this in the queue. interruptInProgressAnim =true; PlayAnimNumTimes(standStraightAnim, NULL, 1, completeStr);}// Moves servos to the specified positions. Time 0 will make it immediate. Otherwise,// it'll tween it over a specified time.// For positions, 0 means centered.// For hips, -ve is hip left, +ve is hip right// For feet, -ve is foot down, +ve is foot upvoid SetServos(int tweenTime, int leftHip, int leftFoot, int rightHip, int rightFoot, const char* completeStr){ // Save the completeStr if (completeStr ==NULL) { nextAnimCompleteStr[0] ='-'; nextAnimCompleteStr[1] ='-'; } else { nextAnimCompleteStr[0] =completeStr[0]; nextAnimCompleteStr[1] =completeStr[1]; } // Decide which tween data we use. We don't want to over-write the one that is // in progress. We have and reuse these to keep memory allocation fixed. int (*tweenServoData)[5]; if (currAnim !=setServosAnim1) { tweenServoData =setServosAnim1; } else { tweenServoData =setServosAnim2; } // Set the tween information into the animation data. tweenServoData[1][TWEEN_TIME_VALUE] =tweenTime; tweenServoData[1][LEFT_HIP_VALUE] =LeftHipIn(leftHip); tweenServoData[1][LEFT_FOOT_VALUE] =LeftFootUp(leftFoot); tweenServoData[1][RIGHT_HIP_VALUE] =RightHipIn(rightHip); tweenServoData[1][RIGHT_FOOT_VALUE] =RightFootUp(rightFoot); // Queue this tween to be played next. PlayAnim(tweenServoData, NULL, completeStr);}// Set up variables for animation. This is called in setup(). Should be not called by anywhere else.void setup_Animation(){ // Set the servos to the feet flat, feet even position. currLeftHip =LEFT_HIP_CENTRE; currLeftFoot =LEFT_FOOT_CENTRE; currRightHip =RIGHT_HIP_CENTRE; currRightFoot =RIGHT_FOOT_CENTRE; UpdateServos(); // Set the "start" positions to the current ones. So, when // we pay the next anim, we will tween from the current positions. startLeftHip =currLeftHip; startLeftFoot =currLeftFoot; startRightHip =currRightHip; startRightFoot =currRightFoot; // No animation is playing yet, and nothing in the queue yet. timeAtLastAnimUpdate =millis(); animInProgress =false; interruptInProgressAnim =false; currAnim =NULL; finishAnim =NULL; nextAnim =NULL; nextFinishAnim =NULL;}// Loop function for processing animation. This is called in every loop(). Should be be called by anywhere else.//// NOTE:The way looping animations work is that they basically add themselves back to the queue// when a cycle is done, and if there's nothing already queued up! This way, looping animations// work in a similar way to single-play animations, and fits into the queueing system.void loop_Animation(){ // Get the time at the start of this frame. long currTime =millis(); //-------------------------------------------------------------------------------------- // Decide if we want to perform the animation update. We don't execute this every frame. //-------------------------------------------------------------------------------------- if (timeAtLastAnimUpdate + millisBetweenAnimUpdate> currTime) { // Not yet time to do an anim update, so jump out.戻る; } else { // We reset the timer, and then proceed below to handle the current anim update. timeAtLastAnimUpdate =currTime; } //-------------------------------------------------------------------------------------- // Decide if we need to setup and start a new animation. We do if there's no anim // playing or we've been asked to interrupt the anim. //-------------------------------------------------------------------------------------- if ( (nextAnim !=NULL) &&(!animInProgress || interruptInProgressAnim) ) { // If this was an interrupt, we also set the "start" servo positions // to the current ones. This way, the animation system will tween from the // current positions. if (interruptInProgressAnim) { // This is the place to notify someone of an animation finishing after getting interrupted // Print the command string we just finished. -1 parameter indicates it was interrupted. softwareSerial.print("<"); softwareSerial.print(animCompleteStr); softwareSerial.println(",-1>"); // Set the "start" positions to the current ones. So, when // we pay the next anim, we will tween from the current positions. startLeftHip =currLeftHip; startLeftFoot =currLeftFoot; startRightHip =currRightHip; startRightFoot =currRightFoot; // We've handled any interrupt request, so clear the flag. interruptInProgressAnim =false; } // Store the animation we are now playing. currAnim =nextAnim; finishAnim =nextFinishAnim; animCompleteStr[0] =nextAnimCompleteStr[0]; animCompleteStr[1] =nextAnimCompleteStr[1]; nextAnim =NULL; // Queue is cleared. nextFinishAnim =NULL; nextAnimCompleteStr[0] ='-'; nextAnimCompleteStr[1] ='-'; // Record the number of times to play the animation. animNumLoops =nextAnimNumLoops; // Treat current time as start of frame for the initial lerp to the first frame. timeAtStartOfFrame =currTime; // Set the frame counters. targetFrame =1; // First frame we are lerping to. Index 0 is metadata, so skip. // An animation is now in progress animInProgress =true; } //-------------------------------------------------------------------------------------- // If we are currently playing an animation, then update the animation state and the // servo positions. //-------------------------------------------------------------------------------------- if (animInProgress) { // Determine if we need to switch to the next frame. int timeInCurrFrame =currTime - timeAtStartOfFrame; if (timeInCurrFrame> currAnim[targetFrame][TWEEN_TIME_VALUE]) { // Set the servo positions to the targetFrame's values. // We only set this if the value is> 0. -ve values means that // the current target keyframe did not alter that servos position. if (currAnim[targetFrame][LEFT_HIP_VALUE]>=0) { currLeftHip =currAnim[targetFrame][LEFT_HIP_VALUE]; } if (currAnim[targetFrame][LEFT_FOOT_VALUE]>=0) { currLeftFoot =currAnim[targetFrame][LEFT_FOOT_VALUE]; } if (currAnim[targetFrame][RIGHT_HIP_VALUE]>=0) { currRightHip =currAnim[targetFrame][RIGHT_HIP_VALUE]; } if (currAnim[targetFrame][RIGHT_FOOT_VALUE]>=0) { currRightFoot =currAnim[targetFrame][RIGHT_FOOT_VALUE]; } UpdateServos(); // These current values are now the start of frame values. startLeftHip =currLeftHip; startLeftFoot =currLeftFoot; startRightHip =currRightHip; startRightFoot =currRightFoot; // Now, we try to move to the next frame. // - If there is a next frame, set that as the new target, and proceed. // - If there's no next frame, but it's looping, we re-add this animation // to the queue. // - If there's no next frame, and this is not looping, we stop animating. // (Remember that targetFrame is 1-based since the first element of the animation // data array is metadata) // Increment targetFrame, and reset time in the current frame. targetFrame++; timeAtStartOfFrame =currTime; // If there is no next frame, we stop this current animation. // If it is looping, then we re-queue the current animation if the queue is empty. if (targetFrame> NumOfFrames(currAnim)) { // Stop the current animation. animInProgress =false; // If we're looping forever, and there's no next anim, re-queue the // animation if the queue is empty. if ((animNumLoops <0) &&(nextAnim ==NULL)) { LoopAnim(currAnim, finishAnim, animCompleteStr); } // If we're looping forever, and there is something in the queue, then // finish the animation and proceed. else if ((animNumLoops <0) &&(nextAnim !=NULL)) { if (finishAnim !=NULL) { // Switch to the finish anim. currAnim =finishAnim; finishAnim =NULL; // Record the number of times to play the animation. animNumLoops =1; // Treat current time as start of frame for the initial lerp to the first frame. timeAtStartOfFrame =currTime; // Set the frame counters. targetFrame =1; // First frame we are lerping to. Index 0 is metadata, so skip. // An animation is now in progress animInProgress =true; } else { // We've stopped, so can notify if needed. // Print the command string we just finished. softwareSerial.print("<"); softwareSerial.print(animCompleteStr); softwareSerial.println(">"); } } // If we're looping a limited number of times, and there's no next anim, // re-queue the animation if the queue is empty. else if ((animNumLoops> 1) &&(nextAnim ==NULL)) { PlayAnimNumTimes(currAnim, finishAnim, animNumLoops-1, animCompleteStr); } // In this case, numAnimLoops is 1, this is the last loop through, so // we're done. We play the finishAnim first if needed. else { // If there is a finish animation, switch to that animation. if (finishAnim !=NULL) { // Switch to the finish anim. currAnim =finishAnim; finishAnim =NULL; // Record the number of times to play the animation. animNumLoops =1; // Treat current time as start of frame for the initial lerp to the first frame. timeAtStartOfFrame =currTime; // Set the frame counters. targetFrame =1; // First frame we are lerping to. Index 0 is metadata, so skip. // An animation is now in progress animInProgress =true; } // Otherwise, we're done! We've played the finishAnim if there was one. else { // Print the command string we just finished. softwareSerial.print("<"); softwareSerial.print(animCompleteStr); softwareSerial.println(">"); } } } } // If we're still animating (i.e. the previous check didn't find that // we've finished the current animation), then proceed. if (animInProgress) { // Set the servos per data in the current frame. We only update the servos that have target // microsecond values> 0. This is to support the feature where we leave a servo at its // existing position if an animation data item is -1. float frameTimeFraction =(currTime - timeAtStartOfFrame) / ((float) currAnim[targetFrame][TWEEN_TIME_VALUE]); if (currAnim[targetFrame][LEFT_HIP_VALUE]>=0) { currLeftHip =startLeftHip + ((currAnim[targetFrame][LEFT_HIP_VALUE] - startLeftHip) * frameTimeFraction); } if (currAnim[targetFrame][LEFT_FOOT_VALUE]>=0) { currLeftFoot =startLeftFoot + ((currAnim[targetFrame][LEFT_FOOT_VALUE] - startLeftFoot) * frameTimeFraction); } if (currAnim[targetFrame][RIGHT_HIP_VALUE]>=0) { currRightHip =startRightHip + ((currAnim[targetFrame][RIGHT_HIP_VALUE] - startRightHip) * frameTimeFraction); } if (currAnim[targetFrame][RIGHT_FOOT_VALUE]>=0) { currRightFoot =startRightFoot + ((currAnim[targetFrame][RIGHT_FOOT_VALUE] - startRightFoot) * frameTimeFraction); } UpdateServos(); } }}// Move all the servo to the positions set in the curr... variables.// In the code, we update those variables and then call this to set the servos.void UpdateServos(){ servoLeftHip.writeMicroseconds(currLeftHip); servoLeftFoot.writeMicroseconds(currLeftFoot); servoRightHip.writeMicroseconds(currRightHip); servoRightFoot.writeMicroseconds(currRightFoot);}// Return the number of frames in the given animation data.// Have this helper function to avoid the "magic number" reference of animData[0][0].int NumOfFrames(int animData[][5]){ return animData[0][0];}

回路図