def callback_multi_velocity_control(multi_servo_command): global num, id, initial_process_flag, voltage # execute only single time if initial_process_flag == 1: num = set_the_num_of_servo() id = set_servo_id() for i in range(num): Kondo_B3M.resetServo(id[i]) Kondo_B3M.enFreeServo(id[i]) Kondo_B3M.reset_encoder_total_count(id[i]) # mode : 00>positionCTRL, 04>velocityCTRL, 08>current(torque)CTRL, 12>feedforwardCTRL Kondo_B3M.change_servocontrol_mode(id[i], 4) print("") rospy.logwarn( "you are controlling [ " + str(num) + " ] servos whose IDs is : " + str(id) + " with VELOCITY CONTROL MODE. If you want to change the number of servos or their IDs, abort this code and try again after execute <$ rosparam set /num_of_servo THE_NUMBER_OF_SERVOS> and <$ rosparam set /multi_servo_id [YOUR_ID#1, YOUR_ID#2 etc]> or change them via launch file" ) initial_process_flag = 0 target_velocity = multi_servo_command.target_velocity target_velocity = list(target_velocity) for i in range(num): Kondo_B3M.control_servo_by_Velocity(id[i], target_velocity[i]) publish_servo_info()
def callback_multi_position_control(multi_servo_command): global num, id, initial_process_flag, target_torque, pre_target_torque, voltage target_velocity = multi_servo_command.target_velocity target_velocity = list(target_velocity) for i in range(num): Kondo_B3M.control_servo_by_Velocity(id[i], target_velocity[i]) publish_servo_info()
def callback_velocity_control(servo_command): global id, initial_process_flag target_velocity = servo_command.target_velocity if initial_process_flag == 1: id = set_servo_id() Kondo_B3M.resetServo(id) Kondo_B3M.enFreeServo(id) Kondo_B3M.reset_encoder_total_count(id) # mode : 00>positionCTRL, 04>velocityCTRL, 08>current(torque)CTRL, 12>feedforwardCTRL Kondo_B3M.change_servocontrol_mode(id, 4) print("") print("you are controlling servo ID : " + str(id)) print("if you want to change the ID, abort this code and try again after execute <$ rosparam set /servo_id YOUR_ID>") initial_process_flag = 0 Kondo_B3M.control_servo_by_Velocity(id, target_velocity) publish_servo_info()