def setUpClass(cls):
channel = guess_channel(bustype_hint=bustype)
can_bus: can.Bus = can.Bus(bustype=bustype, channel=channel)
iface: IFace = CAN(can_bus)
cls.tm = Tinymovr(node_id=1, iface=iface)
cls.tm.reset()
time.sleep(0.2)
def main():
global tm1, tm2
channel = guess_channel(bustype_hint='slcan')
can_bus: can.Bus = can.Bus(bustype='slcan',
channel=channel,
bitrate=1000000)
iface: IFace = CAN(can_bus)
tm1 = Tinymovr(node_id=1, iface=iface)
tm2 = Tinymovr(node_id=2, iface=iface)
assert (tm1.motor_config.flags == 1)
assert (tm2.motor_config.flags == 1)
tm1.set_limits(300000, 25)
tm2.set_limits(300000, 25)
tm1.set_gains(280, 1.1e-4)
tm2.set_gains(280, 1.1e-4)
sleep(0.1)
tm1.position_control()
tm2.position_control()
sleep(0.1)
offset_1 = tm1.encoder_estimates.position
offset_2 = tm2.encoder_estimates.position
while True:
est_1 = tm1.encoder_estimates
est_2 = tm2.encoder_estimates
mean_pos = ((est_1.position - offset_1) +
(est_2.position - offset_2)) / 2.0
mean_vel = (est_1.velocity + est_2.velocity) / 2.0
tm1.set_pos_setpoint(mean_pos + offset_1, mean_vel, 0)
tm2.set_pos_setpoint(mean_pos + offset_2, mean_vel, 0)
sleep(0.0005)
def main():
global tm1, tm2
channel = guess_channel(bustype_hint='slcan')
can_bus: can.Bus = can.Bus(bustype='slcan',
channel=channel,
bitrate=1000000)
iface: IFace = CAN(can_bus)
tm1 = Tinymovr(node_id=1, iface=iface)
tm2 = Tinymovr(node_id=2, iface=iface)
assert (tm1.motor_config.flags == 1)
assert (tm2.motor_config.flags == 1)
tm1.set_limits(200000, 15)
tm2.set_limits(200000, 15)
sleep(0.1)
tm1.current_control()
tm2.current_control()
sleep(0.1)
offset_1 = tm1.encoder_estimates.position
offset_2 = tm2.encoder_estimates.position
while True:
est_1 = tm1.encoder_estimates
est_2 = tm2.encoder_estimates
p_1 = est_1.position - offset_1
p_2 = est_2.position - offset_2
v_1 = est_1.velocity
v_2 = est_2.velocity
Iq_1 = (3e-3 * (p_2 - p_1) * (A / tick) + 5e-5 * (v_2 - v_1) *
(A * s / tick))
Iq_2 = (3e-3 * (p_1 - p_2) * (A / tick) + 5e-5 * (v_1 - v_2) *
(A * s / tick))
tm1.set_cur_setpoint(Iq_1)
tm2.set_cur_setpoint(Iq_2)
sleep(0.0001)
def __init__(self, **kwargs):
self.tinymovr = Tinymovr(**kwargs)
class UserWrapper:
'''
This is a user-friendly wrapper to the Tinymover class, that
allows more intuitive functions to be used instead of plain
getters/setters.
This class sacrifices Pythonic-ness (e.g. raising exceptions)
for better user-friendliness (e.g. using simple print statements).
There are also additional checks performed prior to changing
states, with errors displayed if checks fail.
'''
def __init__(self, **kwargs):
self.tinymovr = Tinymovr(**kwargs)
def __getattr__(self, attr):
return getattr(self.tinymovr, attr)
def calibrate(self):
'''
Enter calibration state, perform motor and encoder calibration
'''
state = self.tinymovr.state
if state.error != 0:
print("Error flag present, cannot continue with calibration. \
Please reset Tinymovr.")
elif state.state != 0:
print("Tinymovr state is not idle, \
calibration needs to be started from idle state.")
else:
input("Ready to calibrate. Please remove any loads \
from the motor and hit Enter to continue")
self.tinymovr.calibrate()
def idle(self):
'''
Enter idle state
'''
self.tinymovr.set_state(0)
def position_control(self):
'''
Enter closed loop control state and position control mode
'''
state = self.tinymovr.state
if state.error != 0:
print("Error flag present, cannot enable position control. \
Please reset Tinymovr.")
elif state.state == 1:
print("Tinymovr is currently calibrating, \
please do not interrupt.")
else:
self.tinymovr.position_control()
def velocity_control(self):
'''
Enter closed loop control state and velocity control mode
'''
state = self.tinymovr.state
if state.error != 0:
print("Error flag present, cannot enable velocity control. \
Please reset Tinymovr.")
elif state.state == 1:
print("Tinymovr is currently calibrating, \
please do not interrupt.")
else:
self.tinymovr.velocity_control()
def current_control(self):
'''
Enter closed loop control state and current control mode
'''
state = self.tinymovr.state
if state.error != 0:
print("Error flag present, cannot enable current control. \
Please reset Tinymovr.")
elif state.state == 1:
print("Tinymovr is currently calibrating, \
please do not interrupt.")
else:
self.tinymovr.current_control()
@property
def error(self):
'''
Report controller error in human-readable form
'''
state = self.tinymovr.state
error_id = ErrorIDs(state.error)
print(error_descriptions[error_id] + " (error code: " + str(error_id) +
")")
def __dir__(self):
tm_keys = self.tinymovr.__dir__()
self_keys = object.__dir__(self)
self_keys.remove("tinymovr")
return tm_keys + self_keys
def get_tm() -> Tinymovr:
can_bus: can.Bus = can.Bus(bustype=bustype, channel=channel)
iface: IFace = CAN(can_bus)
tm = Tinymovr(node_id=1, iface=iface)
tm.iface.bus.legacy_errors = True
return tm
def get_tm() -> Tinymovr:
can_bus: can.Bus = can.Bus(bustype=bustype, channel=channel)
iface: IFace = CANBus(can_bus)
return Tinymovr(node_id=1, iface=iface)
dt = 0.01
step = 1 * deg
period = 2 * s
current_threshold = 8.0 * A
sign = 0
ratio = 9
goon = True
position = 0 * deg
modepygame = 0
# channel = guess_channel(bustype_hint='slcan')
channel = '/dev/ttyS7'
can_bus = can.Bus(bustype='slcan', channel=channel, bitrate=1000000)
iface = CAN(can_bus)
tm = Tinymovr(node_id=1, iface=iface)
tm.set_limits(velocity=2000 * turn / min, current=8.0 * A)
tm.set_gains(position=100.0, velocity=0.0001)
tm.set_integrator_gains(velocity=0.001)
print(tm.motor_info)
print(tm.device_info)
print("ZERO")
position = tm.encoder_estimates.position
while goon:
for event in pygame.event.get():
if event.type == pygame.QUIT:
goon = False
t0 = 0 Y0 = 0.0 * m Z0 = 0.0 * m alpha = 0.0 * deg D = 0.0 * m E = 0.0 * m F = 0.0 * m Yfoot = 0.0 * m Zfoot = (L1 + L2) * m modepygame = -1 # channel = guess_channel(bustype_hint='slcan') channel = '/dev/ttyS7' can_bus = can.Bus(bustype='slcan', channel=channel, bitrate=1000000) iface = CAN(can_bus) tm1 = Tinymovr(node_id=2, iface=iface) tm2 = Tinymovr(node_id=1, iface=iface) maxspeed = 1500 * turn / min tm1.set_limits(velocity=maxspeed * 1.5, current=22.0 * A) tm2.set_limits(velocity=maxspeed, current=22.0 * A) tm1.set_gains(position=100.0, velocity=0.0002) tm2.set_gains(position=100.0, velocity=0.0002) tm1.set_integrator_gains(velocity=0.001) tm2.set_integrator_gains(velocity=0.001) # print(tm1.motor_info) # print(tm2.motor_info) # print(tm1.device_info) # print(tm2.device_info) jump = 0.08
class UserWrapper:
"""
This is a user-friendly wrapper to the Tinymover class, that
allows more intuitive functions to be used instead of plain
getters/setters.
This class sacrifices Pythonic-ness (e.g. raising exceptions)
for better user-friendliness (e.g. using simple print statements).
There are also additional checks performed prior to changing
states, with errors displayed if checks fail.
"""
def __init__(self, *args, **kwargs):
self.tinymovr = Tinymovr(*args, **kwargs)
def __getattr__(self, attr):
return getattr(self.tinymovr, attr)
def calibrate(self):
"""
Enter calibration state, perform motor and encoder calibration
"""
state = self.tinymovr.state
if state.errors:
print(
"Error flag present, cannot continue with calibration. Please reset Tinymovr."
)
elif state.state != ControlStates.Idle:
print("Tinymovr not in idle state, calibration cannot start.")
else:
input(
"Ready to calibrate. Please remove any loads from the motor and hit Enter to continue."
)
self.tinymovr.calibrate()
def idle(self):
"""
Enter idle state
"""
if self.tinymovr.state.state == ControlStates.Calibration:
print(msg_calibrating)
else:
self.tinymovr.idle()
def position_control(self):
"""
Enter closed loop control state and position control mode
"""
state = self.tinymovr.state
if state.errors:
print(
"Error flag present, cannot enable position control. Please reset Tinymovr."
)
elif state.state == ControlStates.Calibration:
print(msg_calibrating)
else:
self.tinymovr.position_control()
def velocity_control(self):
"""
Enter closed loop control state and velocity control mode
"""
state = self.tinymovr.state
if state.errors:
print(
"Error flag present, cannot enable velocity control. Please reset Tinymovr."
)
elif state.state == ControlStates.Calibration:
print(msg_calibrating)
else:
self.tinymovr.velocity_control()
def current_control(self):
"""
Enter closed loop control state and current control mode
"""
state = self.tinymovr.state
if state.errors:
print(
"Error flag present, cannot enable current control. Please reset Tinymovr."
)
elif state.state == ControlStates.Calibration:
print(msg_calibrating)
else:
self.tinymovr.current_control()
def __dir__(self):
tm_attrs = self.tinymovr.__dir__()
self_attrs = [k for k in object.__dir__(self) if not k.startswith("_")]
self_attrs.remove("tinymovr")
return sorted(list(set(tm_attrs + self_attrs)))
T0 = 21.3 / 20 #20 oscillations in 21.3s
# pendulum natural pulsation
w0 = 2 * pi / T0 * rad / s
nrj0 = -w0 * w0
# pygame init
pygame.init()
ecran = pygame.display.set_mode((1000, 1000))
font = pygame.font.SysFont(None, 48)
# Instanciate Tinymovr interface
# channel = guess_channel(bustype_hint='slcan')
channel = '/dev/ttyS17'
can_bus = can.Bus(bustype='slcan', channel=channel, bitrate=1000000)
iface = CANBus(can_bus)
tm = Tinymovr(node_id=1, iface=iface)
# initialise motor control gains and limits
tm.set_limits(velocity=2000 * turn / min, current=10.0 * A)
tm.set_gains(position=40.0, velocity=0.002)
tm.set_integrator_gains(velocity=0.0000)
print(tm.motor_info)
print(tm.device_info)
# set the zero (assuming the pendulum is downward and still, else, press "z" key)
theta0 = tm.encoder_estimates.position.to(rad)
thetap0 = tm.encoder_estimates.velocity.to(rad / s)
# control loop (100Hz)
while goon:
# recup keyboard input
