287 lines
8.0 KiB
Python
287 lines
8.0 KiB
Python
from servoclass import Servo
|
|
import time
|
|
|
|
current_pose = "pose_base"
|
|
current_state = "state_startup"
|
|
previous_state = None
|
|
animation_bool_a = False
|
|
animation_bool_b = False
|
|
last_toggle_a = time.ticks_ms()
|
|
last_toggle_b = time.ticks_ms()
|
|
new_state_flag = False
|
|
|
|
|
|
servos = {
|
|
"YAW": Servo(pin_num=6, max_speed=400, max_accel=100, min_angle=10, max_angle=170), #Base Yaw Rotation
|
|
"ROL": Servo(pin_num=7, max_speed=600, max_accel=400, min_angle=30, max_angle=120), #Neck Roll
|
|
"PIT": Servo(pin_num=8, max_speed=600, max_accel=400, min_angle=1, max_angle=80), #Neck Pitch
|
|
"MOU": Servo(pin_num=19, max_speed=100000, max_accel=3500, min_angle=5, max_angle=150), #Mouth
|
|
"EYL": Servo(pin_num=12, max_speed=200, max_accel=500, min_angle=30, max_angle=150), #Left Eyeball
|
|
"EYR": Servo(pin_num=13, max_speed=250, max_accel=500, min_angle=30, max_angle=150), #Right Eyeball
|
|
"LID": Servo(pin_num=14, max_speed=100000, max_accel=7000, min_angle=30, max_angle=160), #EyeLid
|
|
"EAL": Servo(pin_num=15, max_speed=250, max_accel=200, min_angle=60, max_angle=150), #Left Ear
|
|
"EAR": Servo(pin_num=16, max_speed=500, max_accel=200, min_angle=30, max_angle=120), #Right Ear
|
|
}
|
|
|
|
def apply_pose(pose):
|
|
pose = pose_map.get(pose, pose_calibrate)
|
|
for name, angle in pose.items():
|
|
if name in servos:
|
|
servos[name].set_target(angle)
|
|
|
|
def apply_state(state_name):
|
|
state_func = state_map.get(state_name)
|
|
if state_func:
|
|
state_func() # Call the function
|
|
else:
|
|
print(f"Unknown state: {state_name}")
|
|
|
|
#_________________# Poses (static servo positions used in states) #_________________#
|
|
|
|
pose_calibrate = { # Each dictionary key = servo name, value = angle
|
|
"YAW": 90,
|
|
"ROL": 90,
|
|
"PIT": 80,
|
|
"MOU": 170,
|
|
"LID": 110,
|
|
"EYL": 90,
|
|
"EYR": 90,
|
|
"EAL": 90,
|
|
"EAR": 90,
|
|
}
|
|
pose_sleep = { # Each dictionary key = servo name, value = angle
|
|
# "YAW": 88,
|
|
# "RWH": 89,
|
|
"ROL": 90,
|
|
"PIT": 80,
|
|
"MOU": 170,
|
|
"LID": 30,
|
|
# "EYL": 90,
|
|
# "EYR": 90,
|
|
"EAL": 150,
|
|
"EAR": 30,
|
|
}
|
|
|
|
pose_base = { # Each dictionary key = servo name, value = angle
|
|
# "YAW": 88,
|
|
# "RWH": 89,
|
|
"ROL": 90,
|
|
# "PIT": 20,
|
|
# "MOU": 170,
|
|
# "LID": 130,
|
|
# "EYL": 90,
|
|
# "EYR": 90,
|
|
"EAL": 130,
|
|
"EAR": 70,
|
|
}
|
|
|
|
pose_speaking = { # Each dictionary key = servo name, value = angle
|
|
# "YAW": 88,
|
|
# "RWH": 89,
|
|
# "ROL": 90,
|
|
# "PIT": 20,
|
|
"MOU": 10,
|
|
# "LID": 130,
|
|
# "EYL": 90,
|
|
# "EYR": 90,
|
|
"EAL": 130,
|
|
"EAR": 70,
|
|
}
|
|
|
|
pose_stop_speaking = { # Each dictionary key = servo name, value = angle
|
|
# "YAW": 88,
|
|
# "RWH": 89,
|
|
# "ROL": 90,
|
|
# "PIT": 20,
|
|
"MOU": 150,
|
|
# "LID": 130,
|
|
# "EYL": 90,
|
|
# "EYR": 90,
|
|
"EAL": 130,
|
|
"EAR": 70,
|
|
}
|
|
|
|
pose_thinking_1 = { # Each dictionary key = servo name, value = angle
|
|
# "YAW": 90,
|
|
# "RWH": 90,
|
|
"ROL": 130,
|
|
# "PIT": 50,
|
|
"MOU": 150,
|
|
# "LID": 70,
|
|
# "EYL": 90,
|
|
# "EYR": 90,
|
|
"EAL": 150,
|
|
"EAR": 120,
|
|
}
|
|
|
|
pose_curious_2 = { # Each dictionary key = servo name, value = angle
|
|
# "YAW": 90,
|
|
# "RWH": 90,
|
|
"ROL": 40,
|
|
# "PIT": 10,
|
|
"MOU": 160,
|
|
# "LID": 130,
|
|
# "EYL": 90,
|
|
# "EYR": 90,
|
|
"EAL": 60,
|
|
"EAR": 60,
|
|
}
|
|
|
|
pose_map={
|
|
"pose_calibrate": pose_calibrate,
|
|
"pose_base": pose_base,
|
|
"pose_thinking_1": pose_thinking_1,
|
|
"pose_curious_2": pose_curious_2,
|
|
"pose_sleep": pose_sleep,
|
|
"pose_speaking": pose_speaking,
|
|
"pose_stop_speaking": pose_stop_speaking,
|
|
}
|
|
|
|
#_________________# #_________________#
|
|
|
|
#_________________# States (Mix of poses + animations) #_________________#
|
|
|
|
|
|
|
|
def state_startup():
|
|
global new_state_flag
|
|
if new_state_flag == True:
|
|
apply_pose("pose_calibrate")
|
|
new_state_flag = False
|
|
|
|
def state_calibrate():
|
|
global new_state_flag
|
|
# if new_state_flag == True:
|
|
apply_pose("pose_calibrate")
|
|
new_state_flag = False
|
|
|
|
def state_sleep():
|
|
global new_state_flag
|
|
if new_state_flag == True:
|
|
apply_pose("pose_sleep")
|
|
new_state_flag = False
|
|
|
|
def state_speaking():
|
|
global new_state_flag
|
|
if new_state_flag == True:
|
|
apply_pose("pose_speaking")
|
|
new_state_flag = False
|
|
# if animation_bool_a == False:
|
|
# servos["MOU"].set_target(130)
|
|
# if animation_bool_a == True:
|
|
# servos["MOU"].set_target(10)
|
|
# if time.ticks_diff(now, last_toggle_a) >= 300:
|
|
# animation_bool_a = not animation_bool_a # flip the boolean
|
|
# last_toggle_a = now
|
|
|
|
def state_thinking():
|
|
now=time.ticks_ms()
|
|
global animation_bool_a, last_toggle_a, animation_bool_b, last_toggle_b, new_state_flag
|
|
if new_state_flag == True:
|
|
apply_pose("pose_base")
|
|
new_state_flag = False
|
|
if animation_bool_a == False:
|
|
servos["PIT"].set_target(50)
|
|
if animation_bool_a == True:
|
|
servos["PIT"].set_target(120)
|
|
if time.ticks_diff(now, last_toggle_a) >= 700:
|
|
animation_bool_a = not animation_bool_a # flip the boolean
|
|
last_toggle_a = now
|
|
|
|
|
|
def state_listen():
|
|
global new_state_flag
|
|
if new_state_flag == True:
|
|
apply_pose("pose_curious_2")
|
|
new_state_flag = False
|
|
|
|
def state_neutral():
|
|
global new_state_flag
|
|
if new_state_flag == True:
|
|
apply_pose("pose_stop_speaking")
|
|
new_state_flag = False
|
|
|
|
def state_happy():
|
|
now=time.ticks_ms()
|
|
global animation_bool_a, last_toggle_a, animation_bool_b, last_toggle_b, new_state_flag
|
|
if new_state_flag == True:
|
|
apply_pose("pose_base")
|
|
new_state_flag = False
|
|
# print("idle")
|
|
if animation_bool_a == False:
|
|
servos["ROL"].set_target(70)
|
|
if animation_bool_a == True:
|
|
servos["ROL"].set_target(110)
|
|
if time.ticks_diff(now, last_toggle_a) >= 2000:
|
|
animation_bool_a = not animation_bool_a # flip the boolean
|
|
last_toggle_a = now
|
|
|
|
if animation_bool_b == False:
|
|
servos["EAL"].set_target(100)
|
|
servos["EAR"].set_target(60)
|
|
if animation_bool_b == True:
|
|
servos["EAL"].set_target(160)
|
|
servos["EAR"].set_target(120)
|
|
if time.ticks_diff(now, last_toggle_b) >= 1000:
|
|
animation_bool_b = not animation_bool_b # flip the boolean
|
|
last_toggle_b = now
|
|
# print("Toggled:", animation_bool_a)
|
|
|
|
def state_limber_up():
|
|
now=time.ticks_ms()
|
|
global animation_bool_a, last_toggle_a, animation_bool_b, last_toggle_b, new_state_flag
|
|
if new_state_flag == True:
|
|
apply_pose("pose_base")
|
|
new_state_flag = False
|
|
# print("idle")
|
|
if animation_bool_a == False:
|
|
# servos["YAW"].set_target(50)
|
|
servos["ROL"].set_target(70)
|
|
servos["PIT"].set_target(70)
|
|
servos["EAL"].set_target(100)
|
|
servos["EYL"].set_target(60)
|
|
servos["MOU"].set_target(20)
|
|
servos["LID"].set_target(20)
|
|
servos["EYR"].set_target(60)
|
|
servos["EAL"].set_target(60)
|
|
servos["EAR"].set_target(60)
|
|
if animation_bool_a == True:
|
|
# servos["YAW"].set_target(130)
|
|
servos["ROL"].set_target(110)
|
|
servos["PIT"].set_target(5)
|
|
servos["EAL"].set_target(160)
|
|
servos["EYL"].set_target(120)
|
|
servos["MOU"].set_target(160)
|
|
servos["LID"].set_target(160)
|
|
servos["EYR"].set_target(120)
|
|
servos["EAL"].set_target(120)
|
|
servos["EAR"].set_target(120)
|
|
if time.ticks_diff(now, last_toggle_a) >= 1000:
|
|
animation_bool_a = not animation_bool_a # flip the boolean
|
|
last_toggle_a = now
|
|
|
|
# Note: system states:
|
|
# - idle: sleeping, wakeword not initiated listening mode
|
|
# - neutral: between finished speaking and begun listening. potentially use as marker for end of speech
|
|
# - listening
|
|
# - speaking
|
|
|
|
state_map={
|
|
"neutral": state_neutral,
|
|
"idle": state_sleep,
|
|
"listening": state_listen,
|
|
"state_startup": state_startup,
|
|
"thinking": state_thinking,
|
|
"speaking": state_speaking,
|
|
"state_limber_up": state_limber_up,
|
|
"happy": state_happy,
|
|
"state_calibrate": state_calibrate
|
|
}
|
|
|
|
#_________________# #_________________#
|
|
|
|
|
|
|
|
|
|
|