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 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)
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
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
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
while goon:
tr0 = time.time()
# sensors