![[PukiWiki]](image/pukiwiki.png "[PukiWiki]")
私まさゆきが初めて設計したロボ。
何もわからない状態から、ROBO-ONE on PCの第2回大会でInventer8を使っての設計でした。
2004年12月に城南通信機さんに板金製作を依頼して、2005年冬の第7回ROBO-ONEに出場、棄権となる。
このときはサーボにHitec HS-5645MGで組み立てていたのだが、まともに動けなかった。
その後、みなさんのアドバイスでHitec HSR-5995TGに置き換え動くようにはなったものの、起き上がりができないまま第10回ROBO-ONE棄権に至る。
設計の古さや剛性、倒れるたびに破損、起き上がりができないバランスなど、山篭り(^^;して見直すことにしました。
要は、作り直すぞってコトです。
つらつらと
;---------------------------------------------------------------- ;無線操縦メイン V1.3 ;機能: プロボ操作より、各モーションを呼び出す ; 名前 操作 SW1 SW2 SW3 SW4 操作コマンド ; ------------------ -------------------- ------------ ; 前進 a 11 ; 後退 b 12 ; 横歩き(左) c 13 ; 横歩き(右) d 14 ; 前進(左) ac 21 ; 前進(右) ad 22 ; 左旋回 bc 23 ; 右旋回 bd 24 ; ------------------ -------------------- ------------ ; 右パンチ 1 31 ; 左パンチ 3 32 ; 両手アッパー 5 33 ; スイングパンチ1 4 B 34 ; スイングパンチ2 2 B 35 ; ------------------ -------------------- ------------ ; 右発射体/後転倒 4 A 71 ; 左発射体/前転倒 2 A 72 ; ------------------ -------------------- ------------ ; 起き上がり A 77 ; ホームポジション A/B -- ; ------------------ -------------------- ------------ ; グレー/レッド同時押しでExit ;共通: 変数一覧 ; (N32 = LED色 ; (N33 = 前回LED色 ; (N40 = サブタスク初期化(0=初期化中,1=完了,2=停止) ; (N41 = 加速度センサ原点(前後) ; (N42 = 加速度センサ原点(左右) ; (N43 = 転倒方向(0=非転倒,1=左,2=右,3=前,4=後) ; (N44 = バッテリレベル(4=H,3=M,1=L,0=LL) ; (N46 = ホームポジション(0=高,1=低) ; (N50 = 今回キー入力判定値 ; (N51 = 前回キー入力判定値 ; (N52 = 今回操作コマンド ; (N53 = 前回操作コマンド ; (N54 = 今回操作状態(0=未処理,1=押した,2=押している,3=離した) ; (N253= ローバッテリタイマー ; (N254= キー入力タイマー ; ワーク変数一覧 ; (N60 -(N99 = メインタスク計算用(歩行パラメタ含む) ; V00 - V09 = メインタスク計算用 ; V10 - V10 = サブタスク計算用 ; V11 - V11 = LED制御 ;ポート: ; 頭部LED(DO 1=R,2=B,3=G) ; 低速シリアル(V12,V13), 高速シリアル(V14-V22) ; ジャイロ(V23-V25) ;バッテリモニタ: ; HtoH2でONにすると自動的に有効 ; (N44=0で停止モーション後にバッテリモニタが動作するよう ; 設定を調整すること ;---------------------------------------------------------------- :START ;---------------------------------------------------------------- ;初期化 ;---------------------------------------------------------------- ;頭部LED ------------------------- ;前回LED色初期値 (N33 = 0 ;ホームポジション ---------------- HomeSet(0,0,0,0,88,0,102,120,91,93,85,0,68,67,88,81,90,0,107,110,116,110,85,93,0,71,71,60,64,93,94,0) ;ジャイロ初期化 ------------------ MOVE(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,200,1,N) GyroReset(0) GyroMode(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0) GyroMode(1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0) ;左右 GyroAdjust(0,19,2) GyroAdjust(0,24,2) GyroAdjust(0,26,2) GyroAdjust(0,31,2) ;前後 GyroAdjust(1,23,-2) GyroAdjust(1,30,21) ;GyroSwitch(1,1,0) ;サブタスク開始 ------------------ (N40 = 0 TaskSet(SUBTASK) TaskCall(1) ;ローバッテリ誤動作防止 Wait(50) (N44 = 4 ;操作コマンド関係 ---------------- ;ホームポジション位置 (N46 = 0 ;前回コマンド初期値 (N53 = 99 ;---------------------------------------------------------------- ;メインループ ;---------------------------------------------------------------- :MAINLOOP ;サブタスク初期化チェック V00 = (N40 JumpIf(V00,=,0,MAINLOOP) ;サブタスク緊急停止チェック JumpIf(V00,=,2,END) ;ローバッテリ判定 V00 = (N44 JumpIf(V00,=,0,LOWBATT) ;バッテリーレベル表示 (N32 = (N44 Call(LEDSW) :MAL01EXIT ;前回操作後処理 ------------------ V00 = (N52 V01 = (N53 V02 = (N54 JumpIf(V01,=,99,MAL02S) JumpIf(V00,=,V01,MAL02S) JumpIf(V02,=,0,MAL02S) ;押されていた場合、押しっぱなし/離した時の動作 ;押しっぱなしの場合、離した時の動作 JumpIf(V02,=,2,MAL02P) (N54 = 2 V00 = (N53 V01 = (N54 Call(MOVEJUMP) :MAL02P (N54 = 3 V00 = (N53 V01 = (N54 Call(MOVEJUMP) (N54 = 0 :MAL02S ;今回操作処理 -------------------- (N53 = (N52 V00 = (N53 V01 = (N54 ;ボタン判定値0ならホームポジション設定へ JumpIf(V00,=,0,MAL03S) ;押されていた場合、押したとき/押しっぱなしの動作 JumpIf(V01,#,0,MAL02C) (N54 = 1 V00 = (N53 V01 = (N54 Call(MOVEJUMP) Jump(MAINLOOP) :MAL02C (N54 = 2 V00 = (N53 V01 = (N54 Call(MOVEJUMP) Jump(MAINLOOP) :MAL03S ;プロボSW3(A=低姿勢,B=高姿勢) V00 = V15 AND 4 V01 = (N46 Jumpif(V00,=,0,MAL03L) :MAL03H ;高姿勢ホームポジションへ移行 Jumpif(V01,=,0,MAL03EXIT) (N46 = 0 ;◆[高姿勢モーション]============= Wait(50) MOVE(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,300,1,N) Jump(MAL03EXIT) :MAL03L ;低姿勢ホームポジションへ移行 Jumpif(V01,=,1,MAL03EXIT) (N46 = 1 ;◆[低姿勢モーション]============= Wait(50) MOVE(0,0,0,0,0,0,0,0,0,-10,0,0,0,0,0,10,0,0,0,0,-20,-40,-20,0,0,0,0,20,40,20,0,0,200,1,N) :MAL03EXIT Jump(MAINLOOP) ;---------------------------------------------------------------- ;モーション分岐SUB ;入力: V00 = ボタン判定値 ; V01 = 再生モーション(1=開始,2=継続,3=終了) ;---------------------------------------------------------------- :MOVEJUMP ;テスト表示(判定値表示) ---------- Jump(MJS01EXIT) ;デバッグ表示 (N32 = 0 Call(LEDSW) Wait(100) (N32 = V00 / 10 Call(LEDSW) Wait(30) (N32 = 0 Call(LEDSW) Wait(20) (N32 = V00 / 10 (N32 = (N32 * 10 (N32 = V00 - (N32 Call(LEDSW) Wait(30) (N32 = 0 Call(LEDSW) Wait(20) (N32 = V01 Call(LEDSW) Wait(30) (N32 = 0 Call(LEDSW) Wait(100) :MJS01EXIT ;モーション実行 ------------------ V01 = V01 * 100 V00 = V00 + V01 ;内部でV00-V09が使用可能 JumpIf(V00,=,111,L111) JumpIf(V00,=,211,L211) JumpIf(V00,=,311,L311) JumpIf(V00,=,112,L112) JumpIf(V00,=,212,L212) JumpIf(V00,=,312,L312) JumpIf(V00,=,113,L113) JumpIf(V00,=,213,L213) JumpIf(V00,=,313,L313) JumpIf(V00,=,114,L114) JumpIf(V00,=,214,L214) JumpIf(V00,=,314,L314) JumpIf(V00,=,121,L121) JumpIf(V00,=,221,L221) JumpIf(V00,=,321,L321) JumpIf(V00,=,122,L122) JumpIf(V00,=,222,L222) JumpIf(V00,=,322,L322) JumpIf(V00,=,123,L123) JumpIf(V00,=,223,L223) JumpIf(V00,=,323,L323) JumpIf(V00,=,124,L124) JumpIf(V00,=,224,L224) JumpIf(V00,=,324,L324) JumpIf(V00,=,131,L131) JumpIf(V00,=,132,L132) JumpIf(V00,=,133,L133) JumpIf(V00,=,134,L134) JumpIf(V00,=,135,L135) JumpIf(V00,=,171,L171) JumpIf(V00,=,172,L172) JumpIf(V00,=,177,L177) Return ;---------------------------------------------------------------- ;◆歩行モーションSUB ;---------------------------------------------------------------- ;定数設定 :WLKIN ;蹴り-上肢増分 (N60 = 8 ;引き-上肢増分/反対軸角度増分 (N62 = 10 (N63 = 8 ;移動-前後移動/左右移動(その場足踏み用設定値) (N64 = 0 (N65 = 0 ;移動-振り補正値 (N66 = 1 Return ;左足上げ :WLKLU ;上に蹴る V00 = 0 - (N60 V01 = V00 * 2 AutoSet(28,V00) AutoSet(29,V01) AutoSet(30,V00) AutoMove(10,1,N) ;足を引く/反対軸角度調整 V00 = 0 - (N63 AutoSet(19,V00) V00 = (N65 V01 = (N64 V02 = 0 - V01 AutoSet(26,V00) AutoSet(31,V00) AutoSet(21,V01) AutoSet(23,V02) V00 = 0 - (N66 AutoSet(5,V00) V00 = (N60 + (N62 V01 = V00 * 2 AutoSet(28,V00) AutoSet(29,V01) AutoSet(30,V00) AutoMove(10,1,N) Return ;左足下げ :WLKLD ;足を着地 V00 = (N63 AutoSet(19,V00) V00 = 0 - (N65 V01 = 0 - (N64 V02 = 0 - V01 AutoSet(26,V00) AutoSet(31,V00) AutoSet(21,V01) AutoSet(23,V02) V00 = (N66 AutoSet(5,V00) V00 = 0 - (N62 V01 = V00 * 2 AutoSet(28,V00) AutoSet(29,V01) AutoSet(30,V00) AutoMove(20,1,N) Return ;右足上げ :WLKRU ;上に蹴る V00 = (N60 V01 = V00 * 2 AutoSet(21,V00) AutoSet(22,V01) AutoSet(23,V00) AutoMove(10,1,N) ;足を引く/反対軸角度調整 V00 = (N63 AutoSet(26,V00) V00 = 0 - (N65 V01 = 0 - (N64 V02 = 0 - V01 AutoSet(19,V00) AutoSet(24,V00) AutoSet(28,V01) AutoSet(30,V02) V00 = (N66 AutoSet(5,V00) V00 = 0 - (N60 - (N62 V01 = V00 * 2 AutoSet(21,V00) AutoSet(22,V01) AutoSet(23,V00) AutoMove(10,1,N) Return ;右足下げ :WLKRD ;足を着地 V00 = 0 - (N63 AutoSet(26,V00) V00 = (N65 V01 = (N64 V02 = 0 - V01 AutoSet(19,V00) AutoSet(24,V00) AutoSet(28,V01) AutoSet(30,V02) V00 = 0 - (N66 AutoSet(5,V00) V00 = (N62 V01 = V00 * 2 AutoSet(21,V00) AutoSet(22,V01) AutoSet(23,V00) AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[前進-開始] ;---------------------------------------------------------------- :L111 ;定数初期化 Call(WLKIN) ;移動-前後移動/左右移動/かかと角度 (N64 = (N64 + 2 (N65 = (N65 + 0 (N67 = 10 ;移動-振り補正値 (N66 = 0 ;上体起こす (N100 = 5 (N101 = 2 V00 = 0 - (N100 V01 = 0 - (N101 V02 = (N100 v03 = (N101 AutoSet(21,V00) AutoSet(22,V01) AutoSet(28,V02) AutoSet(29,V03) AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[前進-継続] ;---------------------------------------------------------------- :L211 ;足踏み Call(WLKRU) Call(WLKRD) Call(WLKLU) Call(WLKLD) Return ;---------------------------------------------------------------- ;◆モーション[前進-終了] ;---------------------------------------------------------------- :L311 ;上体戻す V00 = (N100 V01 = (N101 V02 = 0 - (N100 v03 = 0 - (N101 AutoSet(21,V00) AutoSet(22,V01) AutoSet(28,V02) AutoSet(29,V03) AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[後退-開始] ;---------------------------------------------------------------- :L112 ;定数初期化 Call(WLKIN) ;移動-前後移動/左右移動/かかと角度 (N64 = (N64 - 0 (N65 = (N65 + 0 (N67 = 0 ;移動-振り補正値 (N66 = 0 ;上体起こす (N100 = 0 (N101 = 0 V00 = 0 - (N100 V01 = 0 - (N101 V02 = (N100 v03 = (N101 AutoSet(21,V00) AutoSet(22,V01) AutoSet(28,V02) AutoSet(29,V03) AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[後退-継続] ;---------------------------------------------------------------- :L212 ;足踏み Call(WLKRU) Call(WLKRD) Call(WLKLU) Call(WLKLD) Return ;---------------------------------------------------------------- ;◆モーション[後退-終了] ;---------------------------------------------------------------- :L312 ;上体戻す V00 = (N100 V01 = (N101 V02 = 0 - (N100 v03 = 0 - (N101 AutoSet(21,V00) AutoSet(22,V01) AutoSet(28,V02) AutoSet(29,V03) AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[横歩き(左)-開始] ;---------------------------------------------------------------- :L113 ;■ ;定数初期化 Call(WLKIN) ;移動-前後移動/左右移動 (N64 = (N64 + 0 (N65 = (N65 - 2 ;上体起こす ; AutoSet(28,-2) ; AutoSet(21, 2) ; AutoMove(10,1,N) ;右足押し出し ; AutoSet(19,-2) ; AutoMove(1,1,N) Return ;---------------------------------------------------------------- ;◆モーション[横歩き(左)-継続] ;---------------------------------------------------------------- :L213 ;■ ;足踏み Call(WLKLU) Call(WLKLD) ;右足復帰/左足押し出し ; AutoSet(19, 2) ; AutoSet(26,-2) ; AutoMove(1,1,N) Call(WLKRU) Call(WLKRD) ;左足復帰/右足押し出し ; AutoSet(19,-2) ; AutoSet(26, 2) ; AutoMove(1,1,N) Return ;---------------------------------------------------------------- ;◆モーション[横歩き(左)-終了] ;---------------------------------------------------------------- :L313 ;■ ;上体戻す ;右足復帰 ; AutoSet(19, 2) ; AutoMove(1,1,N) ; AutoSet(28, 2) ; AutoSet(21,-2) ; AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[横歩き(右)-開始] ;---------------------------------------------------------------- :L114 ;■ ;定数初期化 Call(WLKIN) ;移動-前後移動/左右移動 (N64 = (N64 + 0 (N65 = (N65 + 2 ;上体起こす ; AutoSet(28,-2) ; AutoSet(21, 2) ; AutoMove(10,1,N) ;左足押し出し ; AutoSet(26, 2) ; AutoMove(1,1,N) Return ;---------------------------------------------------------------- ;◆モーション[横歩き(右)-継続] ;---------------------------------------------------------------- :L214 ;■ ;足踏み Call(WLKRU) Call(WLKRD) ;左足復帰/右足押し出し ; AutoSet(19, 2) ; AutoSet(26,-2) ; AutoMove(1,1,N) Call(WLKLU) Call(WLKLD) ;右足復帰/左足押し出し ; AutoSet(19,-2) ; AutoSet(26, 2) ; AutoMove(1,1,N) Return ;---------------------------------------------------------------- ;◆モーション[横歩き(右)-終了] ;---------------------------------------------------------------- :L314 ;■ ;上体戻す ;左足復帰 ; AutoSet(26,-2) ; AutoMove(1,1,N) ; AutoSet(28, 2) ; AutoSet(21,-2) ; AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[前進(左)-開始] ;---------------------------------------------------------------- :L121 ;■ ;上体起こす ; AutoSet(28,-2) ; AutoSet(21, 2) ; AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[前進(左)-継続] ;---------------------------------------------------------------- :L221 ;■ ;足踏み ; Call(WLKRU) ; Call(WLKRD) ; Call(WLKLU) ; Call(WLKLD) Return ;---------------------------------------------------------------- ;◆モーション[前進(左)-終了] ;---------------------------------------------------------------- :L321 ;■ ;上体戻す ; AutoSet(28, 2) ; AutoSet(21,-2) ; AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[前進(右)-開始] ;---------------------------------------------------------------- :L122 ;■ ;上体起こす ; AutoSet(28,-2) ; AutoSet(21, 2) ; AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[前進(右)-継続] ;---------------------------------------------------------------- :L222 ;■ ;足踏み ; Call(WLKRU) ; Call(WLKRD) ; Call(WLKLU) ; Call(WLKLD) Return ;---------------------------------------------------------------- ;◆モーション[前進(右)-終了] ;---------------------------------------------------------------- :L322 ;■ ;上体戻す ; AutoSet(28, 2) ; AutoSet(21,-2) ; AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[左旋回-開始] ;---------------------------------------------------------------- :L123 ;■ ;上体起こす ; AutoSet(28,-2) ; AutoSet(21, 2) ; AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[左旋回-継続] ;---------------------------------------------------------------- :L223 ;■ ;足踏み ; Call(WLKRU) ; Call(WLKRD) ; Call(WLKLU) ; Call(WLKLD) Return ;---------------------------------------------------------------- ;◆モーション[左旋回-終了] ;---------------------------------------------------------------- :L323 ;■ ;上体戻す ; AutoSet(28, 2) ; AutoSet(21,-2) ; AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[右旋回-開始] ;---------------------------------------------------------------- :L124 ;■ ;上体起こす ; AutoSet(28,-2) ; AutoSet(21, 2) ; AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[右旋回-継続] ;---------------------------------------------------------------- :L224 ;■ ;足踏み ; Call(WLKRU) ; Call(WLKRD) ; Call(WLKLU) ; Call(WLKLD) Return ;---------------------------------------------------------------- ;◆モーション[右旋回-終了] ;---------------------------------------------------------------- :L324 ;■ ;上体戻す ; AutoSet(28, 2) ; AutoSet(21,-2) ; AutoMove(10,1,N) Return ;---------------------------------------------------------------- ;◆モーション[右パンチ] ;---------------------------------------------------------------- :L131 ;■ ;Jumpif(V11,=,1,L131L) ;パンチ ;-------------------------------- ;AutoSet( 5,-50) ;AutoSet( 7,-10) ;AutoSet( 8,-41) ;AutoSet(10, 10) ;AutoSet(28, 10) ;AutoSet(29, 20) ;AutoSet(30, 10) ;AutoSet(21,-10) ;AutoSet(22,-20) ;AutoSet(23,-10) ;AutoMove(150,1,N) ;Wait(50) ;戻り ;-------------------------------- ;AutoSet( 5, 50) ;AutoSet( 8, 41) ;AutoSet(10,-10) ;AutoSet(28,-10) ;AutoSet(29,-20) ;AutoSet(30,-10) ;AutoSet(21, 10) ;AutoSet(22, 20) ;AutoSet(23, 10) ;AutoSet( 7,-10) ;AutoMove(150,1,N) ;AutoSet( 7, 20) ;AutoMove(50,1,N) Return :L131L ;パンチ ;-------------------------------- ;AutoSet( 5,-50) ;AutoSet( 8,-31) ;AutoSet(10, 20) ;AutoSet( 7,-17) ;AutoMove(150,1,N) ;AutoSet( 7, 17) ;AutoMove(50,1,N) ;Wait(50) ;戻り ;-------------------------------- ;AutoSet( 5, 50) ;AutoSet( 8, 31) ;AutoSet(10,-20) ;AutoSet( 7,-17) ;AutoMove(150,1,N) ;AutoSet( 7, 17) ;AutoMove(50,1,N) Return ;---------------------------------------------------------------- ;◆モーション[左パンチ] ;---------------------------------------------------------------- :L132 ;■ ;Jumpif(V11,=,1,L132L) ;パンチ ;-------------------------------- ;AutoSet( 5, 50) ;AutoSet(13, 10) ;AutoSet(14, 41) ;AutoSet(16,-10) ;AutoSet(21,-10) ;AutoSet(22,-20) ;AutoSet(23,-10) ;AutoSet(28, 10) ;AutoSet(29, 20) ;AutoSet(30, 10) ;AutoMove(150,1,N) ;Wait(50) ;戻り ;-------------------------------- ;AutoSet( 5,-50) ;AutoSet(14,-41) ;AutoSet(16, 10) ;AutoSet(21, 10) ;AutoSet(22, 20) ;AutoSet(23, 10) ;AutoSet(28,-10) ;AutoSet(29,-20) ;AutoSet(30,-10) ;AutoSet(13, 10) ;AutoMove(150,1,N) ;AutoSet(13,-20) ;AutoMove(50,1,N) Return :L132L ;パンチ ;-------------------------------- ;AutoSet( 5, 50) ;AutoSet(14, 31) ;AutoSet(16,-20) ;AutoSet(13, 17) ;AutoMove(150,1,N) ;AutoSet(13,-17) ;AutoMove(50,1,N) ;Wait(50) ;戻り ;-------------------------------- ;AutoSet( 5,-50) ;AutoSet(14,-31) ;AutoSet(16, 20) ;AutoSet(13, 17) ;AutoMove(100,1,N) ;AutoSet(13,-17) ;AutoMove(150,1,N) Return ;---------------------------------------------------------------- ;◆モーション[両手アッパー] ;---------------------------------------------------------------- :L133 ;■ ;V05 = 1 ;V06 = 1 ;Call(LEDSW) ;Wait(30) Return ;---------------------------------------------------------------- ;◆モーション[スイングパンチ1] ;---------------------------------------------------------------- :L134 ;■ ;V05 = 1 ;V06 = 1 ;Call(LEDSW) ;Wait(30) Return ;---------------------------------------------------------------- ;◆モーション[スイングパンチ2] ;---------------------------------------------------------------- :L135 ;■ ;V05 = 1 ;V06 = 1 ;Call(LEDSW) ;Wait(30) Return ;---------------------------------------------------------------- ;◆モーション[右発射体/後転倒] ;---------------------------------------------------------------- :L171 ;■ ;V05 = 1 ;V06 = 1 ;Call(LEDSW) ;Wait(30) Return ;---------------------------------------------------------------- ;◆モーション[左発射体/前転倒] ;---------------------------------------------------------------- :L172 ;■ ;V05 = 1 ;V06 = 1 ;Call(LEDSW) ;Wait(30) Return ;---------------------------------------------------------------- ;◆モーション[起き上がり] ;---------------------------------------------------------------- :L177 ;転倒判定 ------------------------ ;V01 = InputADC(1) ;左 ;JumpIf(V01,>,135,L177HAND) ;右 ;JumpIf(V01,<,115,L177HAND) ;V01 = InputADC(0) ;前 ;JumpIf(V01,>,135,L177FORWORD) ;後 ;JumpIf(V01,<,115,L177BACK) ;転倒していない Return ;側転倒 -------------------------- :L177HAND ;腕で仰向けまたはうつ伏せにする ;■ ;V05 = 3 ;V06 = 1 ;Call(LEDSW) ;Wait(30) Jump(L177) ;うつ伏せ ------------------------ :L177FORWORD ;■ ;V05 = 7 ;V06 = 1 ;Call(LEDSW) ;Wait(30) Return ;仰向け -------------------------- :L177BACK ;■ ;V05 = 6 ;V06 = 1 ;Call(LEDSW) ;Wait(30) Return ;---------------------------------------------------------------- ;ローバッテリモーション ;---------------------------------------------------------------- :LOWBATT ;バッテリチェック停止 TaskCall(0) ;頭部LEDピンク (N32 = 5 Call(LEDSW) ;◆[転倒防止モーション]=========== HomeMove(500,1,N) MOVE(0,0,0,0,0,0,0,2,0,-7,0,0,0,-2,0,7,0,0,0,0,-20,-50,-29,0,0,0,0,20,50,29,0,0,500,1,N) MOVE(0,0,0,0,0,0,0,2,0,-7,0,0,0,-2,0,7,0,0,0,0,-30,-50,-29,0,0,0,0,30,50,29,0,0,500,1,N) ;頭部LED水色 (N32 = 6 Call(LEDSW) ;ジャイロ停止 GyroSwitch(0,0,0) Jump(END) ;---------------------------------------------------------------- ;サブタスク(センサ値および操作コマンド判定) ;---------------------------------------------------------------- :SUBTASK ;一度消灯して点灯 Walt(500) (N32 = 0 Call(LEDSW) Walt(50) (N32 = 7 Call(LEDSW) Walt(50) ;加速度センサ原点(誤動作するため DATA CHECKによる測定値を指定) ; (N41 = InputADC(1) ; (N42 = InputADC(0) (N41 = 123 (N42 = 128 (N43 = 0 ;バッテリ状態(誤動作防止のため初期化完了後にメインタスクで「(N44 = 4」を行うこと) V10 = InputADC(7) (N44 = 4 (N253 = 0 ;通信モード判定初期化 V14 = 0 ;キー入力判定初期化 (N50 = 0 ;操作コマンド初期化 (N52 = 0 ;初期化完了 (N40 = 1 :SUBTASKLOOP ;加速度センサ値取得 V10 = InputADC(1) V10 = V10 + 50 V10 = V10 - (N41 ;左 JumpIf(V10,>,55,STL01L) ;右 JumpIf(V10,<,45,STL01R) V10 = InputADC(0) V10 = V10 + 50 V10 = V10 - (N42 ;前 JumpIf(V10,>,57,STL01F) ;後 JumpIf(V10,<,43,STL01B) ;転倒していない (N43 = 0 Jump(STL01EXIT) :STL01L (N43 = 1 Jump(STL01EXIT) :STL01R (N43 = 2 Jump(STL01EXIT) :STL01F (N43 = 3 Jump(STL01EXIT) :STL01B (N43 = 4 :STL01EXIT ;バッテリーチェック V10 = (N44 JumpIf(V10,=,0,STL02EXIT) V10 = InputADC(7) JumpIf(V10,<,100,STL02R) (N253 = 0 JumpIf(V10,<,105,STL02Y) ;H - 6.5V?以上 (N44 = 4 Jump(STL02EXIT) :STL02Y ;M - 6.1V?以上 (N44 = 3 Jump(STL02EXIT) :STL02R V10 = (N253 JumpIf(V10,>,700,STL02L) ;L - 6.1V未満 (N44 = 1 Jump(STL02EXIT) :STL02L ;LL - LowBatt (N44 = 0 :STL02EXIT ;緊急停止 - グレー/レッド同時ON(08H/10H=24がLow) V10 = InputD V10 = V10 AND 24 JumpIf(V10,#,0,STL03EXIT) (N40 = 2 :STL03EXIT ;シリアル入力 JumpIf(V14,=,17,STL04I) ;高速シリアル未使用ならProboデータで上書き V15 = V12 V16 = V13 :STL04I ;入力文字判定((N50に終了コード) (N51 = (N50 Call(InputChk) V10 = (N50 + 100 V10 = V10 - (N51 JumpIf(V10,#,100,STL04R) ;一定時間維持したら採用 - 20=0.1s V10 = (N254 JumpIf(V10,>,20,STL04S) Jump(STL04EXIT) :STL04R ;キー入力タイマーリセット (N254 = 0 Jump(STL04EXIT) :STL04S ;操作コマンドを設定 (N52 = (N50 :STL04EXIT Jump(SUBTASKLOOP) ;---------------------------------------------------------------- ;LED表示ON/OFF ;入力: (N32 = 色(R=1,G=2,B=4) ;---------------------------------------------------------------- :LEDSW V11 = (N32 + 10 V11 = V11 - (N33 JumpIf(V11,=,10,LEDSWEXIT) (N33 = (N32 RedGainSet(1,5) RedGainSet(2,5) RedGainSet(3,5) V11 = (N32 JumpIf(V11,=,0,LEDSWEXIT) V11 = (N32 AND 1 JumpIf(V11,#,1,LEDSW2) RedGainSet(1,0) :LEDSW2 V11 = (N32 AND 2 JumpIf(V11,#,2,LEDSW4) RedGainSet(3,0) :LEDSW4 V11 = (N32 AND 4 JumpIf(V11,#,4,LEDSWEXIT) RedGainSet(2,0) :LEDSWEXIT Return ;---------------------------------------------------------------- ;入力チェック(サブタスク専用) ;入力: V15 = SW1-4入力 ; V16 = 方向および1-5入力 ; (N50 = 判定値(終了コード) ; V10 = ワークで使用 ;---------------------------------------------------------------- :InputChk ;前進 ---------------------------- (N50 = 11 JumpIf(V16,=,1,InputChkExt) ;後退 ---------------------------- (N50 = 12 JumpIf(V16,=,2,InputChkExt) ;横歩き(左) ---------------------- (N50 = 13 JumpIf(V16,=,8,InputChkExt) ;横歩き(右) ---------------------- (N50 = 14 JumpIf(V16,=,4,InputChkExt) ;前進(左) ------------------------ (N50 = 21 JumpIf(V16,=,9,InputChkExt) ;前進(右) ------------------------ (N50 = 22 JumpIf(V16,=,5,InputChkExt) ;左旋回 -------------------------- (N50 = 23 JumpIf(V16,=,10,InputChkExt) ;右旋回 -------------------------- (N50 = 24 JumpIf(V16,=,6,InputChkExt) ;右パンチ ------------------------ (N50 = 31 JumpIf(V16,=,16,InputChkExt) ;左パンチ ------------------------ (N50 = 32 JumpIf(V16,=,128,InputChkExt) ;両手アッパー -------------------- (N50 = 33 JumpIf(V16,=,144,InputChkExt) ;[SW1=B]-------------------------- V10 = V15 AND 1 JumpIf(V10,=,0,InputChkSW1A1) ;スイングパンチ1 ----------------- (N50 = 34 JumpIf(V16,=,64,InputChkExt) ;スイングパンチ2 ----------------- (N50 = 35 JumpIf(V16,=,32,InputChkExt) Jump(InputChkSW1C1) ;[SW1=A]-------------------------- :InputChkSW1A1 ;右発射体 ------------------------ (N50 = 71 JumpIf(V16,=,64,InputChkExt) ;左発射体 ------------------------ (N50 = 72 JumpIf(V16,=,32,InputChkExt) :InputChkSW1C1 ;起き上がり ---------------------- (N50 = 77 V10 = V15 AND 2 JumpIf(V10,=,0,InputChkExt) (N50 = 0 :InputChkExt Return ;---------------------------------------------------------------- ;終了 ;---------------------------------------------------------------- :END ;ジャイロ停止 GyroSwitch(0,0,0) ;サブタスク停止 TaskCall(0) Wait(50) ;頭部LED点滅 (N32 = 0 Call(LEDSW) Wait(200) (N32 = 1 Call(LEDSW) Wait(20) (N32 = 0 Call(LEDSW) Wait(20) (N32 = 1 Call(LEDSW) Wait(20) (N32 = 0 Call(LEDSW) Wait(20) (N32 = 1 Call(LEDSW) Wait(20) (N32 = 0 Call(LEDSW) (N255 = 99 Exit
Total:1787 Today:1 Yesterday:0
