version 2.04 platform bioloid2 o // param_text:Bioloid Premium Kit Puppy Task code o // param_text:Initial Release : October 1. 2009. o // param_text:Revised : Assembly check added (Se[tebmer 28, 2010). o // param_text:How the program works o // param_text:The puppy avoids obstacles. o // param_text:The puppy is cherished when you touch its snouth. o // param_text:The puppy sleeps when idle. o main o begin o if param_src:cm:32 lop:== param_src:button_num:4 rop:then o jump laddr_dest:AssemblyCheckMode o if param_src:custom:b:1:3 lop:!= param_src:dec_num:1 rop:then o call faddr_dest:AssemblyError o // param_text:Dynamixel ID check o if param_src:custom:w:1:8 lop:== param_src:dec_num:0 rop:then o load param_dest:custom:w:1:8 param_src:dec_num:1023 o for param_var:ID param_src:dec_num:4 param_src:dec_num:17 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:Stop o label laddr_src:Sleep o call faddr_dest:Stop o load param_dest:motion:24 param_src:dec_num:10 o call faddr_dest:CompleteMotion o load param_dest:cm:56 param_src:dec_num:0 o label laddr_src:SleepAgain o if param_src:cm:56 lop:>= param_src:dec_num:1 rop:then o jump laddr_dest:Awake o jump laddr_dest:SleepAgain - o label laddr_src:Awake o load param_dest:cm:33 param_src:timer_num:100 o load param_dest:motion:24 param_src:dec_num:11 o call faddr_dest:CompleteMotion o load param_dest:motion:24 param_src:dec_num:24 o call faddr_dest:CompleteMotion o load param_dest:cm:56 param_src:dec_num:0 o label laddr_src:Order1 o if param_src:cm:56 lop:>= param_src:dec_num:1 rop:then o jump laddr_dest:ReadyWalk o elseif param_src:cm:33 lop:<= param_src:dec_num:0 rop:then o begin o load param_dest:motion:24 param_src:dec_num:25 o call faddr_dest:CompleteMotion o jump laddr_dest:Sleep o end o jump laddr_dest:Order1 o label laddr_src:ReadyWalk o call faddr_dest:Stop o load param_dest:motion:24 param_src:dec_num:7 o // param_text:complete motion o call faddr_dest:CompleteMotion o load param_dest:motion:24 param_src:dec_num:13 o // param_text:complete motion o call faddr_dest:CompleteMotion o load param_dest:motion:24 param_src:dec_num:27 o // param_text:complete motion o call faddr_dest:CompleteMotion o load param_dest:cm:56 param_src:dec_num:0 o load param_dest:cm:33 param_src:timer_num:100 o label laddr_src:Order2 o if param_src:cm:56 lop:>= param_src:dec_num:1 rop:then o jump laddr_dest:Walk o elseif param_src:aux_ir:1 lop:>= param_src:dec_num:10 rop:then o jump laddr_dest:Cherish o elseif param_src:cm:33 lop:<= param_src:dec_num:0 rop:then o begin o jump laddr_dest:RestAfterWalk o end o jump laddr_dest:Order2 o label laddr_src:Walk o load param_dest:cm:33 param_src:timer_num:50 o load param_dest:motion:24 param_src:dec_num:35 o label laddr_src:ForwardAgain o if param_src:cm:33 lop:<= param_src:dec_num:0 rop:then o jump laddr_dest:RestAfterWalk o elseif param_src:aux_ir:1 lop:>= param_src:dec_num:10 rop:then o jump laddr_dest:AvoidObstacle o jump laddr_dest:ForwardAgain - o label laddr_src:AvoidObstacle o call faddr_dest:Stop o load param_dest:motion:24 param_src:dec_num:37 o call faddr_dest:CompleteMotion o jump laddr_dest:Walk - o label laddr_src:RestAfterWalk o call faddr_dest:Stop o load param_dest:motion:24 param_src:dec_num:90 o call faddr_dest:CompleteMotion o load param_dest:cm:33 param_src:timer_num:100 o load param_dest:cm:56 param_src:dec_num:0 o jump laddr_dest:Order1 - o label laddr_src:Cherish o load param_dest:cm:56 param_src:dec_num:0 o load param_dest:cm:33 param_src:timer_num:80 o while(1) o begin o if param_src:aux_ir:1 lop:>= param_src:dec_num:100 rop:then o begin o call faddr_dest:Cheer1 o load param_dest:cm:56 param_src:dec_num:0 o load param_dest:cm:33 param_src:timer_num:80 o end o elseif param_src:cm:56 lop:== param_src:dec_num:1 rop:then o begin o call faddr_dest:Cheer2 o load param_dest:motion:24 param_src:dec_num:81 o call faddr_dest:CompleteMotion o load param_dest:cm:56 param_src:dec_num:0 o load param_dest:cm:33 param_src:timer_num:80 o end o elseif param_src:cm:56 lop:== param_src:dec_num:2 rop:then o begin o call faddr_dest:Cheer3 o load param_dest:motion:24 param_src:dec_num:81 o call faddr_dest:CompleteMotion o load param_dest:cm:56 param_src:dec_num:0 o load param_dest:cm:33 param_src:timer_num:80 o end o elseif param_src:cm:56 lop:> param_src:dec_num:2 rop:then o begin o call faddr_dest:Cheer4 o load param_dest:motion:24 param_src:dec_num:81 o call faddr_dest:CompleteMotion o load param_dest:cm:56 param_src:dec_num:0 o load param_dest:cm:33 param_src:timer_num:80 o end o elseif param_src:cm:33 lop:<= param_src:dec_num:0 rop:then o begin o jump laddr_dest:RestAfterWalk 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: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: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:3 o end o elseif param_src:var:ID lop:< param_src:dec_num:17 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:4 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:3 o end o elseif param_src:var:ID lop:> param_src:dec_num:4 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: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:17:30 param_src:dec_num:612 o else o load param_dest:custom:w:17: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:Cheer1 o begin o load param_dest:motion:24 param_src:dec_num:80 o call faddr_dest:CompleteMotion o end - o function faddr_src:Cheer2 o begin o load param_dest:motion:24 param_src:dec_num:70 o call faddr_dest:CompleteMotion o load param_dest:motion:24 param_src:dec_num:15 o call faddr_dest:CompleteMotion o end - o function faddr_src:Cheer3 o begin o load param_dest:motion:24 param_src:dec_num:60 o call faddr_dest:CompleteMotion o end - o function faddr_src:Cheer4 o begin o load param_dest:motion:24 param_src:dec_num:74 o call faddr_dest:CompleteMotion o load param_dest:motion:24 param_src:dec_num:76 o call faddr_dest:CompleteMotion o load param_dest:motion:24 param_src:dec_num:75 o call faddr_dest:CompleteMotion o load param_dest:motion:24 param_src:dec_num:1 o call faddr_dest:CompleteMotion o end - o function faddr_src:Stop o begin o load param_dest:motion:24 param_src:dec_num:0 o call faddr_dest:CompleteMotion o end - o function faddr_src:CompleteMotion 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:17 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