BoROID-01 Ver2.00開発室
をテンプレートにして作成
[
トップ
] [
新規
|
一覧
|
検索
|
最終更新
|
ヘルプ
]
開始行:
* BoROID-01 Ver2.00開発室 [#d1f50f7a]
#contents
----
**BoROID-01とは [#h30e5236]
私まさゆきが初めて設計したロボ。~
何もわからない状態から、ROBO-ONE on PCの第2回大会でInvent...
2004年12月に城南通信機さんに板金製作を依頼して、2005年冬...
このときはサーボにHitec HS-5645MGで組み立てていたのだが、...
その後、みなさんのアドバイスでHitec HSR-5995TGに置き換え...
設計の古さや剛性、倒れるたびに破損、起き上がりができない...
要は、作り直すぞってコトです。
**野望(というか、試して見たいこと) [#mfcc33df]
つらつらと
- 外観
-- 基本的には、これまでの無骨さは継承。
- フレーム
-- できるだけ人間的な動きをさせたい。
-- 使用するサーボは違うが、できるだけ「かりんろぼ」と共通...
- CPU
-- 基本的にはモーションプロセッサですが、RTSEMB1200A にす...
- その他
-- 機会があったら、BoROID-01 Ver1.03でモーションを作って...
**臨時: 第10回の時のスクリプト(ロボファイト4終了まで公開)...
- 操縦のスクリプトですが、その場足踏みしかできません(^^;
;-------------------------------------------------------...
;無線操縦メイン 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=押している...
; (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...
;ジャイロ初期化 ------------------
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...
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...
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...
;左右
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...
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,-4...
: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...
MOVE(0,0,0,0,0,0,0,2,0,-7,0,0,0,-2,0,7,0,0,0,0,-30,-50...
;頭部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
;バッテリ状態(誤動作防止のため初期化完了後にメインタス...
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
----
#comment
Total:&counter(total); Today:&counter(today); Yesterday...
終了行:
* BoROID-01 Ver2.00開発室 [#d1f50f7a]
#contents
----
**BoROID-01とは [#h30e5236]
私まさゆきが初めて設計したロボ。~
何もわからない状態から、ROBO-ONE on PCの第2回大会でInvent...
2004年12月に城南通信機さんに板金製作を依頼して、2005年冬...
このときはサーボにHitec HS-5645MGで組み立てていたのだが、...
その後、みなさんのアドバイスでHitec HSR-5995TGに置き換え...
設計の古さや剛性、倒れるたびに破損、起き上がりができない...
要は、作り直すぞってコトです。
**野望(というか、試して見たいこと) [#mfcc33df]
つらつらと
- 外観
-- 基本的には、これまでの無骨さは継承。
- フレーム
-- できるだけ人間的な動きをさせたい。
-- 使用するサーボは違うが、できるだけ「かりんろぼ」と共通...
- CPU
-- 基本的にはモーションプロセッサですが、RTSEMB1200A にす...
- その他
-- 機会があったら、BoROID-01 Ver1.03でモーションを作って...
**臨時: 第10回の時のスクリプト(ロボファイト4終了まで公開)...
- 操縦のスクリプトですが、その場足踏みしかできません(^^;
;-------------------------------------------------------...
;無線操縦メイン 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=押している...
; (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...
;ジャイロ初期化 ------------------
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...
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...
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...
;左右
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...
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,-4...
: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...
MOVE(0,0,0,0,0,0,0,2,0,-7,0,0,0,-2,0,7,0,0,0,0,-30,-50...
;頭部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
;バッテリ状態(誤動作防止のため初期化完了後にメインタス...
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
----
#comment
Total:&counter(total); Today:&counter(today); Yesterday...
ページ名: