version 2.05 platform CM-530 o // param_text:title: tumbler [applications phase] o // param_text:initial release: 12-12-2011 o // param_text:last modified: 2012.07.04 o // param_text:3rd modification: 2012.07.4 (modified the time and speed of reverse, forward and pivot left after encountering a obstacle) o // param_text:operation(s) o // param_text:robot moves forward. If it cannot go over the obstacle it turns away o // param_text:if the robot flips over it will continue motion in the same direction. - o main o begin o load param_dest:var:obstacle_threshold param_src:dec_num:60 - o // param_text:L/R wheel speed (0 ~ 1023) o // param_text:Modify speed until robot run straightly in "Straightness_check_mode" o load param_dest:var:l_wheel_speed param_src:dec_num:600 o load param_dest:var:r_wheel_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:time spent trying to overcome obstacle o load param_dest:var:obstacle_judging_time param_src:timer_num:24 - 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:check_ID_mode - 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:Straightness_check_mode - o label laddr_src:Start o call faddr_dest:initialize - o while(1) o begin o if param_src:aux_ir:4 lop:> param_src:var:obstacle_threshold rop:then o begin o if param_src:aux_ir:3 lop:<= param_src:var:obstacle_threshold rop:then o call faddr_dest:forward - o else o begin o call faddr_dest:forward o load param_dest:cm:33 param_src:var:obstacle_judging_time o while param_src:cm:33 lop:> param_src:timer_num:0 rop:then o begin o if param_src:aux_ir:3 lop:<= param_src:var:obstacle_threshold rop:then o break o end - o if param_src:aux_ir:3 lop:> param_src:var:obstacle_threshold rop:then o begin o call faddr_dest:stop o call faddr_dest:slow_reverse o load param_dest:cm:33 param_src:timer_num:13 o call faddr_dest:timer_standby o call faddr_dest:stop o load param_dest:cm:33 param_src:timer_num:1 o call faddr_dest:timer_standby o call faddr_dest:pivot_left o load param_dest:cm:33 param_src:timer_num:6 o call faddr_dest:timer_standby o end o end o end - o // param_text:flipping over o elseif param_src:aux_ir:4 lop:<= param_src:var:obstacle_threshold rop:then o begin o if param_src:aux_ir:3 lop:<= param_src:var:obstacle_threshold rop:then o begin o call faddr_dest:reverse o end - o else o begin o call faddr_dest:reverse o load param_dest:cm:33 param_src:var:obstacle_judging_time o while param_src:cm:33 lop:> param_src:timer_num:0 rop:then o begin o if param_src:aux_ir:3 lop:<= param_src:var:obstacle_threshold rop:then o break o end - o if param_src:aux_ir:3 lop:> param_src:var:obstacle_threshold rop:then o begin o call faddr_dest:stop o call faddr_dest:slow_forward o load param_dest:cm:33 param_src:timer_num:13 o call faddr_dest:timer_standby o call faddr_dest:stop o load param_dest:cm:33 param_src:timer_num:1 o call faddr_dest:timer_standby o call faddr_dest:pivot_left o load param_dest:cm:33 param_src:timer_num:14 o call faddr_dest:timer_standby o end o end o end o end - o label laddr_src:Assembly_check_mode o call faddr_dest:init_assembly_check_mode - 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:Straightness_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 load param_dest:var:prepare_time param_src:timer_num:15 o call faddr_dest:buzzer o load param_dest:cm:33 param_src:var:prepare_time o call faddr_dest:timer_standby 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: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: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: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: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: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:buzzer o begin o load param_dest:cm:55 param_src:buzzertime_num:15 o load param_dest:cm:54 param_src:scale_num:37 o wait param_src:cm:55 lop:> param_src:buzzertime_num:0 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:********************************************** functions for checking assembly **********************************************// - o function faddr_src:init_assembly_check_mode 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:2 o load param_dest:var:assembly_check param_src:bool_num:0 - o // param_text:assembly check melody 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:motor:1:25 param_src:bool_num:0 o load param_dest:motor:2:25 param_src:bool_num:0 o load param_dest:cm:31 param_src:bool_num:0 - o // param_text:set to wheel mode o for param_var:ID_TEMP param_src:dec_num:1 param_src:dec_num:2 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 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_num - 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_num 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_num 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_num 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_num 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_num o end o end - o function faddr_src:LED_port_num 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:********************************************** functions for straight mode **********************************************// - 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:********************************************** functions for checking dynamixel **********************************************// o function faddr_src:check_ID_mode o begin o for param_var:ID param_src:dec_num:1 param_src:dec_num:2 o begin o // param_text:check connection o if param_src:custom:b:ID:3 lop:!= param_src:var:ID rop:then o call faddr_dest:assembly_error - o // param_text:set to wheel mode 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 end 00