class BowPrimitive(pypot.primitive.LoopPrimitive): def __init__(self, robot, steps, freq=1000): self.robot = robot self.left_right_controller = EasingController(robot.left_right, SinusoidalEase, steps) self.left_right_controller.goal = 55 self.left_right_controller.__iter__() self.knee_controller = EasingController(robot.knee, SinusoidalEase, steps) self.knee_controller.goal = 25 self.knee_controller.__iter__() self.anticipate = True #hacky hack pypot.primitive.LoopPrimitive.__init__(self, robot, freq) # The update function is automatically called at the frequency given on the constructor def update(self): try: print("moves motor") self.robot.left_right.goal_position = self.left_right_controller.__next__() self.robot.knee.goal_position = self.knee_controller.__next__() except StopIteration: print("Stops iterator") if self.anticipate: self.left_right_controller.goal = 5 self.left_right_controller.__iter__() self.knee_controller.goal = -10 self.knee_controller.__iter__() self.anticipate = False else: self.stop()
class MainPositionPrimitive(pypot.primitive.LoopPrimitive): def __init__(self, robot, refresh_freq): self.robot = robot self.up_down_controller = EasingController(robot.up_down, SinusoidalEase) self.up_down_controller.goal = 0 self.up_down_controller.__iter__() self.left_right_controller = EasingController(robot.left_right, SinusoidalEase) self.left_right_controller.goal = 0 self.left_right_controller.__iter__() self.rotation_controller = EasingController(robot.rotation, SinusoidalEase) self.rotation_controller.goal = 0 self.rotation_controller.__iter__() self.knee_controller = EasingController(robot.knee, SinusoidalEase) self.knee_controller.goal = 25 self.knee_controller.__iter__() pypot.primitive.LoopPrimitive.__init__(self, robot, refresh_freq) # The update function is automatically called at the frequency given on the constructor def update(self): try: print("moves motor") self.robot.up_down.goal_position = self.up_down_controller.__next__( ) self.robot.left_right.goal_position = self.left_right_controller.__next__( ) self.robot.rotation.goal_position = self.rotation_controller.__next__( ) self.robot.knee.goal_position = self.knee_controller.__next__() except StopIteration: print("Stops iterator") self.stop()
class EasePrimitive(pypot.primitive.LoopPrimitive): def __init__(self, robot, steps, freq=0.5, goal=0): self.robot = robot self.freq = freq self.controller = EasingController(robot.left_right, BounceEase(), steps) self.controller.goal = goal self.controller.__iter__() pypot.primitive.LoopPrimitive.__init__(self, robot, freq) # The update function is automatically called at the frequency given on the constructor def update(self): try: print("moves motorrr") next_step = self.controller.__next__() self.robot.left_right.goal_position = next_step except StopIteration: print("Stops iterator") self.stop()
class GoToPositionPrimitive(pypot.primitive.LoopPrimitive): def __init__(self, robot, motor_id, pos, steps, freq=1000): self.robot = robot self.motor = getattr(self.robot, motor_id) self.freq = freq self.controller = EasingController(self.motor, BounceEase(), steps) #self.controller = EasingController(self.motor, BackEase(), steps) #self.controller = EasingController(self.motor, SinusoidalEase, steps) self.controller.goal = pos self.controller.__iter__() print('Go to posisiont: ', motor_id, pos) pypot.primitive.LoopPrimitive.__init__(self, robot, freq) # The update function is automatically called at the frequency given on the constructor def update(self): try: print("moves motor") next_step = self.controller.__next__() self.motor.goal_position = next_step except StopIteration: print("Stops iterator") self.stop()