class TestFollow(SyncedSketch):
def setup(self):
self.ttt = Timer()
timer = Timer()
stepTimer = Timer()
wallFollowTimer = Timer()
leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION,
LEFT_DRIVE_CONTROLLER_PWM)
rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION,
RIGHT_DRIVE_CONTROLLER_PWM)
intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION,
HUGS_MOTOR_CONTROLLER_PWM)
irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL)
irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR)
irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL)
irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR)
self.wallFollow = WallFollow(leftMotor, rightMotor, wallFollowTimer,
irFL, irFR, irBL, irBR)
blockSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH)
self.blockSwitch = DigitalInput(self.tamp, 21)
self.follow = FollowModule(timer, leftMotor, rightMotor, intakeMotor,
self.wallFollow, FORWARD_SPEED,
self.blockSwitch)
self.follow.start()
def loop(self):
response = self.follow.run()
if self.ttt.millis() > 100:
self.ttt.reset()
print self.wallFollow.distance()
if response != MODULE_FOLLOW:
self.stop()
class TestFollow(SyncedSketch):
def setup(self):
self.ttt = Timer()
timer = Timer()
stepTimer = Timer()
wallFollowTimer = Timer()
leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION, LEFT_DRIVE_CONTROLLER_PWM)
rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION, RIGHT_DRIVE_CONTROLLER_PWM)
intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM)
irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL)
irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR)
irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL)
irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR)
self.wallFollow = WallFollow(leftMotor, rightMotor, wallFollowTimer, irFL, irFR, irBL, irBR)
blockSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH)
self.blockSwitch = DigitalInput(self.tamp, 21)
self.follow = FollowModule(timer, leftMotor, rightMotor, intakeMotor, self.wallFollow, FORWARD_SPEED, self.blockSwitch)
self.follow.start()
def loop(self):
response = self.follow.run()
if self.ttt.millis() > 100:
self.ttt.reset()
print self.wallFollow.distance()
if response != MODULE_FOLLOW:
self.stop()
