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_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 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 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 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