def __init__(self, wrapper, target=None):
Action.__init__(self, wrapper)
self.controller = PIDController(TURN_KP, TURN_KI, TURN_KD)
if target == None:
self.target = wrapper.color
else:
self.target = target
class ForwardToTarget(Action): # or GoForward
def __init__(self, wrapper, target=None):
Action.__init__(self, wrapper)
self.controller = PIDController(TURN_KP, TURN_KI, TURN_KD)
if target == None:
self.target = wrapper.color
else:
self.target = target
def run(self):
pass
# self.wrapper.right_motor.setSpeed(0)
# self.wrapper.left_motor.setSpeed(0)
def loop(self):
print "looping ", self.__class__.__name__
dist = self.wrapper.vs.getTargetDistFromCenter(self.target)
if dist == None:
return
# this will exit the ApproachBallState: lost the ball:`(
adjust = self.controller.adjust(dist[0] / 8)
new_left_speed = LEFT_FORWARD + LEFT_SIGN * adjust
# do I want to subtract from right motor too?
# scale so speeds <=126
new_right_speed = RIGHT_FORWARD - RIGHT_SIGN * adjust
multiplier = max(math.fabs(new_left_speed / 126.0), 1)
new_left_speed = int(new_left_speed / multiplier)
new_right_speed = int(new_right_speed / multiplier)
print "L/R speeds: ", new_left_speed, new_right_speed
self.wrapper[LEFT_MOTOR] = new_left_speed
self.wrapper[RIGHT_MOTOR] = new_right_speed
