def initialize_servo_for_torque_control():
global num, id
for i in range(num): # start resetting and initiating servos again
Drive.resetServo(id[i])
Drive.enFreeServo(id[i])
Drive.reset_encoder_total_count(id[i])
# mode : 00>positionCTRL, 04>velocityCTRL, 08>current(torque)CTRL, 12>feedforwardCTRL
Drive.change_servocontrol_mode(id[i], 8)
def initialize_servo():
global SERVO_ID, control_mode, battery_voltage
for i in range(len(SERVO_ID)):
# reset servo before setting its control mode
Drive.resetServo(SERVO_ID[i])
Drive.enFreeServo(SERVO_ID[i])
Drive.reset_encoder_total_count(SERVO_ID[i])
# prepare global parameter for battery voltage
battery_voltage.append(0)
change_control_mode(SERVO_ID, control_mode)
def enfree_servo_after_node_ends(signal, frame):
global id
for i in range(num):
Drive.enFreeServo(id[i])
sys.exit(0)
def enfree_servo_after_node_ends(signal, frame):
global SERVO_ID
for i in range(len(SERVO_ID)):
Drive.enFreeServo(SERVO_ID[i])
sys.exit(0)