version 2.04 platform bioloid2 o // param_text:Bioloid Premium Kit Quadruped Walking Robot 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:The robot avoids obstacles on its path. 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:8 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 call faddr_dest:Initial - o while(1) o begin o if param_src:aux_dms:1 lop:>= param_src:dec_num:250 rop:then o begin o call faddr_dest:Stop o call faddr_dest:GoRight o end o if param_src:aux_ir:2 lop:>= param_src:dec_num:40 rop:then o begin o call faddr_dest:Stop o call faddr_dest:Down o end o call faddr_dest:Forward 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:WaitTimerCompletion - 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:WaitTimerCompletion o if param_src:var:ID lop:< param_src:dec_num:8 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:WaitTimerCompletion 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:24 param_src:bool_num:0 o load param_dest:motor:254:25 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:2 lop:>= param_src:dec_num:50 rop:then o begin o load param_dest:custom:w:7:30 param_src:dec_num:412 o load param_dest:custom:w:8:30 param_src:dec_num:612 o end o else o begin o load param_dest:custom:w:7:30 param_src:dec_num:512 o load param_dest:custom:w:8:30 param_src:dec_num:512 o end o if param_src:aux_dms:1 lop:>= param_src:dec_num:300 rop:then o begin o load param_dest:custom:w:1:30 param_src:dec_num:412 o load param_dest:custom:w:2:30 param_src:dec_num:612 o end o else o begin o load param_dest:custom:w:1:30 param_src:dec_num:512 o load param_dest:custom:w:2:30 param_src:dec_num:512 o end 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:Stop o begin o load param_dest:motion:24 param_src:dec_num:0 o wait param_src:motion:25 lop:== param_src:bool_num:1 rop:then 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 end - o function faddr_src:Ready o begin o load param_dest:motion:24 param_src:dec_num:1 o call faddr_dest:WaitMotion o end - o function faddr_src:Forward o begin o load param_dest:motion:24 param_src:dec_num:2 o end - o function faddr_src:Reverse o begin o load param_dest:motion:24 param_src:dec_num:10 o end - o function faddr_src:GoRight o begin o load param_dest:motion:24 param_src:dec_num:5 o call faddr_dest:WaitMotion o load param_dest:motion:24 param_src:dec_num:6 - o end - o function faddr_src:GoLeft o begin o load param_dest:motion:24 param_src:dec_num:13 o end o function faddr_src:Down o begin o load param_dest:motion:24 param_src:dec_num:11 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:WaitTimerCompletion 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:8 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 À