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.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], 4)
print("")
rospy.logwarn("you are controlling [" + str(num) + "] servos whose IDs is : " + str(id) +
" at VELOCITY CONTROL MODE, which are autodetected")
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)