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)
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
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)
else:
tm1.current_control()
tm1.set_cur_setpoint(0.0 * A)
tm2.current_control()
tm2.set_cur_setpoint(0.0 * A)
print(time.time() - tr0)
# display
text1 = "Current : {:.2f}||{:.2f}".format(tm1.Iq.estimate,
tm2.Iq.estimate)
img1 = font.render(text1, True, pygame.color.THECOLORS['red'])
rect1 = img1.get_rect()
pygame.draw.rect(img1, pygame.color.THECOLORS['blue'], rect1, 1)
text2 = "Position : {:.0f}||{:.0f}".format(
tm1.encoder_estimates.position.to(deg),
tm2.encoder_estimates.position.to(deg))
print("step:{:.0f}".format(step))
elif event.key == pygame.K_e:
period += 0.1 * s
period = minimum(maximum(period, 0.1 * s), 5.0 * s)
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()
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)))
