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()
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()