def update(self):
if self._anim is not None:
self._emotion = self._anim()
if self._anim.has_ended():
self._anim = None
phi = cmath.phase(self._emotion)
r = abs(self._emotion)
# Create a list of dummy values.
# None indicates that the servo at that index will not be updated.
servo_pos_us = [None for i in range(16)]
# Buffer to store all DOF values
#self.dof_values = {}
# (1) Calculate DOF positions using phi/r
# (2) This step also applies overlay functions to the DOFs
if self.isEmotion:
for dofname, dof in self.dofs.iteritems():
self.dof_values[dofname] = dof.calc(phi, r)
# (3) Update all servos
for servo in self.servos:
if servo.pin < 0 or servo.pin > 15:
continue # Skip invalid pins
if servo.dofname in self.dof_values:
servo_pos_us[servo.pin] = servo.dof_to_us(self.dof_values[servo.dofname])
# TODO: send values to hardware
with Hardware.lock:
Hardware.servo_set_all(servo_pos_us)
def servo_set_all(self, pos_list):
pos_list = list(pos_list.values())
Hardware.servo_set_all(post_list)
