version 2.05 platform CM-530 o // param_text:Title: Cannon Shooter [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 the remote control for direction control and shooting - o main o begin o // param_text:Modify L/R wheel speed until robot run straightly in "Straightness_check_mode" (0 ~ 1023) o load param_dest:var:l_wheel_fast_speed param_src:dec_num:600 o load param_dest:var:r_wheel_fast_speed param_src:dec_num:600 o load param_dest:var:l_wheel_low_speed param_src:dec_num:400 o load param_dest:var:r_wheel_low_speed param_src:dec_num:400 - 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:ID and mode check o call faddr_dest:ID_mode_check - o // param_text:Press START button then press 'U' button until you hear melody to enter "Straightness check mode" o if param_src:cm:32 lop:== param_src:button_num:8 rop:then o jump laddr_dest:Linearity_check_mode - o label laddr_src:Start o call faddr_dest:initialize o while(1) o begin o load param_dest:cm:33 param_src:var:halt_time o while param_src:cm:30 lop:== param_src:bool_num:0 rop:then o begin o if param_src:cm:33 lop:== param_src:dec_num:0 rop:then o call faddr_dest:stop o end - 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:dec_num:0 rop:then o begin o if param_src:cm:28 lop:== param_src:rc100z_num:1 rop:then o call faddr_dest:fast_forward o elseif param_src:cm:28 lop:== param_src:rc100z_num:2 rop:then o call faddr_dest:fast_reverse o elseif param_src:cm:28 lop:== param_src:rc100z_num:4 rop:then o call faddr_dest:fast_pivot_left o elseif param_src:cm:28 lop:== param_src:rc100z_num:8 rop:then o call faddr_dest:fast_pivot_right - o elseif param_src:cm:28 lop:== param_src:rc100z_num:17 rop:then o call faddr_dest:slow_forward o elseif param_src:cm:28 lop:== param_src:rc100z_num:18 rop:then o call faddr_dest:slow_reverse o elseif param_src:cm:28 lop:== param_src:rc100z_num:20 rop:then o call faddr_dest:slow_pivot_left o elseif param_src:cm:28 lop:== param_src:rc100z_num:24 rop:then o call faddr_dest:slow_pivot_right - o elseif param_src:cm:28 lop:== param_src:rc100z_num:5 rop:then o call faddr_dest:turn_left o elseif param_src:cm:28 lop:== param_src:rc100z_num:9 rop:then o call faddr_dest:turn_right o elseif param_src:cm:28 lop:== param_src:rc100z_num:6 rop:then o call faddr_dest:reverse_left o elseif param_src:cm:28 lop:== param_src:rc100z_num:10 rop:then o call faddr_dest:reverse_right o end - o else o call faddr_dest:stop - o if param_src:cm:28 lop:== param_src:rc100z_num:16 rop:then o call faddr_dest:lift_clamp o elseif param_src:cm:28 lop:== param_src:rc100z_num:32 rop:then o call faddr_dest:retract_clamp o elseif param_src:cm:28 lop:== param_src:rc100z_num:64 rop:then o call faddr_dest:lower_clamp o elseif param_src:cm:28 lop:== param_src:rc100z_num:128 rop:then o call faddr_dest:place_clamp 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 label laddr_src:Linearity_check_mode o call faddr_dest:buzzer_straight_mode o load param_dest:cm:33 param_src:timer_num:5 o wait param_src:cm:33 lop:> param_src:timer_num:0 rop:then o while(1) o begin o compute param_dest:motor:1:32 param_src:dir_num:0 aop:+ param_src:var:l_wheel_fast_speed o compute param_dest:motor:2:32 param_src:dir_num:1024 aop:+ param_src:var:r_wheel_fast_speed o end o end - o function faddr_src:initialize o begin o load param_dest:motor:3:32 param_src:dir_num:100 o load param_dest:motor:4:32 param_src:dir_num:100 o load param_dest:motor:5:32 param_src:dir_num:100 - o load param_dest:motor:3:30 param_src:position_num:512 o load param_dest:motor:4:30 param_src:position_num:512 o load param_dest:motor:5:30 param_src:position_num:512 - o wait param_src:motor:3:46 lop:== param_src:bool_num:1 rop:|| param_src:motor:4:46 lop:== param_src:bool_num:1 rop:|| param_src:motor:5:46 lop:== param_src:bool_num:1 rop:then - o load param_dest:var:halt_time param_src:timer_num:5 o end - o function faddr_src:fast_forward o begin o compute param_dest:motor:1:32 param_src:dir_num:0 aop:+ param_src:var:l_wheel_fast_speed o compute param_dest:motor:2:32 param_src:dir_num:1024 aop:+ param_src:var:r_wheel_fast_speed o end - o function faddr_src:fast_reverse o begin o compute param_dest:motor:1:32 param_src:dir_num:1024 aop:+ param_src:var:l_wheel_fast_speed o compute param_dest:motor:2:32 param_src:dir_num:0 aop:+ param_src:var:r_wheel_fast_speed o end - o function faddr_src:fast_pivot_left o begin o compute param_dest:motor:1:32 param_src:dir_num:1024 aop:+ param_src:var:l_wheel_fast_speed o compute param_dest:motor:2:32 param_src:dir_num:1024 aop:+ param_src:var:r_wheel_fast_speed o end - o function faddr_src:fast_pivot_right o begin o compute param_dest:motor:1:32 param_src:dir_num:0 aop:+ param_src:var:l_wheel_fast_speed o compute param_dest:motor:2:32 param_src:dir_num:0 aop:+ param_src:var:r_wheel_fast_speed o end - o function faddr_src:slow_forward o begin o compute param_dest:motor:1:32 param_src:dir_num:0 aop:+ param_src:var:l_wheel_low_speed o compute param_dest:motor:2:32 param_src:dir_num:1024 aop:+ param_src:var:r_wheel_low_speed o end - o function faddr_src:slow_reverse o begin o compute param_dest:motor:1:32 param_src:dir_num:1024 aop:+ param_src:var:l_wheel_low_speed o compute param_dest:motor:2:32 param_src:dir_num:0 aop:+ param_src:var:r_wheel_low_speed o end - o function faddr_src:slow_pivot_left o begin o compute param_dest:motor:1:32 param_src:dir_num:1024 aop:+ param_src:var:l_wheel_low_speed o compute param_dest:motor:2:32 param_src:dir_num:1024 aop:+ param_src:var:r_wheel_low_speed o end - o function faddr_src:slow_pivot_right o begin o compute param_dest:motor:1:32 param_src:dir_num:0 aop:+ param_src:var:l_wheel_low_speed o compute param_dest:motor:2:32 param_src:dir_num:0 aop:+ param_src:var:r_wheel_low_speed o end - o function faddr_src:turn_left o begin o compute param_dest:motor:1:32 param_src:dir_num:0 aop:+ param_src:dec_num:0 o compute param_dest:motor:2:32 param_src:dir_num:1024 aop:+ param_src:var:r_wheel_fast_speed o end - o function faddr_src:turn_right o begin o compute param_dest:motor:1:32 param_src:dir_num:0 aop:+ param_src:var:l_wheel_fast_speed o compute param_dest:motor:2:32 param_src:dir_num:1024 aop:+ param_src:dec_num:0 o end - o function faddr_src:reverse_left o begin o compute param_dest:motor:1:32 param_src:dir_num:1024 aop:+ param_src:dec_num:0 o compute param_dest:motor:2:32 param_src:dir_num:0 aop:+ param_src:var:r_wheel_fast_speed o end - o function faddr_src:reverse_right o begin o compute param_dest:motor:1:32 param_src:dir_num:1024 aop:+ param_src:var:l_wheel_fast_speed o compute param_dest:motor:2:32 param_src:dir_num:0 aop:+ param_src:dec_num:0 o end - o function faddr_src:stop o begin o load param_dest:motor:1:32 param_src:dir_num:0 o load param_dest:motor:2:32 param_src:dir_num:0 o end - - o function faddr_src:lift_clamp o begin o load param_dest:motor:3:32 param_src:dir_num:100 - o if param_src:motor:3:36 lop:< param_src:dec_num:889 rop:then o begin o load param_dest:motor:3:30 param_src:position_num:890 o wait param_src:cm:28 lop:== param_src:rc100z_num:16 rop:then o call faddr_dest:stop_clamp o end o end - o function faddr_src:retract_clamp o begin o load param_dest:motor:4:32 param_src:dir_num:100 o load param_dest:motor:5:32 param_src:dir_num:100 - o if param_src:motor:4:36 lop:> param_src:position_num:96 rop:|| param_src:motor:5:36 lop:< param_src:position_num:919 rop:then o begin o load param_dest:motor:4:30 param_src:position_num:95 o load param_dest:motor:5:30 param_src:position_num:920 o wait param_src:cm:28 lop:== param_src:rc100z_num:32 rop:then o call faddr_dest:stop_clamp o end o end - o function faddr_src:place_clamp o begin o load param_dest:motor:4:32 param_src:dir_num:100 o load param_dest:motor:5:32 param_src:dir_num:100 - o if param_src:motor:4:36 lop:< param_src:position_num:511 rop:|| param_src:motor:5:36 lop:> param_src:position_num:513 rop:then o begin o load param_dest:motor:4:30 param_src:position_num:512 o load param_dest:motor:5:30 param_src:position_num:512 o wait param_src:cm:28 lop:== param_src:rc100z_num:128 rop:then o call faddr_dest:stop_clamp o end o end - o function faddr_src:lower_clamp o begin o load param_dest:motor:3:32 param_src:dir_num:100 - o if param_src:motor:3:36 lop:> param_src:position_num:461 rop:then o begin o load param_dest:motor:3:30 param_src:position_num:460 o wait param_src:cm:28 lop:== param_src:rc100z_num:64 rop:then o call faddr_dest:stop_clamp o end o end - o function faddr_src:stop_clamp o begin o load param_dest:motor:3:30 param_src:motor:3:36 o load param_dest:motor:4:30 param_src:motor:4:36 o load param_dest:motor:5:30 param_src:motor:5:36 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:1 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:custom:b:ID:3 lop:!= param_src:var:ID rop:&& param_src:var:ID lop:> param_src:dec_num:0 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:&& param_src:var:ID lop:> param_src:dec_num:0 rop:then o call faddr_dest:buzzer_torque_limit_error 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 select after turning LED off 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:var:ID_TEMP lop:< param_src:dec_num:3 rop:then o begin o if param_src:custom:w:ID_TEMP:8 lop:!= param_src:dec_num:0 rop:then o load param_dest:custom:w:ID_TEMP:8 param_src:dec_num:0 o end - o else o begin 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 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:********************************************** Linearity check function **********************************************// o function faddr_src:buzzer_straight_mode o begin o load param_dest:cm:55 param_src:buzzertime_num:5 o load param_dest:cm:54 param_src:scale_num:36 o wait param_src:cm:55 lop:> param_src:buzzertime_num:0 rop:then 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:1 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 if param_src:var:ID lop:< param_src:dec_num:3 rop:then o begin o // param_text:Dynamixel wheel mode 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:0 o end - o else o begin 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 o end "9