version 2.05 platform CM-530 o // param_text:Title: Walking Droid [Basic phase] o // param_text:Initial release: 2012-08-03 o // param_text:Last modified: 2012-08-03 o // param_text:[Operations] o // param_text:use direction buttons of the remote controler to control robot - o main o begin o // param_text:Press START button then press 'D' button until you hear melody to enter "Assembly check mode" o if param_src:cm:32 lop:== param_src:button_num:4 rop:then o jump laddr_dest:Assembly_check_mode - o // param_text:check Dynamixel's ID and mode o call faddr_dest:ID_mode_check - o label laddr_src:Start o call faddr_dest:Start - o while(1) o begin o wait param_src:cm:28 lop:== param_src:rc100z_num:0 rop:then - o compute param_dest:var:direction_control param_src:cm:28 aop:& param_src:rc100z_num:15 - o if param_src:var:direction_control lop:== param_src:rc100z_num:1 rop:then o call faddr_dest:Assembly_check_mode o elseif param_src:var:direction_control lop:== param_src:rc100z_num:2 rop:then o call faddr_dest:reverse o elseif param_src:var:direction_control lop:== param_src:rc100z_num:4 rop:then o call faddr_dest:pivot_left o elseif param_src:var:direction_control lop:== param_src:rc100z_num:8 rop:then o call faddr_dest:pivot_right o end - o label laddr_src:Assembly_check_mode o call faddr_dest:init_assembly_check - o while(1) o begin o call faddr_dest:assembly_check o if param_src:var:assembly_check lop:== param_src:bool_num:1 rop:then o jump laddr_dest:Start o end o end - o function faddr_src:Start o begin o load param_dest:motion:24 param_src:dec_num:1 o call faddr_dest:motion_standby o end - o function faddr_src:Assembly_check_mode o begin o load param_dest:motion:24 param_src:dec_num:2 o call faddr_dest:motion_standby o load param_dest:motion:24 param_src:dec_num:3 o call faddr_dest:motion_standby o end - o function faddr_src:reverse o begin o load param_dest:motion:24 param_src:dec_num:6 o call faddr_dest:motion_standby o end - o function faddr_src:pivot_left o begin o load param_dest:motion:24 param_src:dec_num:4 o call faddr_dest:motion_standby o end - o function faddr_src:pivot_right o begin o load param_dest:motion:24 param_src:dec_num:5 o call faddr_dest:motion_standby o end - o function faddr_src:motion_standby o begin o wait param_src:motion:25 lop:== param_src:bool_num:1 rop:then o end - o function faddr_src:timer_standby o begin o wait param_src:cm:33 lop:> param_src:timer_num:0 rop:then o end - o // param_text:********************************************** assembly check function **********************************************// - o function faddr_src:init_assembly_check o begin o load param_dest:var:ID param_src:dec_num:0 o load param_dest:var:ID_MIN param_src:dec_num:3 o load param_dest:var:ID_MAX param_src:dec_num:6 o load param_dest:var:assembly_check 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:1 - o load param_dest:motor:254:25 param_src:bool_num:0 o load param_dest:cm:33 param_src:timer_num:8 o call faddr_dest:timer_standby o end - o function faddr_src:assembly_check o begin o if param_src:cm:32 lop:== param_src:button_num:8 rop:then o call faddr_dest:ID_up - o elseif param_src:cm:32 lop:== param_src:button_num:4 rop:then o call faddr_dest:ID_down - o elseif param_src:cm:32 lop:== param_src:button_num:16 rop:then o begin o call faddr_dest:motor_init o load param_dest:var:assembly_check param_src:bool_num:1 o end - o if param_src:var:ID lop:> param_src:dec_num:2 rop:then o begin o if param_src:custom:b:ID:3 lop:!= param_src:var:ID rop:then o call faddr_dest:buzzer_motor_comm_fail - o elseif param_src:custom:w:ID:34 lop:== param_src:dec_num:0 rop:then o call faddr_dest:buzzer_torque_limit_error o end - o call faddr_dest:IR_sensor_port_check o end - o function faddr_src:ID_up o begin o load param_dest:motor:254:25 param_src:bool_num:0 - o if param_src:var:ID lop:< param_src:var:ID_MAX rop:then o begin o compute param_dest:var:ID param_src:var:ID aop:+ param_src:dec_num:1 o load param_dest:custom:b:ID:25 param_src:dec_num:1 o end - o else o load param_dest:custom:b:ID:25 param_src:dec_num:1 - 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:timer_standby o end - o function faddr_src:ID_down o begin o load param_dest:motor:254:25 param_src:bool_num:0 - o if param_src:var:ID lop:> param_src:var:ID_MIN rop:then o begin o compute param_dest:var:ID param_src:var:ID aop:- param_src:dec_num:1 o load param_dest:custom:b:ID:25 param_src:dec_num:1 o end - o else o load param_dest:custom:b:ID:25 param_src:dec_num:1 - 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:timer_standby o end - o function faddr_src:buzzer_motor_comm_fail 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 wait param_src:cm:55 lop:> param_src:buzzertime_num:0 rop:&& param_src:cm:32 lop:== param_src:button_num:0 rop:then o end - o function faddr_src:buzzer_torque_limit_error 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 wait param_src:cm:55 lop:> param_src:buzzertime_num:0 rop:&& param_src:cm:32 lop:== param_src:button_num:0 rop:then o end - o function faddr_src:motor_init o begin o load param_dest:cm:31 param_src:bool_num:0 - o // param_text:mode setting after turning off LED o for param_var:ID_TEMP param_src:var:ID_MIN param_src:var:ID_MAX o begin o load param_dest:custom:b:ID_TEMP:25 param_src:bool_num:0 - o if param_src:custom:w:ID_TEMP:8 lop:!= param_src:dec_num:1023 rop:then o load param_dest:custom:w:ID_TEMP:8 param_src:dec_num:1023 o end o end - o function faddr_src:IR_sensor_port_check o begin o if param_src:aux_ir:1 lop:> param_src:dec_num:100 rop:then o call faddr_dest:LED_port_number - o elseif param_src:aux_ir:2 lop:> param_src:dec_num:100 rop:then o begin o for param_var:iterations param_src:dec_num:1 param_src:dec_num:2 o call faddr_dest:LED_port_number o end - o elseif param_src:aux_ir:3 lop:> param_src:dec_num:100 rop:then o begin o for param_var:iterations param_src:dec_num:1 param_src:dec_num:3 o call faddr_dest:LED_port_number o end - o elseif param_src:aux_ir:4 lop:> param_src:dec_num:100 rop:then o begin o for param_var:iterations param_src:dec_num:1 param_src:dec_num:4 o call faddr_dest:LED_port_number o end - o elseif param_src:aux_ir:5 lop:> param_src:dec_num:100 rop:then o begin o for param_var:iterations param_src:dec_num:1 param_src:dec_num:5 o call faddr_dest:LED_port_number o end - o elseif param_src:aux_ir:6 lop:> param_src:dec_num:100 rop:then o begin o for param_var:iterations param_src:dec_num:1 param_src:dec_num:6 o call faddr_dest:LED_port_number o end o end - o function faddr_src:LED_port_number o begin o if param_src:cm:32 lop:== param_src:button_num:0 rop:then o begin o load param_dest:cm:31 param_src:bool_num:1 o load param_dest:cm:33 param_src:timer_num:2 o call faddr_dest:timer_standby o end o if param_src:cm:32 lop:== param_src:button_num:0 rop:then o begin o load param_dest:cm:31 param_src:bool_num:0 o load param_dest:cm:33 param_src:timer_num:2 o call faddr_dest:timer_standby o end o end - o function faddr_src:assembly_error o begin o load param_dest:motor:254:32 param_src:dir_num:0 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 - o // param_text:********************************************** motor check function **********************************************// o function faddr_src:ID_mode_check o begin o for param_var:ID param_src:dec_num:3 param_src:dec_num:6 o begin o // param_text:Dynamixel ID check o if param_src:custom:b:ID:3 lop:!= param_src:var:ID rop:then o call faddr_dest:assembly_error - o // param_text:Dynamixel joint mode check o if param_src:custom:w:ID:8 lop:!= param_src:dec_num:1023 rop:then o load param_dest:custom:w:ID:8 param_src:dec_num:1023 o end o end Ҭ