Esempio n. 1
0
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()
Esempio n. 2
0
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()
Esempio n. 3
0
 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()
Esempio n. 4
0
 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()
Esempio n. 5
0
    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