def set(actions):
ch = servo.pins_to_timer_channels(PINS)
brain = servo.ServoBrain()
brain.init(ch, inversions=INV)
try:
while True:
brain.positions = actions
utime.sleep_ms(1000)
finally:
brain.deinit()
def zero():
ch = servo.pins_to_timer_channels(PINS)
brain = servo.ServoBrain()
brain.init(ch, inversions=INV)
try:
print(brain.positions)
brain.positions = [0.0, 0.0, 0.0, 0.0]
while True:
utime.sleep_ms(1000)
finally:
brain.deinit()
def stand():
ch = servo.pins_to_timer_channels(PINS)
brain = servo.ServoBrain()
brain.init(ch, inversions=INV)
try:
while True:
brain.positions = [0.0, 0.0, 0.0, 0.0]
utime.sleep_ms(1000)
brain.positions = [0.5, 0.5, 0.5, 0.5]
utime.sleep_ms(1000)
finally:
brain.deinit()
def cycle(pin):
ch = servo.pins_to_timer_channels(PINS)
brain = servo.ServoBrain()
brain.init(ch, inversions=INV)
v = 0.0
d = 0.1
try:
while True:
brain.positions = [
v if idx == pin else 0.0 for idx in range(len(PINS))
]
print(brain.positions)
utime.sleep_ms(1000)
v += d
if abs(v) >= 1.0 or v <= 0:
d = -d
finally:
brain.deinit()
def run(model_name):
model = do_import(model_name)
i2c = I2C(freq=400000, scl=SCL, sda=SDA)
acc = lis.Lis331DLH(i2c)
buf = SensorsBuffer([acc],
timer_index=1,
freq=100,
batch_size=10,
buffer_size=100)
post = PostPitchRoll(buf, pad_yaw=True)
buf.start()
ch = servo.pins_to_timer_channels(PINS)
brain = servo.ServoBrain()
brain.init(ch, inversions=INV)
obs = []
obs_len = STACK_OBS * (3 + 4)
frames = 0
frame_time = 0
ts = utime.ticks_ms()
try:
while True:
for v in post:
for n in brain.positions:
obs.append([n])
for n in v:
obs.append([n])
obs = obs[-obs_len:]
if len(obs) == obs_len:
frames += 1
frame_time += utime.ticks_diff(utime.ticks_ms(), ts)
ts = utime.ticks_ms()
res = model.forward(obs)
pos = [v[0] for v in res]
print("%s, FPS: %.3f" % (pos, frames * 1000 / frame_time))
brain.positions = pos
finally:
buf.stop()
brain.deinit()