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)
# 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))
img2 = font.render(text2, True, pygame.color.THECOLORS['red'])
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()
tm.set_pos_setpoint(position)
tm.reset()
elif event.key == pygame.K_SPACE:
print("balance")
modepygame = 2
elif event.type == pygame.KEYUP:
modepygame = 0
# recup motor sensors and convert to S.I.
theta = tm.encoder_estimates.position.to(rad) - theta0
thetap = tm.encoder_estimates.velocity.to(rad / s) - thetap0
nrj = 0.5 * thetap * thetap - w0 * w0 * cos(theta)
# test finite state machine
if modepygame == 0:
tm.current_control()
tm.set_cur_setpoint(0.0 * A)
elif modepygame == 1:
tm.velocity_control()
tm.set_vel_setpoint(0.0 * rad / s)
elif modepygame == 2:
tm.current_control()
error = nrj - (-nrj0 - 0.0 * rad * rad / (s * s))
torque = (-0.05 * thetap * error).magnitude
torque = maximum(minimum(torque, 10.0), -10.0)
# little kick at beginning
if abs(nrj - nrj0) < 1.0 * rad * rad / (s * s):
torque = 10.0
# print("{:.2f}\t>{:.1f}\t>{:.1f}\t>{:.1f}".format(torque,nrj,theta,thetap))
tm.set_cur_setpoint(torque)
if cos(tm.encoder_estimates.position.to(rad) - theta0) < -0.90:
modepygame = 3
