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()