version 2.05 platform CM-530 o // param_text:Title: Hexapod [Practical phase] o // param_text:Initial release: 2012-08-03 o // param_text:Last modified: 2012-08-03 o // param_text:[Operations] o // param_text:upper sensor triggered, but no fast-speed modetrigerred; move slow speed o // param_text:right sensor trigered pivot left o // param_text:left sensor triggered pivot right o // param_text:left and right sensors triggered; move in reverse then make 2 pivot left turns - 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:initialize - o call faddr_dest:standby o call faddr_dest:buzzer - o while(1) o begin o if param_src:aux_ir:5 lop:>= param_src:var:u_sensor_base_val rop:then o begin o if param_src:aux_ir:2 lop:>= param_src:var:r_sensor_base_val rop:&& param_src:aux_ir:1 lop:< param_src:var:l_sensor_base_val rop:then o call faddr_dest:fast_left_pivot - o elseif param_src:aux_ir:2 lop:< param_src:var:r_sensor_base_val rop:&& param_src:aux_ir:1 lop:>= param_src:var:l_sensor_base_val rop:then o call faddr_dest:fast_right_pivot - o elseif param_src:aux_ir:2 lop:>= param_src:var:r_sensor_base_val rop:&& param_src:aux_ir:1 lop:>= param_src:var:l_sensor_base_val rop:then o begin o call faddr_dest:fast_reverse o call faddr_dest:fast_left_pivot o call faddr_dest:fast_left_pivot o end - o else o call faddr_dest:fast_forward o end - o else o begin o if param_src:aux_ir:2 lop:>= param_src:var:r_sensor_base_val rop:&& param_src:aux_ir:1 lop:< param_src:var:l_sensor_base_val rop:then o call faddr_dest:pivot_left - o elseif param_src:aux_ir:2 lop:< param_src:var:r_sensor_base_val rop:&& param_src:aux_ir:1 lop:>= param_src:var:l_sensor_base_val rop:then o call faddr_dest:pivot_right - o elseif param_src:aux_ir:2 lop:>= param_src:var:r_sensor_base_val rop:&& param_src:aux_ir:1 lop:>= param_src:var:l_sensor_base_val rop:then o begin o call faddr_dest:reverse o call faddr_dest:pivot_left o call faddr_dest:pivot_left o end - o else o call faddr_dest:forward o end 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:standby o begin o load param_dest:cm:33 param_src:timer_num:8 o wait param_src:cm:33 lop:> param_src:timer_num:0 rop:then o end - o function faddr_src:initialize o begin o load param_dest:var:u_sensor_base_val param_src:dec_num:100 o load param_dest:var:l_sensor_base_val param_src:dec_num:100 o load param_dest:var:r_sensor_base_val param_src:dec_num:100 - o load param_dest:motion:24 param_src:dec_num:1 o call faddr_dest:moving_standby o end - o function faddr_src:fast_forward o begin o load param_dest:motion:24 param_src:dec_num:2 o call faddr_dest:moving_standby o end - o function faddr_src:fast_reverse o begin o load param_dest:motion:24 param_src:dec_num:3 o call faddr_dest:moving_standby o end - o function faddr_src:fast_left_pivot o begin o load param_dest:motion:24 param_src:dec_num:4 o call faddr_dest:moving_standby o end - o function faddr_src:fast_right_pivot o begin o load param_dest:motion:24 param_src:dec_num:5 o call faddr_dest:moving_standby o end - o function faddr_src:forward o begin o load param_dest:motion:24 param_src:dec_num:6 o call faddr_dest:moving_standby o end - o function faddr_src:reverse o begin o load param_dest:motion:24 param_src:dec_num:7 o call faddr_dest:moving_standby o end - o function faddr_src:pivot_left o begin o load param_dest:motion:24 param_src:dec_num:8 o call faddr_dest:moving_standby o end - o function faddr_src:pivot_right o begin o load param_dest:motion:24 param_src:dec_num:9 o call faddr_dest:moving_standby o end - o function faddr_src:moving_standby o begin o wait param_src:motion:25 lop:== param_src:bool_num:1 rop:then o end - o function faddr_src:buzzer o begin o load param_dest:cm:55 param_src:var:buzzer_time o load param_dest:cm:54 param_src:var:buzzer_scale o wait param_src:cm:55 lop:> param_src:dec_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:********************************************** 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: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: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: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 // 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 µY