def read_only(fxs, port, baud_rate, run_time=8, time_step=0.1):
"""
Reads FlexSEA device and prints gathered data.
"""
debug_logging_level = 0 # 6 is least verbose, 0 is most verbose
data_log = True # False means no logs will be saved
dev_id = fxs.open(port, baud_rate, debug_logging_level)
fxs.start_streaming(dev_id, freq=100, log_en=data_log)
app_type = fxs.get_app_type(dev_id)
if app_type.value == fxe.FX_ACT_PACK.value:
print("\nYour device is an ActPack.\n")
input("Press Enter to continue...")
elif app_type.value == fxe.FX_NET_MASTER.value:
print("\nYour device is a NetMaster.\n")
input("Press Enter to continue...")
elif app_type.value == fxe.FX_BMS.value:
print("\nYour device is a BMS.\n")
input("Press Enter to continue...")
elif app_type.value == fxe.FX_EXO.value:
print("\nYour device is an Exo or ActPack Plus.\n")
input("Press Enter to continue...")
else:
raise RuntimeError(f"Unsupported application type: {app_type}")
total_loop_count = int(run_time / time_step)
for i in range(total_loop_count):
fxu.print_loop_count(i, total_loop_count)
sleep(time_step)
fxu.clear_terminal()
data = fxs.read_device(dev_id)
fxu.print_device(data, app_type)
fxs.close(dev_id)
return True
def two_devices_position_control(fxs, ports, baud_rate):
"""Runs position control on two devices"""
exp_time = 8
time_step = 0.1
dev_id_0 = fxs.open(ports[0], baud_rate, 0)
dev_id_1 = fxs.open(ports[1], baud_rate, 0)
fxs.start_streaming(dev_id_0, 200, log_en=False)
sleep(0.1)
fxs.start_streaming(dev_id_1, 200, log_en=False)
sleep(0.1)
act_pack_state_0 = fxs.read_device(dev_id_0)
act_pack_state_1 = fxs.read_device(dev_id_1)
initial_angle_0 = act_pack_state_0.mot_ang
initial_angle_1 = act_pack_state_1.mot_ang
fxs.set_gains(dev_id_0, 50, 3, 0, 0, 0, 0)
fxs.set_gains(dev_id_1, 50, 3, 0, 0, 0, 0)
fxs.send_motor_command(dev_id_0, fxe.FX_POSITION, initial_angle_0)
fxs.send_motor_command(dev_id_1, fxe.FX_POSITION, initial_angle_1)
num_time_steps = int(exp_time / time_step)
for i in range(num_time_steps):
sleep(time_step)
fxu.clear_terminal()
act_pack_state_0 = fxs.read_device(dev_id_0)
act_pack_state_1 = fxs.read_device(dev_id_1)
current_angle_0 = act_pack_state_0.mot_ang
current_angle_1 = act_pack_state_1.mot_ang
print("Device 0:\n---------\n")
print(f"Desired: {initial_angle_0}")
print(f"Measured: {current_angle_0}")
print(f"Difference: {current_angle_0 - initial_angle_0}\n")
fxu.print_device(act_pack_state_0, fxe.FX_ACT_PACK)
print("\nDevice 1:\n---------\n")
print(f"Desired: {initial_angle_1}")
print(f"Measured: {current_angle_1}")
print(f"Difference: {current_angle_1 - initial_angle_1}\n",
flush=True)
fxu.print_device(act_pack_state_1, fxe.FX_ACT_PACK)
fxu.print_loop_count(i, num_time_steps)
print("Turning off position control...")
fxs.set_gains(dev_id_0, 0, 0, 0, 0, 0, 0)
fxs.set_gains(dev_id_1, 0, 0, 0, 0, 0, 0)
fxs.send_motor_command(dev_id_1, fxe.FX_NONE, 0)
fxs.send_motor_command(dev_id_0, fxe.FX_NONE, 0)
sleep(0.5)
fxs.close(dev_id_0)
fxs.close(dev_id_1)
def current_control(fxs,
port,
baud_rate,
hold_current=[1000],
time=6,
time_step=0.1):
"""
demo current control
"""
dev_id = fxs.open(port, baud_rate, log_level=6)
fxs.start_streaming(dev_id, 100, log_en=False)
app_type = fxs.get_app_type(dev_id)
print("Setting controller to current...")
# Gains are, in order: kp, ki, kd, K, B & ff
fxs.set_gains(dev_id, 40, 400, 0, 0, 0, 128)
sleep(0.5)
prev_current = hold_current[0]
num_time_steps = int(time / time_step)
for current in hold_current:
for i in range(num_time_steps):
des_current = int((current - prev_current) *
(i / float(num_time_steps)) + prev_current)
fxs.send_motor_command(dev_id, fxe.FX_CURRENT, des_current)
sleep(time_step)
act_pack = fxs.read_device(dev_id)
fxu.clear_terminal()
print("Desired (mA): ", des_current)
print("Measured (mA): ", act_pack.mot_cur)
print("Difference (mA): ", (act_pack.mot_cur - des_current),
"\n")
fxu.print_device(act_pack, app_type)
prev_current = current
print("Turning off current control...")
# Ramp down first
ramp_down_steps = 50
for step in range(ramp_down_steps):
des_current = prev_current * (ramp_down_steps - step) / ramp_down_steps
fxs.send_motor_command(dev_id, fxe.FX_CURRENT, des_current)
act_pack = fxs.read_device(dev_id)
fxu.clear_terminal()
print("Desired (mA): ", des_current)
print("Measured (mA): ", act_pack.mot_cur)
print("Difference (mA): ", (act_pack.mot_cur - des_current), "\n")
fxu.print_device(act_pack, app_type)
sleep(time_step)
# When we exit we want the motor to be off
fxs.send_motor_command(dev_id, fxe.FX_NONE, 0)
sleep(0.5)
fxs.close(dev_id)
return True
def leader_follower(fxs, ports, baud_rate):
"""
lead the motion of an ActPack by manually moving another one
"""
dev_id_0 = fxs.open(ports[0], baud_rate, 6) # Leader
dev_id_1 = fxs.open(ports[1], baud_rate, 6) # Follower
fxs.start_streaming(dev_id_0, 200, False)
fxs.start_streaming(dev_id_1, 200, False)
sleep(0.2)
act_pack_state_0 = fxs.read_device(dev_id_0)
act_pack_state_1 = fxs.read_device(dev_id_1)
initial_angle_0 = act_pack_state_0.mot_ang
initial_angle_1 = act_pack_state_1.mot_ang
# Set first device to current controller with 0 current (0 torque)
fxs.set_gains(dev_id_0, 40, 400, 0, 0, 0, 128)
fxs.send_motor_command(dev_id_0, fxe.FX_CURRENT, 0)
# Set position controller for second device
fxs.set_gains(dev_id_1, 50, 10, 0, 0, 0, 0)
fxs.send_motor_command(dev_id_1, fxe.FX_POSITION, initial_angle_1)
loop_count = 200
try:
for i in range(loop_count):
sleep(0.05)
fxu.clear_terminal()
leader_data = fxs.read_device(dev_id_0)
follower_data = fxs.read_device(dev_id_1)
angle0 = leader_data.mot_ang
diff = angle0 - initial_angle_0
fxs.send_motor_command(dev_id_1, fxe.FX_POSITION,
initial_angle_1 + diff)
print(f"Device {dev_id_1} following device {dev_id_0}\n")
fxu.print_device(follower_data, fxe.FX_ACT_PACK)
print("") # Empty line
fxu.print_device(leader_data, fxe.FX_ACT_PACK)
fxu.print_loop_count(i, loop_count)
except Exception as err:
print(f"Problem encountered: {err}")
print(traceback.format_exc())
print("Turning off position control...")
fxs.set_gains(dev_id_0, 0, 0, 0, 0, 0, 0)
fxs.set_gains(dev_id_1, 0, 0, 0, 0, 0, 0)
fxs.send_motor_command(dev_id_1, fxe.FX_NONE, 0)
fxs.send_motor_command(dev_id_0, fxe.FX_NONE, 0)
sleep(0.5)
fxs.close(dev_id_0)
fxs.close(dev_id_1)
def open_control(
fxs,
port,
baud_rate,
run_time=2,
num_times=5,
time_resolution=0.1,
max_voltage=3000,
sign=-1,
):
"""Implements open control for ActPack"""
dev_id = fxs.open(port, baud_rate, log_level=6)
fxs.start_streaming(dev_id, 100, log_en=False)
app_type = fxs.get_app_type(dev_id)
print("Setting open control...")
fxs.send_motor_command(dev_id, fxe.FX_VOLTAGE, 0)
sleep(0.5)
step_count = int((run_time / 2) / time_resolution)
for rep in range(num_times):
# TODO(CA): Refactor loop to remove code repetition
# Ramp-up:
for step in range(step_count):
sleep(time_resolution)
voltage_mv = sign * max_voltage * (step * 1.0 / step_count)
fxs.send_motor_command(dev_id, fxe.FX_VOLTAGE, voltage_mv)
fxu.clear_terminal()
print(f"Ramping up motor voltage {rep}...\n")
data0 = fxs.read_device(dev_id)
fxu.print_device(data0, app_type)
# Ramp-down:
for step in range(step_count):
sleep(time_resolution)
voltage_mv = sign * max_voltage * ((step_count - step) * 1.0 / step_count)
fxs.send_motor_command(dev_id, fxe.FX_VOLTAGE, voltage_mv)
fxu.clear_terminal()
print(f"Ramping down motor voltage {rep}...\n")
data0 = fxs.read_device(dev_id)
fxu.print_device(data0, app_type)
fxs.send_motor_command(dev_id, fxe.FX_NONE, 0)
sleep(0.1)
fxs.close(dev_id)
return True
def position_control(fxs,
port,
baud_rate,
time=8,
time_step=0.1,
resolution=100):
"""
Implement position control
"""
dev_id = fxs.open(port, baud_rate, log_level=6)
fxs.start_streaming(dev_id, resolution, log_en=False)
sleep(0.1)
act_pack_state = fxs.read_device(dev_id)
fxu.print_device(act_pack_state, fxe.FX_ACT_PACK)
initial_angle = act_pack_state.mot_ang
# Gains are, in order: kp, ki, kd, K, B & ff
fxs.set_gains(dev_id, 400, 50, 0, 0, 0, 0)
fxs.send_motor_command(dev_id, fxe.FX_POSITION, initial_angle)
num_time_steps = int(time / time_step)
for i in range(num_time_steps):
sleep(time_step)
fxu.clear_terminal()
act_pack_state = fxs.read_device(dev_id)
current_angle = act_pack_state.mot_ang
print("Desired: ", initial_angle)
print("Measured: ", current_angle)
print("Difference: ",
current_angle - initial_angle,
"\n",
flush=True)
fxu.print_device(act_pack_state, fxe.FX_ACT_PACK)
fxu.print_loop_count(i, num_time_steps)
# When we exit we want the motor to be off
fxs.send_motor_command(dev_id, fxe.FX_NONE, 0)
sleep(0.5)
fxs.close(dev_id)
return True
def two_position_control(
fxs,
port,
baud_rate,
exp_time=13,
time_step=0.1,
delta=10000,
transition_time=1.5,
resolution=100,
):
"""
Send two positions commands to test the positionc controller
"""
# Open device
dev_id = fxs.open(port, baud_rate, 0)
fxs.start_streaming(dev_id, resolution, log_en=True)
sleep(0.1)
# Setting initial angle and angle waypoints
act_pack_state = fxs.read_device(dev_id)
initial_angle = act_pack_state.mot_ang
# Setting angle waypoints
positions = [initial_angle, initial_angle + delta]
current_pos = 0
num_pos = 2
# Setting loop duration and transition rate
num_time_steps = int(exp_time / time_step)
transition_steps = int(transition_time / time_step)
# Setting gains (dev_id, kp, ki, kd, K, B, ff)
fxs.set_gains(dev_id, 50, 0, 0, 0, 0, 0)
# kp=50 and ki=0 is a very soft controller, perfect for bench top experiments
# Setting position control at initial position
fxs.send_motor_command(dev_id, fxe.FX_POSITION, initial_angle)
# Matplotlib - initialize lists
requests = []
measurements = []
times = []
start_time = time()
# Start two position control
for i in range(num_time_steps):
sleep(time_step)
act_pack_state = fxs.read_device(dev_id)
fxu.clear_terminal()
measured_pos = act_pack_state.mot_ang
print(f"Desired: {positions[current_pos]}")
print(f"Measured: {measured_pos}")
print(
f"Difference: {(measured_pos - positions[current_pos])}\n"
)
fxu.print_device(act_pack_state, fxe.FX_ACT_PACK)
if i % transition_steps == 0:
current_pos = (current_pos + 1) % num_pos
fxs.send_motor_command(dev_id, fxe.FX_POSITION,
positions[current_pos])
# Plotting
times.append(time() - start_time)
requests.append(positions[current_pos])
measurements.append(measured_pos)
# Disable the controller, send 0 PWM
fxs.send_motor_command(dev_id, fxe.FX_VOLTAGE, 0)
sleep(0.1)
# Plot before exit:
plt.title("Two Position Control Demo")
plt.plot(times, requests, color="b", label="Desired position")
plt.plot(times, measurements, color="r", label="Measured position")
plt.xlabel("Time (s)")
plt.ylabel("Encoder position")
plt.legend(loc="upper right")
fxu.print_plot_exit()
plt.show()
# Close device and do device cleanup
return fxs.close(dev_id)
def impedance_control(
fxs,
port,
baud_rate,
exp_time=10,
time_step=0.02,
delta=7500,
transition_time=0.8,
resolution=500,
):
"""
Implements impedance control
"""
# Open device
dev_id = fxs.open(port, baud_rate, log_level=6)
fxs.start_streaming(dev_id, resolution, log_en=False)
sleep(0.1)
# Read initial angle
data = fxs.read_device(dev_id)
initial_angle = data.mot_ang
result = True
transition_steps = int(transition_time / time_step)
# Initialize lists for matplotlib
requests = []
measurements = []
times = []
# Setpoint = initial angle
fxs.send_motor_command(dev_id, fxe.FX_IMPEDANCE, initial_angle)
# Set gains (in order: kp, ki, kd, K, B & ff)
fxs.set_gains(dev_id, GAINS["kp"], GAINS["ki"], 0, GAINS["K"], GAINS["B"], GAINS["FF"])
# Select transition rate and positions
current_pos = 0
num_time_steps = int(exp_time / time_step)
positions = [initial_angle, initial_angle + delta]
sleep(0.4)
# Record start time of experiment
start_time = time()
# Run demo
loop_ctr = 0
print("")
for i in range(num_time_steps):
loop_ctr += 1
data = fxs.read_device(dev_id)
measured_pos = data.mot_ang
if i % transition_steps == 0:
GAINS["B"] += GAINS["B_Increments"] # Increments every cycle
fxs.set_gains(
dev_id, GAINS["kp"], GAINS["ki"], 0, GAINS["K"], GAINS["B"], GAINS["FF"]
)
delta = abs(positions[current_pos] - measured_pos)
result &= delta < resolution
current_pos = (current_pos + 1) % 2
fxs.send_motor_command(dev_id, fxe.FX_IMPEDANCE, positions[current_pos])
sleep(time_step)
# We downsample the display refresh:
if i % 10 == 0:
fxu.clear_terminal()
print(f"Loop {loop_ctr} of {num_time_steps}")
print(f"Holding position: {positions[current_pos]}")
print(GAINS)
fxu.print_device(data, fxe.FX_ACT_PACK)
# Plotting:
measurements.append(measured_pos)
times.append(time() - start_time)
requests.append(positions[current_pos])
# Disable the controller, send 0 PWM
fxs.send_motor_command(dev_id, fxe.FX_VOLTAGE, 0)
# Plot before we exit:
title = "Impedance Control Demo"
plt.plot(times, requests, color="b", label="Desired position")
plt.plot(times, measurements, color="r", label="Measured position")
plt.xlabel("Time (s)")
plt.ylabel("Encoder position")
plt.title(title)
plt.legend(loc="upper right")
fxu.print_plot_exit()
plt.show()
# Close device
print("End of script, closing device.")
fxs.close(dev_id)
return True