version 2.05 platform CM-530 o // param_text:Title: Beetle Gripper [Appications 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 controller's directional buttons to control robot direction and numbered buttons for clamp control o // param_text:U - forward, D - reverse, L - pivot left, R - pivot right o // param_text:1 - lift clamp, 2 - retract clamp, 3 - lower clamp, 4 - place clamp - 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_speed param_src:dec_num:200 o load param_dest:var:r_wheel_speed param_src:dec_num:200 - o // param_text:Hold the 'D' button and press START to begin 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:Hold the 'U' button and press START to begin Straight mode check 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 compute param_dest:var:clamp_control param_src:cm:28 aop:& param_src:rc100z_num:240 - o if param_src:var:direction_control lop:!= param_src:rc100z_num:0 rop:then o begin o if param_src:var:direction_control lop:== param_src:rc100z_num:1 rop:then o call faddr_dest:forward 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 elseif param_src:var:clamp_control lop:!= param_src:rc100z_num:0 rop:then o begin o if param_src:var:clamp_control lop:== param_src:rc100z_num:16 rop:then o call faddr_dest:raise_clamp o elseif param_src:var:clamp_control lop:== param_src:rc100z_num:32 rop:then o call faddr_dest:retract_clamp o elseif param_src:var:clamp_control lop:== param_src:rc100z_num:64 rop:then o call faddr_dest:lower_clamp o elseif param_src:var:clamp_control lop:== param_src:rc100z_num:128 rop:then o call faddr_dest:place_clamp o end - o else o call faddr_dest:stop 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_speed o compute param_dest:motor:2:32 param_src:dir_num:1024 aop:+ param_src:var:r_wheel_speed o end o end - o function faddr_src:initialize o begin o // param_text:initializing motor position for clamp o load param_dest:motor:3:32 param_src:dir_num:200 o load param_dest:motor:4:32 param_src:dir_num:120 o load param_dest:motor:5:32 param_src:dir_num:120 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 load param_dest:var:toggle_pos param_src:dec_num:0 o end - o function faddr_src:forward o begin o compute param_dest:motor:1:32 param_src:dir_num:0 aop:+ param_src:var:l_wheel_speed o compute param_dest:motor:2:32 param_src:dir_num:1024 aop:+ param_src:var:r_wheel_speed o end - o function faddr_src:reverse o begin o compute param_dest:motor:1:32 param_src:dir_num:1024 aop:+ param_src:var:l_wheel_speed o compute param_dest:motor:2:32 param_src:dir_num:0 aop:+ param_src:var:r_wheel_speed o end - o function faddr_src:pivot_left o begin o compute param_dest:motor:1:32 param_src:dir_num:1024 aop:+ param_src:var:l_wheel_speed o compute param_dest:motor:2:32 param_src:dir_num:1024 aop:+ param_src:var:r_wheel_speed o end - o function faddr_src:pivot_right o begin o compute param_dest:motor:1:32 param_src:dir_num:0 aop:+ param_src:var:l_wheel_speed o compute param_dest:motor:2:32 param_src:dir_num:0 aop:+ param_src:var:r_wheel_speed 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:raise_clamp o begin o // param_text:clamp's initial position half lift o if param_src:var:toggle_pos lop:== param_src:dec_num:0 rop:then o begin o load param_dest:motor:3:30 param_src:position_num:672 o load param_dest:var:toggle_pos param_src:dec_num:1 o end o // param_text:from half to full o elseif param_src:var:toggle_pos lop:== param_src:dec_num:1 rop:then o begin o load param_dest:motor:3:30 param_src:position_num:912 o load param_dest:var:toggle_pos param_src:dec_num:2 o end o wait param_src:motor:3:46 lop:== param_src:bool_num:1 rop:then o end - o function faddr_src:retract_clamp o begin o if param_src:motor:4:36 lop:> param_src:position_num:513 rop:|| param_src:motor:5:36 lop:< param_src:position_num:511 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:32 rop:then o call faddr_dest:stop_clamp o end o end - o function faddr_src:place_clamp o begin o if param_src:motor:4:36 lop:< param_src:position_num:651 rop:|| param_src:motor:5:36 lop:> param_src:position_num:373 rop:then o begin o load param_dest:motor:4:30 param_src:position_num:652 o load param_dest:motor:5:30 param_src:position_num:372 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 // param_text:returning clamp to initial position o if param_src:var:toggle_pos lop:> param_src:dec_num:0 rop:then o begin o load param_dest:motor:3:30 param_src:position_num:512 o wait param_src:motor:3:46 lop:== param_src:bool_num:1 rop:then o load param_dest:var:toggle_pos param_src:dec_num:0 o end o end - o // param_text:clamp halt 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:5 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:5 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 %D