def callback_multi_position_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], 0)
print("")
rospy.logwarn("you are controlling [ " + str(num) + " ] servos whose IDs is : " + str(id) +
" with POSITION 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_position = multi_servo_command.target_position
target_position = list(target_position)
for i in range(num):
Kondo_B3M.control_servo_by_position_without_time(
id[i], target_position[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_position = multi_servo_command.target_position
target_position = list(target_position)
# print(num)
for i in range(num):
Kondo_B3M.control_servo_by_position_without_time(
id[i], target_position[i])
publish_servo_info()
def position_control(servo_command):
global id, initial_process_flag
target_position = servo_command.target_position
# execute only single time
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, 0)
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_position_without_time(
id, target_position)
# print("1")
publish_servo_info()