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)
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
# mesured length
# print("theta20: "+str(theta20.to(deg)))
# print("tm2.position: "+str(tm2.encoder_estimates.position.to(deg)))
# print("ratioKnee: "+str(ratioKnee))
# print("theta2m: "+str(theta2m.to(deg)))
Lm = sqrt(L1**2 + L2**2 + 2 * L1 * L2 * cos(theta2m))
# print("Lm: "+str(Lm))
# compensation
thetaC = arccos((Lm**2 + L1**2 - L2**2) / (2 * L1 * Lm))
# print("thetaC: "+str(thetaC.to(deg)))
theta1 = theta - thetaC
# print("theta1: "+str(theta1.to(deg)))
tm1.position_control()
tm1.set_pos_setpoint(theta10 - ratioThigh * theta1)
tm2.position_control()
tm2.set_pos_setpoint(theta20 + ratioKnee * theta2)
elif activated:
# foot trajectory
Yfoot = E * cos(alpha)
Zfoot = F + D * sin(alpha)
# print("YFoot,Zfoot: ("+str(Yfoot)+','+str(Zfoot)+")")
# lenght and angle of leg
L = sqrt(Yfoot**2 + Zfoot**2)
L = maximum(minimum(L, LMAX * 1.0), LMAX * 0.3)
# print("L: "+str(L))
theta = arcsin(Yfoot / L)
theta = maximum(minimum(theta, 45 * deg), -45 * deg)
print("period:{:.1f}".format(period))
elif event.key == pygame.K_d:
period -= 0.1 * s
period = minimum(maximum(period, 0.1 * s), 5.0 * s)
print("period:{:.1f}".format(period))
elif event.key == pygame.K_m:
modepygame = 7
elif event.type == pygame.KEYUP:
modepygame = 0
if modepygame == 0:
tm.current_control()
tm.set_cur_setpoint(0.0 * A)
elif modepygame == 1:
position += step
tm.position_control()
tm.set_pos_setpoint(position)
elif modepygame == 2:
position -= step
tm.position_control()
tm.set_pos_setpoint(position)
elif modepygame == 3:
tm.position_control()
tm.set_pos_setpoint(position)
elif modepygame == 4:
position += sign * step
tm.position_control()
tm.set_pos_setpoint(position)
if sign * tm.Iq.estimate >= current_threshold:
count += 1
else:
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)))
