def callback_multi_torque_control(multi_servo_command):
global num, id, initial_process_flag, target_torque, pre_target_torque, 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], 8)
pre_target_torque.append(0)
print("")
rospy.logwarn(
"you are controlling [" + str(num) + "] servos whose IDs is : " +
str(id) +
" with TORQUE 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_torque = multi_servo_command.target_torque
target_torque = list(target_torque)
for i in range(num):
# damp target torque since drastic difference of target torque may cause lock of servo
target_torque[i] = damp_target_torque(target_torque[i],
pre_target_torque[i])
# print(str(target_torque))
Kondo_B3M.control_servo_by_Torque(id[i], target_torque[i])
pre_target_torque[i] = target_torque[i]
publish_servo_info()
def torque_control(servo_command):
global pre_target_torque, id, initial_process_flag
target_torque = servo_command.target_torque
# 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, 8)
print("")
rospy.logwarn(
"you are controlling servo ID : " + str(id) +
". If you want to change the ID, abort this code and try again after execute <$ rosparam set /servo_id YOUR_ID>, or change it via launch file."
)
initial_process_flag = 0
# ramp target torque since drastic difference of target torque may cause lock of servo
target_torque = ramp_target_torque(target_torque, pre_target_torque)
Kondo_B3M.control_servo_by_Torque(id, target_torque)
publish_servo_info()
pre_target_torque = target_torque
def publish_servo_info():
global id, num, battery_voltage_warn_flag, battery_voltage_fatal_flag, voltage, k
multi_servo_info = Multi_servo_info()
for i in range(num):
voltage = list(voltage)
multi_servo_info.encoder_count.append(
Kondo_B3M.get_encoder_total_count(id[i]))
# ommit to get motor velocity due to low control rate
# multi_servo_info.motor_velocity.append(
# Kondo_B3M.get_servo_Velocity(id[i]))
multi_servo_info.input_voltage.append(voltage[i])
# we don't have to monitor voltage at every loop, so get sparsed at a time per 100 loops
if k % 100 == 0:
k = 0
for j in range(num):
voltage[j] = Kondo_B3M.get_servo_voltage(id[j])
if voltage[j] < BATTERY_VOLTAGE_WARN and battery_voltage_warn_flag == 0:
print("")
rospy.logwarn('battery voltage is low !')
battery_voltage_warn_flag = 1
elif voltage[j] < BATTERY_VOLTAGE_FATAL and battery_voltage_fatal_flag == 0:
print("")
rospy.logfatal('battery voltage is fatally low !')
k = k + 1
multi_servo_info_pub.publish(multi_servo_info)
del multi_servo_info
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_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 initial_process():
global id, num, initial_process_flag, the_number_of_servo_pub
if initial_process_flag == 1:
for i in range(255):
# Kondo_B3M.resetServo(i)
result = Kondo_B3M.enFreeServo(i)
# print(result)
if result == 1:
id.append(i)
num = num + 1
for j in range(num):
Kondo_B3M.resetServo(id[j])
Kondo_B3M.enFreeServo(id[j])
Kondo_B3M.reset_encoder_total_count(id[j])
# mode : 00>positionCTRL, 04>velocityCTRL, 08>current(torque)CTRL, 12>feedforwardCTRL
Kondo_B3M.change_servocontrol_mode(id[j], 0)
print("")
rospy.logwarn(
"you are controlling [" + str(num) + "] servos whose IDs is : " +
str(id) +
" at POSITION CONTROL MODE, which are automatically detected.")
initial_process_flag = 0
the_number_of_servo_pub.publish(num)
else:
pass
def initial_process():
global id, num, initial_process_flag, the_number_of_servo_pub
global target_torque, ramped_target_torque, pre_target_torque, voltage
# for i in range(255):
for i in range(10):
# Kondo_B3M.resetServo(i)
result = Kondo_B3M.initServo(i)
# print(result)
if result == 1:
id.append(i)
num = num + 1
for j in range(num):
Kondo_B3M.resetServo(id[j])
Kondo_B3M.enFreeServo(id[j])
Kondo_B3M.reset_encoder_total_count(id[j])
# mode : 00>positionCTRL, 04>velocityCTRL, 08>current(torque)CTRL, 12>feedforwardCTRL
Kondo_B3M.change_servocontrol_mode(id[j], 8)
target_torque.append(0)
ramped_target_torque.append(0)
pre_target_torque.append(0)
voltage.append(16000)
print("")
rospy.logwarn("you are controlling [ " + str(num) + " ] servos whose IDs is : " + str(
id) + " at TORQUE CONTROL MODE, which are automatically detected.")
target_torque = list(target_torque)
ramped_target_torque = list(ramped_target_torque)
pre_target_torque = list(pre_target_torque)
initial_process_flag = 0
the_number_of_servo_pub.publish(num)
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 publish_servo_info():
global id
servo_info.encoder_count = Kondo_B3M.get_encoder_total_count(id)
voltage = Kondo_B3M.get_servo_voltage(id)
if voltage < BATTERY_VOLTAGE_WARN and battery_voltage_warn_flag == 0:
rospy.logwarn('battery voltage is low !')
battery_voltage_warn_flag = 1
elif voltage < BATTERY_VOLTAGE_FATAL and battery_voltage_fatal_flag == 0:
rospy.logfatal('battery voltage is low !')
battery_voltage_fatal_flag = 1
servo_info.input_voltage = voltage
servo_info.motor_velocity = Kondo_B3M.get_servo_Velocity(id)
servo_info_pub.publish(servo_info)
def callback_servo_command(multi_servo_command):
global num, target_torque, pre_target_torque, ramped_target_torque
target_torque = multi_servo_command.target_torque
target_torque = list(target_torque)
# print(pre_target_torque)
# print(target_torque)
for i in range(num):
# ramp target torque since drastic difference of target torque may cause lock of servo
ramped_target_torque[i] = ramp_target_torque(
target_torque[i], pre_target_torque[i])
Kondo_B3M.control_servo_by_Torque(id[i], ramped_target_torque[i])
# print(ramped_target_torque)
# print("")
pre_target_torque = ramped_target_torque
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()
def callback_servo_command(multi_servo_command):
global num, target_torque, pre_target_torque, ramped_target_torque, id, merged_command, servo_reset_flag, servo_drive_flag
target_torque = multi_servo_command.target_torque
target_torque = list(target_torque)
# print(pre_target_torque)
# print(target_torque)
for i in range(num):
# ramp target torque since drastic difference of target torque may cause lock of servo
ramped_target_torque[i] = ramp_target_torque(
target_torque[i], pre_target_torque[i])
for j in range(num):
merged_command.append(id[j])
for k in range(num):
merged_command.append(ramped_target_torque[k])
# actual locomotions are only conducted when the flag is active
if servo_drive_flag == 1:
Kondo_B3M.control_servo_by_Torque_multicast(merged_command)
pre_target_torque = ramped_target_torque
publish_servo_info()
merged_command = []
# if flag for resetting servos is active
if servo_reset_flag == 1:
for j in range(num): # start resetting and initiating servos again
Kondo_B3M.resetServo(id[j])
Kondo_B3M.enFreeServo(id[j])
Kondo_B3M.reset_encoder_total_count(id[j])
# mode : 00>positionCTRL, 04>velocityCTRL, 08>current(torque)CTRL, 12>feedforwardCTRL
Kondo_B3M.change_servocontrol_mode(id[j], 8)
rospy.logwarn('complete resetting servos!')
servo_reset_flag = 0 # deactivate flag for resetting servos
# deactivate flag for servo locomoting until it's triggered to restart
servo_drive_flag = 0
def publish_servo_info():
global id, num, battery_voltage_warn_flag, battery_voltage_fatal_flag, voltage
multi_servo_info = Multi_servo_info()
for i in range(num):
multi_servo_info.encoder_count.append(
Kondo_B3M.get_encoder_total_count(id[i]))
multi_servo_info.input_voltage.append(
Kondo_B3M.get_servo_voltage(id[i]))
voltage = multi_servo_info.input_voltage
if voltage[i] < BATTERY_VOLTAGE_WARN and battery_voltage_warn_flag == 0:
print("")
rospy.logwarn('battery voltage is low !')
battery_voltage_warn_flag = 1
elif voltage[i] < BATTERY_VOLTAGE_FATAL and battery_voltage_fatal_flag == 0:
print("")
rospy.logfatal('battery voltage is fatally low !')
multi_servo_info.motor_velocity.append(
Kondo_B3M.get_servo_Velocity(id[i]))
multi_servo_info_pub.publish(multi_servo_info)
del multi_servo_info
def enfree_servo_after_node_ends(signal, frame):
global id
for i in range(num):
Kondo_B3M.enFreeServo(id[i])
sys.exit(0)
def enfree_servo_after_node_ends(signal, frame):
Kondo_B3M.enFreeServo(4)
sys.exit(0)
def enfree_servo_after_node_ends(signal, frame):
global id
Kondo_B3M.enFreeServo(id)
sys.exit(0)
