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()
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 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 setup(self):
####################
#### EE SETUP ####
####################
# Motor object representing the left motor.
self.leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION,
LEFT_DRIVE_CONTROLLER_PWM)
# Encoder object for the left motor.
self.leftEncoder = Encoder(self.tamp, LEFT_DRIVE_ENCODER_YELLOW,
LEFT_DRIVE_ENCODER_WHITE)
# Motor object representing the right motor.
self.rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION,
RIGHT_DRIVE_CONTROLLER_PWM)
# Encoder object for the right motor.
self.rightEncoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW,
RIGHT_DRIVE_ENCODER_WHITE)
# Motor object representing the intake mechanism motors.
self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION,
HUGS_MOTOR_CONTROLLER_PWM)
# Encoder object for the intake motor.
self.intakeEncoder = Encoder(self.tamp, HUGS_MOTOR_ENCODER_YELLOW,
HUGS_MOTOR_ENCODER_WHITE)
# Motor object representing the conveyor belt motor.
self.conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION,
BELT_MOTOR_CONTROLLER_PWM)
# Encoder object for the conveyor belt motor.
self.conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW,
BELT_MOTOR_ENCODER_WHITE)
# Long range IR sensors
self.irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL)
self.irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR)
self.irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL)
self.irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR)
# Color sensor
self.color = Color(self.tamp)
# Limit switches
self.conveyorLimSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH)
self.blockLimSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH)
# Servo controlling the door of the collection chamber.
self.backDoorServo = Servo(self.tamp, SERVO_PIN)
# Make sure the door is closed
self.backDoorServo.write(SERVO_CLOSE)
# The switch that tells the program that the competition has started
self.competitionModeSwitch = DigitalInput(self.tamp, COMPETITION_MODE)
#################################
#### INTERNAL MODULE SETUP ####
#################################
# Timer object to moderate checking for intake errors.
self.intakeTimer = Timer()
# Are the intake motors reversing? True if so, False if going forwards.
self.intakeDirection = False
# Start the intake motor.
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
# Wall Follow object
self.wallFollow = WallFollow(self.leftMotor, self.rightMotor, Timer(),
self.irFL, self.irFR, self.irBL,
self.irBR)
# A timer to make sure timesteps are only 10 times/second
self.timestepTimer = Timer()
# Timer object describing how long the current module has been running.
self.moduleTimer = Timer()
# Timer for the whole game
self.gameTimer = Timer()
# Runs the PICKUP process
self.pickup = PickupModule(self.moduleTimer, self.conveyorLimSwitch,
self.conveyorMotor, self.conveyorEncoder)
# Runs the DROPOFF process
self.dropoff = DropoffModule(self.moduleTimer, self.backDoorServo,
self.rightMotor, self.leftMotor,
self.rightEncoder)
# Runs the FOLLOW process TODO: Fix forward to actually mean forward.
self.follow = FollowModule(self.moduleTimer, self.leftMotor,
self.rightMotor, self.intakeMotor,
self.wallFollow, FORWARD_SPEED,
self.blockLimSwitch)
# Runs the CHECK process. TODO: pass in proper timers.
self.check = CheckModule(self.moduleTimer, self.leftMotor,
self.rightMotor, self.intakeMotor, self.color)
# Waits for the game to start
self.off = OffModule(self.gameTimer, self.competitionModeSwitch)
# Describes which stage of the program is running.
self.module = MODULE_OFF
