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)