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)
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
# keyboard
for event in pygame.event.get():
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:
goon = False
# 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
for event in pygame.event.get():
if event.type == pygame.QUIT:
goon = False
elif event.type == pygame.KEYDOWN:
