def connect_to_exos(file_ID: str = None,
target_freq: int = 200,
actpack_freq: int = 200,
log_en: bool = False,
log_level: int = 3,
do_read_fsrs: bool = False,
max_allowable_current: int = 20000):
'''Connect to Exos, instantiate Exo objects.'''
# Load Ports and baud rate
if fxu.is_win(): # Need for WebAgg server to work in Python 3.8
print('Detected win32')
import asyncio
asyncio.set_event_loop_policy(asyncio.WindowsSelectorEventLoopPolicy())
port_cfg_path = os.path.join(
os.path.dirname(os.path.abspath(__file__)), "ports.yaml")
ports, baud_rate = fxu.load_ports_from_file(port_cfg_path)
elif fxu.is_pi64() or fxu.is_pi():
ports = ['/dev/ttyACM0', '/dev/ttyACM1']
baud_rate = 230400
else:
raise ValueError('Max Code only supporting Windows or pi64 so far')
print(f"Using ports:\t{ports}")
exo_list = []
for port in ports:
try:
dev_id = fxs.open(port, baud_rate, log_level=log_level)
fxs.start_streaming(dev_id=dev_id,
freq=actpack_freq,
log_en=log_en)
exo_list.append(
Exo(dev_id=dev_id,
file_ID=file_ID,
target_freq=target_freq,
do_read_fsrs=do_read_fsrs,
max_allowable_current=max_allowable_current))
except IOError:
print('Unable to open exo on port: ', port,
' This is okay if only one exo is connected!')
if not exo_list: # (if empty)
raise RuntimeError('No Exos connected')
return exo_list
def __init__(self,
dev_id: int,
max_allowable_current: int,
file_ID: str = None,
target_freq: float = 200,
do_read_fsrs: bool = False):
'''Exo object is the primary interface with the Dephy ankle exos, and corresponds to a single physical exoboot.
Args:
dev_id: int. Unique integer to identify the exo in flexsea's library. Returned by connect_to_exo
file_ID: str. Unique string added to filename. If None, no file will be saved.
do_read_dsrs: bool indicating whether to read FSRs '''
self.dev_id = dev_id
self.max_allowable_current = max_allowable_current
self.file_ID = file_ID
self.do_read_fsrs = do_read_fsrs
if self.dev_id is None:
print(
'Exo obj created but no exoboot connected. Some methods available'
)
elif self.dev_id in constants.LEFT_EXO_DEV_IDS:
self.side = constants.Side.LEFT
self.motor_sign = -1
self.ankle_to_motor_angle_polynomial = constants.LEFT_ANKLE_TO_MOTOR
self.ankle_angle_offset = constants.LEFT_ANKLE_ANGLE_OFFSET
elif self.dev_id in constants.RIGHT_EXO_DEV_IDS:
self.side = constants.Side.RIGHT
self.motor_sign = 1
self.ankle_to_motor_angle_polynomial = constants.RIGHT_ANKLE_TO_MOTOR
self.ankle_angle_offset = constants.RIGHT_ANKLE_ANGLE_OFFSET
else:
raise ValueError(
'dev_id: ', self.dev_id,
'not found in constants.LEFT_EXO_DEV_IDS or constants.RIGHT_EXO_DEV_IDS'
)
self.motor_offset = 0
# ankle velocity filter is hardcoded for simplicity, but can be factored out if necessary
self.ankle_velocity_filter = filters.Butterworth(N=2,
Wn=10,
fs=target_freq)
if self.do_read_fsrs:
print('do_read_fsrs: True. Checking if rpi')
if fxu.is_pi() or fxu.is_pi64():
import gpiozero # pylint: disable=import-error
if self.side == constants.Side.LEFT:
self.heel_fsr_detector = gpiozero.InputDevice(
pin=constants.LEFT_HEEL_FSR_PIN, pull_up=True)
self.toe_fsr_detector = gpiozero.InputDevice(
pin=constants.LEFT_TOE_FSR_PIN, pull_up=True)
else:
self.heel_fsr_detector = gpiozero.InputDevice(
pin=constants.RIGHT_HEEL_FSR_PIN, pull_up=True)
self.toe_fsr_detector = gpiozero.InputDevice(
pin=constants.RIGHT_TOE_FSR_PIN, pull_up=True)
else:
raise Exception('Can only use FSRs with rapberry pi!')
self.data = self.DataContainer(do_include_FSRs=do_read_fsrs)
self.has_calibrated = False
self.is_clipping = False
if self.file_ID is not None:
self.setup_data_writer(file_ID=file_ID)
if self.dev_id is not None:
self.update_gains(Kp=constants.DEFAULT_KP,
Ki=constants.DEFAULT_KI,
Kd=constants.DEFAULT_KD,
k_val=0,
b_val=0,
ff=constants.DEFAULT_FF)
self.TR_from_ankle_angle = interpolate.PchipInterpolator(
constants.ANKLE_PTS, self.motor_sign * constants.TR_PTS)
#!/usr/bin/env python3
"""
FlexSEA Two Position Control Demo
"""
from time import sleep, time
import matplotlib
import matplotlib.pyplot as plt
from flexsea import fxUtils as fxu
from flexsea import fxEnums as fxe
from flexsea import flexsea as flex
matplotlib.use("WebAgg")
if fxu.is_pi():
matplotlib.rcParams.update({"webagg.address": "0.0.0.0"})
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)