version 2.04
platform bioloid2
o // param_text:Bioloid Premium Kit Fawn Task code
o // param_text:Initial Release : July 9, 2010.
o // param_text:Revised : Assembly check added (September 28, 2010).
o // param_text:How the program works
o // param_text:When there's nothing happening the deer sits.
o // param_text:If the deer detects an object then it follows it.
o main
o begin
o if param_src:cm:32 lop:== param_src:button_num:4 rop:then
o jump laddr_dest:AssemblyCheckMode
o for param_var:ID param_src:dec_num:1 param_src:dec_num:7
o begin
o if param_src:custom:b:ID:3 lop:!= param_src:var:ID rop:then
o call faddr_dest:AssemblyError
o // param_text:Dynamixel ID check
o if param_src:custom:w:ID:8 lop:== param_src:dec_num:0 rop:then
o load param_dest:custom:w:ID:8 param_src:dec_num:1023
o // param_text:Dynamixel mode check (joint or wheel)
o end
o // param_text:Assembly check
o load param_dest:var:Toggle param_src:dec_num:0
o call faddr_dest:Initial
- 
o while(1)
o begin
o if param_src:aux_ir:1 lop:>= param_src:dec_num:10 rop:then
o call faddr_dest:Follow
- 
o if param_src:motor:1:46 lop:!= param_src:dec_num:1 rop:then
o begin
o if param_src:var:Toggle lop:== param_src:dec_num:0 rop:then
o begin
o // param_text:turn head left
o load param_dest:motor:1:32 param_src:dec_num:30
o load param_dest:motor:1:30 param_src:dec_num:712
o load param_dest:var:Toggle param_src:dec_num:1
o end
o elseif param_src:var:Toggle lop:== param_src:dec_num:1 rop:then
o begin
o // param_text:turn head right
o load param_dest:motor:1:32 param_src:dec_num:30
o load param_dest:motor:1:30 param_src:dec_num:312
o load param_dest:var:Toggle param_src:dec_num:0
o end
o end
o end
o // param_text:Assembly check
o label laddr_src:AssemblyCheckMode
o // param_text:Assembly check tune
o load param_dest:cm:55 param_src:buzzertime_num:255
o load param_dest:cm:54 param_src:melody_num:1
- 
o load param_dest:var:ID param_src:dec_num:1
o load param_dest:motor:254:32 param_src:dir_num:100
o load param_dest:motor:254:24 param_src:bool_num:0
o load param_dest:motor:254:25 param_src:bool_num:0
o load param_dest:cm:33 param_src:timer_num:12
o call faddr_dest:WaitTimerPosition
- 
o while(1)
o begin
o if param_src:cm:32 lop:== param_src:button_num:8 rop:then
o begin
o load param_dest:motor:254:24 param_src:bool_num:0
o load param_dest:cm:33 param_src:timer_num:5
o load param_dest:cm:55 param_src:buzzertime_num:5
o load param_dest:cm:54 param_src:scale_num:15
o call faddr_dest:WaitTimerPosition
o if param_src:var:ID lop:< param_src:dec_num:7 rop:then
o begin
o load param_dest:custom:b:ID:24 param_src:dec_num:0
o load param_dest:custom:b:ID:25 param_src:dec_num:0
o compute param_dest:var:ID param_src:var:ID aop:+ param_src:dec_num:1
o end
o end
o elseif param_src:cm:32 lop:== param_src:button_num:4 rop:then
o begin
o load param_dest:motor:254:24 param_src:bool_num:0
o load param_dest:cm:33 param_src:timer_num:5
o load param_dest:cm:55 param_src:buzzertime_num:5
o load param_dest:cm:54 param_src:scale_num:3
o call faddr_dest:WaitTimerPosition
o if param_src:var:ID lop:> param_src:dec_num:1 rop:then
o begin
o load param_dest:custom:b:ID:24 param_src:dec_num:0
o load param_dest:custom:b:ID:25 param_src:dec_num:0
o compute param_dest:var:ID param_src:var:ID aop:- param_src:dec_num:1
o end
o end
o elseif param_src:cm:32 lop:== param_src:button_num:1 rop:then
o jump laddr_dest:SensorCheckMode
o if param_src:custom:b:ID:3 lop:!= param_src:var:ID rop:then
o begin
o if param_src:cm:55 lop:== param_src:buzzertime_num:0 rop:then
o begin
o load param_dest:cm:55 param_src:buzzertime_num:255
o load param_dest:cm:54 param_src:melody_num:16
o end
o end
o elseif param_src:custom:w:ID:34 lop:== param_src:dec_num:0 rop:then
o begin
o if param_src:cm:55 lop:== param_src:buzzertime_num:0 rop:then
o begin
o load param_dest:cm:55 param_src:buzzertime_num:255
o load param_dest:cm:54 param_src:melody_num:18
o end
o end
o else
o begin
o load param_dest:custom:w:ID:30 param_src:dec_num:512
o load param_dest:custom:b:ID:25 param_src:dec_num:1
o end
o end
o label laddr_src:SensorCheckMode
o load param_dest:motor:254:32 param_src:dir_num:100
o load param_dest:motor:254:25 param_src:bool_num:0
o load param_dest:motor:254:24 param_src:bool_num:0
o load param_dest:cm:55 param_src:buzzertime_num:255
o load param_dest:cm:54 param_src:melody_num:10
o call faddr_dest:AssemblyCheckPosition
o while(1)
o begin
o if param_src:cm:32 lop:== param_src:button_num:2 rop:then
o jump laddr_dest:AssemblyCheckMode
o if param_src:aux_ir:1 lop:>= param_src:dec_num:50 rop:then
o load param_dest:custom:w:1:30 param_src:dec_num:412
o else
o load param_dest:custom:w:1:30 param_src:dec_num:512
o load param_dest:motor:254:32 param_src:dir_num:100
o end
o // param_text:Assembly check
o end
- 
o function faddr_src:Follow
o begin
o call faddr_dest:GetUp
o call faddr_dest:Forward
o while(1)
o begin
o if param_src:aux_ir:1 lop:>= param_src:dec_num:120 rop:then
o begin
o call faddr_dest:Forward
o end
o elseif param_src:aux_ir:1 lop:>= param_src:dec_num:15 rop:&& param_src:aux_ir:1 lop:< param_src:dec_num:120 rop:then
o call faddr_dest:FastForward
o else
o break
o end
o call faddr_dest:SitDown
o end
- 
- 
o function faddr_src:Initial
o begin
o load param_dest:cm:55 param_src:dec_num:255
o load param_dest:cm:54 param_src:dec_num:0
o call faddr_dest:Ready
o call faddr_dest:Forward
o call faddr_dest:Reverse
o call faddr_dest:SitDown
o end
- 
o function faddr_src:Ready
o begin
o load param_dest:motion:24 param_src:dec_num:2
o call faddr_dest:WaitMotion
o end
- 
o function faddr_src:Forward
o begin
o load param_dest:motion:24 param_src:dec_num:3
o call faddr_dest:WaitMotion
o end
- 
o function faddr_src:Reverse
o begin
o load param_dest:motion:24 param_src:dec_num:5
o call faddr_dest:WaitMotion
o end
- 
o function faddr_src:FastForward
o begin
o load param_dest:motion:24 param_src:dec_num:4
o call faddr_dest:WaitMotion
o end
- 
o function faddr_src:SitDown
o begin
o load param_dest:motion:24 param_src:dec_num:7
o call faddr_dest:WaitMotion
o end
- 
o function faddr_src:GetUp
o begin
o load param_dest:motion:24 param_src:dec_num:8
o call faddr_dest:WaitMotion
o end
- 
o function faddr_src:WaitMotion
o begin
o wait param_src:motion:25 lop:== param_src:bool_num:1 rop:then
o end
o // param_text:Assembly check
o function faddr_src:WaitTimerPosition
o begin
o wait param_src:cm:33 lop:> param_src:timer_num:0 rop:then
o end
- 
o function faddr_src:AssemblyCheckPosition
o begin
o for param_var:ID param_src:dec_num:1 param_src:dec_num:7
o begin
o load param_dest:custom:w:ID:30 param_src:dec_num:512
o end
o end
- 
o function faddr_src:AssemblyError
o begin
o load param_dest:motor:254:32 param_src:dir_num:100
o load param_dest:motor:254:24 param_src:bool_num:0
o load param_dest:motor:254:25 param_src:bool_num:0
o while(1)
o begin
o if param_src:cm:55 lop:== param_src:buzzertime_num:0 rop:then
o begin
o load param_dest:cm:55 param_src:buzzertime_num:255
o load param_dest:cm:54 param_src:melody_num:16
o end
o end
o end
`ö