예제 #1
0
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)
예제 #3
0
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
예제 #4
0
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)
예제 #5
0
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
예제 #6
0
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