;┌───────────────────────────────────┐
;│＊ F08_自動起き上がりファイル ＊　        　　　　　　　　　　　　　　│
;└───────────────────────────────────┘
;(有)姫路ソフトワークス
; Modify by 二名川↓
;  2013/01/17 調整
;
;┏━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━┓
;┃自動起き上がりシークエンス　　        　　　　　　　　　　　　　　　　┃
;┗━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━┛
JUMPIF(V126,=,3,EIEI);スタートならえいえいおー
:OKIAGARI
CALL(OKBEEP) 
JUMPIF(V126,=,12,OKIAGARISTART);ビープの後もセレクト押し続けで脱力なし
WAIT(100)
V00=0
V01=0
V02=0
V03=0
V04=0
V05=0
V06=0
V07=0
V08=0
V09=0
V10=0
V11=0
V12=0
V13=0
V14=0
V15=0
V16=0
V17=0
V18=0
V19=0
V20=0
V21=0
V22=0
V23=0
POWER
(50)
:OKIAGARIWAIT
JUMPIF(V126,=,12,OKIAGARISTART);脱力中のスタートボタンで起き上がり開始。
JUMPIF(V126,=,3,OKIAGARISTART);脱力中のスタートボタンで起き上がり開始。
CALL(CALLBEEP) 
WAIT(100)
JUMP(OKIAGARIWAIT)
;
:OKIAGARISTART
CALL(OKBEEP) 
V00=1
V01=1
V02=1
V03=1
V04=1
V05=1
V06=1
V07=1
V08=1
V09=1
V10=1
V11=1
V12=1
V13=1
V14=1
V15=1
V16=1
V17=1
V18=1
V19=1
V20=1
V21=1
V22=1
V23=1
POWER
(50)
;
;┌──＜ まずホームポジションへ ＞───────────────────┐
MOVE(0,100,200,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-300,-100,0,480)
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,480)
;
;┌──＜ 電源をオフし、腕の下がる向きを判定 ＞─────────────┐
;・腕が重力に従って後ろに下がったら、仰向け。[USHIRO]へ。
CALL(POWERDOWN)
MOVE(-400,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,400,1200)
WAIT(200)
V50=V505
V51=V643
V50=V50-V457
V51=V51-V480
JUMPIF(V51,<,370,USIRO)
;
;・腕が重力に従って前に下がったら、うつ伏せ。[MAE]へ。
MOVE(600,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,-600,1200)
WAIT(200)
V50=V505
V51=V643
V50=V50-V457
V51=V51-V480
JUMPIF(V50,<,300,MAE)
;
;・腕がそのままなら、立っているので抜ける。
:POWERONEXIT
CALL(POWERON)
JUMP(EXIT)
;
;┌──＜【MAE】うつ伏せ起き上がり ＞──────────────────┐
:MAE
CALL(POWERON)
;うつ伏せ起き上がり 
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,1200)
;01
MOVE(1,525,898,438,x,-229,-78,-129,43,5,-1,-660,297,2,-215,-207,108,34,-8,x,187,-283,-508,69,1200)
;02
MOVE(873,433,904,439,x,-107,-57,-42,79,-942,7,-53,52,-1,927,-71,38,63,-9,x,105,-859,-506,-892,1200)
;03
MOVE(872,186,901,-476,x,-110,-56,-55,91,-927,15,-71,65,4,909,-77,43,59,2,x,419,-924,-37,-935,1200)
;04
MOVE(834,-341,916,-431,x,-859,11,792,1055,-1012,21,-613,-101,256,906,-734,132,260,8,x,222,-877,244,-989,900)
;05
MOVE(355,-220,910,187,x,-777,23,911,471,-541,-3,-1068,-88,252,886,-1058,218,268,-4,x,111,-710,72,-513,600)
;06
MOVE(283,-48,782,-747,x,-448,-168,732,483,-269,-326,-558,307,107,904,-858,-211,269,-319,x,98,-742,-274,-195,1200)
;07
MOVE(72,63,1203,-347,x,-716,-203,458,510,-370,-331,-596,500,115,573,-917,-601,283,-236,x,18,-939,-208,-246,600)
;07b
MOVE(52,-139,1043,-334,x,-685,-275,130,530,-546,-204,-539,103,209,563,-826,27,254,-242,x,84,-829,-121,-218,600)
;08
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,1200)
JUMP(EXIT)
;└───────────────────────────────────┘
;
;┌──＜【USIRO】仰向け起き上がり ＞──────────────────┐
:USIRO
CALL(POWERON)
;仰向け起き上がり 
MOVE(96,190,0,106,x,22,-3,4,-5,-1,-3,-1,-1,-4,-2,5,-4,0,-8,x,26,0,-393,58,1200)
;エビゾって腕を頭後ろにし、胴を持ち上げる。
MOVE(1980,-50,760,-380,x,150,-7,-242,416,-583,33,-746,634,17,521,-498,223,29,-1,x,320,-760,-50,-1980,1800)
;腕を締めてブリッジ気味に
MOVE(2270,-300,935,-280,x,145,23,-243,632,-967,35,-737,633,-6,936,-694,257,-4,-7,x,280,-921,300,-2255,900)
;右手を回して足の方へ
MOVE(2154,-474,935,-183,x,143,19,-243,629,-923,34,-740,633,-7,884,-692,256,-7,19,x,-23,-917,-60,133,600)
;左手も回して足の方へ
MOVE(-133,60,935,20,x,143,19,-243,629,-923,34,-740,633,-7,884,-692,256,-7,19,x,-20,-917,-60,133,600)
;背筋を伸ばししゃがんだ状態
MOVE(470,-120,800,-190,x,-367,26,-8,648,-1009,27,-742,719,-18,1002,-666,2,13,-15,x,190,-800,120,-470,1200)
;ホームポジションへ
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,1800)
JUMP(EXIT)
;└───────────────────────────────────┘
;┌──＜ 【EIEI】えいえいおー ＞────────────────────┐
:EIEI
CALL(OKBEEP) 
;
V109 = 101
V120=0
V121=0
;えいえい 
;ファイトホーム
MOVE(-450,-226,931,-946,x,-120,-80,383,331,-329,-90,-331,331,70,329,-331,-383,80,0,x,978,-928,180,450,600)
MOVE(-559,-178,1008,-599,x,-17,-106,369,370,-356,-120,-401,382,87,351,-409,-384,94,-145,x,594,-1008,79,-525,900)
MOVE(-296,-120,926,-754,x,-176,-260,277,161,-459,-171,-740,182,156,291,-632,-348,62,-123,x,695,-820,172,-703,1200)
;
MOVE(-364,-165,1019,-840,x,-25,-182,14,102,-274,-113,-365,52,121,148,-242,34,51,-120,x,-43,-510,244,-1399,900)
MOVE(-364,-165,1019,-840,x,-25,-182,14,102,-274,-113,-365,52,121,148,-242,34,51,-120,x,-43,-510,244,-1399,600)
MOVE(-296,-120,926,-754,x,-176,-260,277,161,-459,-171,-740,182,156,291,-632,-348,62,-123,x,695,-820,172,-703,900)
MOVE(-296,-120,926,-754,x,-176,-260,277,161,-459,-171,-740,182,156,291,-632,-348,62,-123,x,695,-820,172,-703,600)
;
MOVE(-364,-165,1019,-840,x,-25,-182,14,102,-274,-113,-365,52,121,148,-242,34,51,-120,x,-43,-510,244,-1399,900)
MOVE(-364,-165,1019,-840,x,-25,-182,14,102,-274,-113,-365,52,121,148,-242,34,51,-120,x,-43,-510,244,-1399,600)
MOVE(-296,-120,926,-754,x,-176,-260,277,161,-459,-171,-740,182,156,291,-632,-348,62,-123,x,695,-820,172,-703,900)
MOVE(-296,-120,926,-754,x,-176,-260,277,161,-459,-171,-740,182,156,291,-632,-348,62,-123,x,695,-820,172,-703,600)
;
MOVE(-559,-178,1008,-599,x,-17,-106,369,370,-356,-120,-401,382,87,351,-409,-384,94,-145,x,594,-1008,79,-525,600)
;ファイトホーム
MOVE(-450,-226,931,-946,x,-120,-80,383,331,-329,-90,-331,331,70,329,-331,-383,80,0,x,978,-928,180,450,900)
MOVE(-450,-226,931,-946,x,-120,-80,383,331,-329,-90,-331,331,70,329,-331,-383,80,0,x,978,-928,180,450,240)
V109 = 103
JUMP(EXIT2)
;└───────────────────────────────────┘
;
;┌──＜【EXIT】サーボをオンにしてメインに戻る＞────────────┐
:EXIT
CALL(GYROON)
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,600)
CALL(FIGHTHOME)
V00=1
V01=1
V02=1
V03=1
V04=1
V05=1
V06=1
V07=1
V08=1
V09=1
V10=1
V11=1
V12=1
V13=1
V14=1
V15=1
V16=1
V17=1
V18=1
V19=1
V20=1
V21=1
V22=1
V23=1
POWER
(50)
V109 = 102
:EXIT2
V00=5
FJUMP
;└───────────────────────────────────┘
;
;┏━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━┓
;┃自動起き上がり用サブルーチン　　        　　　　　　　　　　　　　　　┃
;┗━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━┛
;
;
;┌──＜【FIGHTHOME】ファイティングホーム ＞──────────────┐
:FIGHTHOME
;FtgHome1/2
MOVE(0,100,200,0,0,-120,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-300,-100,0,240)
;FtgHome2/2
MOVE(-450,-226,931,-946,x,-120,-80,383,331,-329,-90,-331,331,70,329,-331,-383,80,0,x,978,-928,180,450,240)
V109 = 103;体勢チェックをファイティングホーム状態(3)にセット。
RETURN
;
;┌──＜【POWERDOWN】サーボをオフにする ＞───────────────┐
:POWERDOWN
V51=0
:POWERDOWNLOOP
(V51=100
V51=V51+1
JUMPIF(V51,<,24,POWERDOWNLOOP)
V00=30
V23=30
TORQUESET
(50)
RETURN
;└───────────────────────────────────┘
;
;┌──＜【POWERON】サーボをオンにする ＞────────────────┐
:POWERON
V51=0
:POWERONLOOP
(V51=100
V51=V51+1
JUMPIF(V51,<,24,POWERONLOOP)
TORQUESET
RETURN
;└───────────────────────────────────┘
;
;┏━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━┓
;┃ジャイロ設定　　　　　　　　　　　　　　　　　　　　　　　　　　　　　┃
;┗━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━┛
;ジャイロ中央値として、V48（ピッチ軸）、V49（ロール軸）の変数を使用する。
;
;┌──＜【GYROON】ジャイロオン（弱）足首のみ　─────────────┐
:GYROON
(N389=0
(N393=73
(N394=73
;
(N396=0
(N398=0
(N397=7
(N399=0-7
;
(N401=0
(N403=0
(N402=0-8
(N404=0-8
;
(N389=1
RETURN
;└───────────────────────────────────┘
;
;┌──＜【GYROON2】ジャイロオン（強）足首＆股関節 ───────────┐
:GYROON2
(N389=0
(N393=73
(N394=73
;
(N396=2
(N398=0-2
(N397=8
(N399=0-8
;
(N401=0-3
(N403=0-3
(N402=0-7
(N404=0-7
;
(N389=1
RETURN
;└───────────────────────────────────┘
;
;┌──＜【GYROOFF_CALIB】ジャイロオフ ─────────────────┐
:GYROOFF_CALIB
(N389=0
(N393=0
(N394=0
(N396=0
(N398=0
(N397=0
(N399=0
(N401=0
(N403=0
(N402=0
(N404=0
(N389=0
RETURN
;└───────────────────────────────────┘
;
;【OKBEEP】OK音を鳴らす
:OKBEEP
V00=62
V01=192
V02=0
V03=20
V04=0
SOUND
WAIT(100)
RETURN
;
;【ERRBEEP】エラー音を鳴らす
:ERRBEEP
V00=62
V01=213
V02=0
V03=20
V04=0
SOUND
RETURN
;
;【THINKBEEP】考え中音を鳴らす
:THINKBEEP
V00=62
V01=0
V02=0
V03=21
V04=0
SOUND
WAIT(100)
RETURN
;【CALLBEEP】入力まち音
:CALLBEEP
V00=62
V01=77
V02=0
V03=20
V04=0
SOUND
WAIT(100)
RETURN