87 lines
2.5 KiB
Python
87 lines
2.5 KiB
Python
# motion_servo.py (MicroPython for RP2040)
|
|
from machine import Pin, PWM, UART
|
|
import time, random
|
|
import urandom
|
|
from servoclass import Servo
|
|
import sys, select, uselect
|
|
import math
|
|
import animation
|
|
from animation import servos
|
|
|
|
ESP = UART(1, baudrate=115200, tx=Pin(4), rx=Pin(5))
|
|
rx_buffer = b""
|
|
|
|
mode = Pin(20, Pin.IN, Pin.PULL_UP)
|
|
|
|
# track direction for each servo (1 = going to max, -1 = going to min)
|
|
directions = {name: 1 for name in servos.keys()}
|
|
|
|
last_time = time.ticks_ms()
|
|
last_switch = last_time
|
|
|
|
# servos["YAW"].set_target(88)
|
|
# # servos["RWH"].set_target(89)
|
|
|
|
|
|
while True:
|
|
now = time.ticks_ms()
|
|
dt = time.ticks_diff(now, last_time) / 1000.0
|
|
last_time = now
|
|
|
|
# # check if blink should finish
|
|
# update_blink(servos, now, lid="LID")
|
|
|
|
if ESP.any():
|
|
rx_buffer += ESP.read() # bytes + bytes = OK
|
|
while b"\n" in rx_buffer:
|
|
line, rx_buffer = rx_buffer.split(b"\n", 1)
|
|
rcvstate = line.decode().strip()
|
|
print("RX:", rcvstate)
|
|
if rcvstate in animation.state_map:
|
|
print("applying ", end="")
|
|
print(rcvstate)
|
|
animation.new_state_flag = True
|
|
animation.current_state = rcvstate
|
|
|
|
|
|
# if (mode.value() == 0):
|
|
# animation.apply_state("state_limber_up")
|
|
|
|
if (mode.value() == 1):
|
|
animation.apply_pose("pose_calibrate")
|
|
|
|
animation.apply_state(animation.current_state)
|
|
|
|
for s in servos.values():
|
|
s.update(dt)
|
|
time.sleep_ms(1)
|
|
|
|
# # EXAMPLE: randomly trigger a blink
|
|
# if not blink_state["active"] and (random.randint(0, 1000)<1):
|
|
# trigger_blink(servos, now, closed_angle=30, lid="LID")
|
|
|
|
# blink_state = {
|
|
# "active": False,
|
|
# "start_time": 0,
|
|
# "duration": 150, # ms lids stay closed
|
|
# "original_pos": None,
|
|
# }
|
|
#
|
|
#
|
|
#
|
|
# def trigger_blink(servos, now, closed_angle=30, lid="LID"):
|
|
# if blink_state["active"]:
|
|
# return # already blinking, ignore
|
|
# s = servos["LID"]
|
|
# blink_state["active"] = True
|
|
# blink_state["start_time"] = now
|
|
# blink_state["original_pos"] = s.target # remember current target
|
|
# s.set_target(s.min_angle) # snap to closed target
|
|
#
|
|
# def update_blink(servos, now, lid="LID"):
|
|
# if blink_state["active"]:
|
|
# if time.ticks_diff(now, blink_state["start_time"]) > blink_state["duration"]:
|
|
# s = servos[lid]
|
|
# s.set_target(blink_state["original_pos"]) # restore old target
|
|
# blink_state["active"] = False
|