Пример #1
0
    def execute(self):
        wpilib.SmartDashboard.putNumber("Auto State", self.currentState)

        if self.jsManip.getRawButton(5):
            self.initialize()

        if not (wpilib.DriverStation.getInstance().isAutonomous()
                or self.jsManip.getRawButton(8)):
            return None

        if self.currentState is not self.State.SHOOTING:
            pass
            #self.shooterSubsystem.idle()
            #self.shooterSubsystem.kickerOff()

        if self.currentState is self.State.SCANNING:
            self.shooterSubsystem.setArticulateAngle(config.angle_seek)
            k = vision.calculateShooterParams(self.shooterSubsystem.getAngle())
            if k is not None:
                self.currentState = self.State.AIMING
                return None
            self.driveSubsystem.drive(0, self.rotRate)

        elif self.currentState is self.State.AIMING:
            self.aim_value = wpilib.SmartDashboard.getNumber("Aim at", 155)
            self.do_aim = True

            k = vision.calculateShooterParams(self.shooterSubsystem.getAngle())
            if k is not None:
                pitch, yaw, dist, center_x = k
                self.shooterSubsystem.setArticulateAngle(pitch)
                wpilib.SmartDashboard.putNumber("Distance From Tower", dist)
                wpilib.SmartDashboard.putNumber("Shoot Angle", pitch)
                wpilib.SmartDashboard.putNumber("Azimuth", yaw)
                wpilib.SmartDashboard.putNumber("CenterX", center_x)
                if self.do_aim:
                    if center_x < self.aim_value - config.targeting_max_error and self.seekState < 2:
                        if self.seekCenterX != center_x:
                            self.seekCenterX = center_x
                            if self.seekState == 0:
                                self.seekState = 1
                                self.timer.reset()
                                self.timer.start()
                    elif center_x > self.aim_value + config.targeting_max_error and self.seekState < 2:
                        if self.seekCenterX != center_x:
                            self.seekCenterX = center_x
                            if self.seekState == 0:
                                self.seekState = 1
                                self.timer.reset()
                                self.timer.start()
                    elif self.aim_value - config.targeting_max_error <= \
                            center_x <= self.aim_value + config.targeting_max_error:
                        if self.seekState != 2:
                            self.seekState = 2
                            self.timer.stop()
                            self.timer.reset()
                            self.timer.start()
                    else:
                        self.seekState = 0
            else:
                wpilib.SmartDashboard.putNumber("CenterX", -1.1)
                self.seekCenterX = -1.1

            if self.do_aim:
                if self.seekState == 1:
                    if self.timer.get() > self.timer_var:
                        self.driveSubsystem.drive(0, 0)
                        if self.timer.get() > self.timer_var + .5:
                            self.seekState = 0
                            self.timer.stop()
                            self.timer.reset()
                    else:
                        # self.shooterSubsystem.setArticulateAngle(config.highgoal_outerworks_angle)
                        if self.seekCenterX < self.aim_value - 15:
                            if self.seekCenterX < 0:
                                self.timer_var = 0.1
                            else:
                                self.timer_var = 0.2
                            self.driveSubsystem.drive(0, -.5)
                        elif self.seekCenterX < self.aim_value - config.targeting_max_error:
                            self.timer_var = 0.1
                            self.driveSubsystem.drive(0, -.4)
                        elif self.seekCenterX > self.aim_value + 15:
                            self.timer_var = 0.2
                            self.driveSubsystem.drive(0, .5)
                        elif self.seekCenterX > self.aim_value + config.targeting_max_error:
                            self.timer_var = 0.1
                            self.driveSubsystem.drive(0, .4)
                        else:
                            self.driveSubsystem.drive(0, 0)
                if self.seekState == 2:
                    if self.timer.get() >= 1 and self.seekState == 2:
                        self.shooterSubsystem.spinUp()
                        if self.timer.get() >= 3.5 and self.seekState == 2:
                            self.shooterSubsystem.kickerOn()
                            if self.timer.get() >= 4:
                                self.timer.stop()
                                self.timer.reset()
                                self.timer.start()
                                self.currentState = self.State.DONE
                                self.shooterSubsystem.kickerOff()
                                self.shooterSubsystem.idle()
                                self.seekState = 3

            else:
                self.seekState = 0
                self.seekCenterX = -1.1
                self.timer.stop()
                self.timer.reset()

            if self.do_aim:
                self.seekCenterX = self.seekCenterX

        elif self.currentState is self.State.SHOOTING:
            pass
            self.shooterSubsystem.spinUp()
            if self.timer.get() > 3.5:
                self.shooterSubsystem.kickerOn()
                self.delayTime += 20 / 1000
                if self.delayTime >= 1:
                    self.currentState = self.State.DONE
                    self.timer.reset()
            else:
                self.shooterSubsystem.kickerOff()
        elif self.currentState is self.State.DONE:
            self.shooterSubsystem.idle()
            self.shooterSubsystem.kickerOff()
            if self.timer.get() > 1:
                self.currentState = self.State.SCANNING

        self.shooterSubsystem.updateSmartDashboardValues()
Пример #2
0
    def execute(self):
        super().execute()

        if not wpilib.DriverStation.getInstance().isOperatorControl(
        ) or self.jsManip.getRawButton(8):
            return None

        self.oneeightycd += 20 / 1000

        spencerPow = 1.0 if (self.jsLeft.getRawButton(1)
                             or self.jsRight.getRawButton(1)) else 0.75

        power = self.jsLeft.getY()**3 * spencerPow
        spin = self.jsRight.getX() * (1 if config.isPracticeBot else -1)
        if abs(spin) < 0.05:
            spin = 0

        if spencerPow != 1:
            spin *= config.spinFactor
        self.driveSubsystem.drive(power, spin)

        if self.jsLeft.getRawButton(7):
            self.driveSubsystem.resetEncoderCount()

        if self.jsManip.getRawButton(9):
            self.articulateAngle = config.articulateAngleHigh
        elif self.jsManip.getRawButton(10):
            self.articulateAngle = config.articulateAngleLow

        if self.jsManip.getPOV() == 0:
            self.tomahawkSubsystem.set(config.tomahawkPower)
        elif self.jsManip.getPOV() == 180:
            self.tomahawkSubsystem.set(-config.tomahawkPower)
        else:
            self.tomahawkSubsystem.set(0)

        if self.jsManip.getRawButton(6):
            self.shooterSubsystem.kickerOn()
        else:
            self.shooterSubsystem.kickerOff()

        if self.jsManip.getRawButton(7):
            self.shooterSubsystem.articulatePID.disable()
            self.shooterSubsystem.updateArticulate(self.jsManip.getY())
        else:
            self.shooterSubsystem.articulatePID.enable()

        if self.jsManip.getRawButton(1):
            self.shooterSubsystem.spinUp()
        elif self.jsManip.getRawButton(2):
            self.shooterSubsystem.eject()
        elif self.jsManip.getRawButton(3):
            self.shooterSubsystem.intake()
        elif self.jsManip.getRawButton(4):
            self.shooterSubsystem.spinUpBatter()
        elif self.jsManip.getRawButton(8):
            self.seekCenterX = self.seekCenterX
        else:
            self.shooterSubsystem.idle()

        articulatePow = (self.jsManip.getRawAxis(1) +
                         self.jsManip.getRawAxis(3)) / -2
        if abs(articulatePow) < 0.05:
            articulatePow = 0
        self.articulateAngle += articulatePow * config.articulateRate
        self.articulateAngle = clamp(self.articulateAngle,
                                     config.articulateAngleLow,
                                     config.articulateAngleHigh)

        self.shooterSubsystem.setArticulateAngle(self.articulateAngle)

        k = vision.calculateShooterParams(self.shooterSubsystem.getAngle())
        if k is not None:
            wpilib.SmartDashboard.putNumber("Shoot Angle", k[0])
            wpilib.SmartDashboard.putNumber("Distance From Tower", k[2])

        self.shooterSubsystem.updateSmartDashboardValues()
        self.driveSubsystem.updateSmartDashboardValues()
Пример #3
0
    def execute(self):
        wpilib.SmartDashboard.putNumber("Auto State", self.currentState)

        if self.jsManip.getRawButton(5):
            self.initialize()

        if not (wpilib.DriverStation.getInstance().isAutonomous() or self.jsManip.getRawButton(8)):
            return None

        if self.currentState is not self.State.SHOOTING:
            pass
            #self.shooterSubsystem.idle()
            #self.shooterSubsystem.kickerOff()

        if self.currentState is self.State.SCANNING:
            self.shooterSubsystem.setArticulateAngle(config.angle_seek)
            k = vision.calculateShooterParams(self.shooterSubsystem.getAngle())
            if k is not None:
                self.currentState = self.State.AIMING
                return None
            self.driveSubsystem.drive(0, self.rotRate)

        elif self.currentState is self.State.AIMING:
            self.aim_value = wpilib.SmartDashboard.getNumber("Aim at", 155)
            self.do_aim = True

            k = vision.calculateShooterParams(self.shooterSubsystem.getAngle())
            if k is not None:
                pitch, yaw, dist, center_x = k
                self.shooterSubsystem.setArticulateAngle(pitch)
                wpilib.SmartDashboard.putNumber("Distance From Tower", dist)
                wpilib.SmartDashboard.putNumber("Shoot Angle", pitch)
                wpilib.SmartDashboard.putNumber("Azimuth", yaw)
                wpilib.SmartDashboard.putNumber("CenterX", center_x)
                if self.do_aim:
                    if center_x < self.aim_value - config.targeting_max_error and self.seekState < 2:
                        if self.seekCenterX != center_x:
                            self.seekCenterX = center_x
                            if self.seekState == 0:
                                self.seekState = 1
                                self.timer.reset()
                                self.timer.start()
                    elif center_x > self.aim_value + config.targeting_max_error and self.seekState < 2:
                        if self.seekCenterX != center_x:
                            self.seekCenterX = center_x
                            if self.seekState == 0:
                                self.seekState = 1
                                self.timer.reset()
                                self.timer.start()
                    elif self.aim_value - config.targeting_max_error <= \
                            center_x <= self.aim_value + config.targeting_max_error:
                        if self.seekState != 2:
                            self.seekState = 2
                            self.timer.stop()
                            self.timer.reset()
                            self.timer.start()
                    else:
                        self.seekState = 0
            else:
                wpilib.SmartDashboard.putNumber("CenterX", -1.1)
                self.seekCenterX = -1.1

            if self.do_aim:
                if self.seekState == 1:
                    if self.timer.get() > self.timer_var:
                        self.driveSubsystem.drive(0, 0)
                        if self.timer.get() > self.timer_var + .5:
                            self.seekState = 0
                            self.timer.stop()
                            self.timer.reset()
                    else:
                        # self.shooterSubsystem.setArticulateAngle(config.highgoal_outerworks_angle)
                        if self.seekCenterX < self.aim_value - 15:
                            if self.seekCenterX < 0:
                                self.timer_var = 0.1
                            else:
                                self.timer_var = 0.2
                            self.driveSubsystem.drive(0, -.5)
                        elif self.seekCenterX < self.aim_value - config.targeting_max_error:
                            self.timer_var = 0.1
                            self.driveSubsystem.drive(0, -.4)
                        elif self.seekCenterX > self.aim_value + 15:
                            self.timer_var = 0.2
                            self.driveSubsystem.drive(0, .5)
                        elif self.seekCenterX > self.aim_value + config.targeting_max_error:
                            self.timer_var = 0.1
                            self.driveSubsystem.drive(0, .4)
                        else:
                            self.driveSubsystem.drive(0, 0)
                if self.seekState == 2:
                    if self.timer.get() >= 1 and self.seekState == 2:
                        self.shooterSubsystem.spinUp()
                        if self.timer.get() >= 3.5 and self.seekState == 2:
                            self.shooterSubsystem.kickerOn()
                            if self.timer.get() >= 4:
                                self.timer.stop()
                                self.timer.reset()
                                self.timer.start()
                                self.currentState = self.State.DONE
                                self.shooterSubsystem.kickerOff()
                                self.shooterSubsystem.idle()
                                self.seekState = 3

            else:
                self.seekState = 0
                self.seekCenterX = -1.1
                self.timer.stop()
                self.timer.reset()

            if self.do_aim:
                self.seekCenterX = self.seekCenterX

        elif self.currentState is self.State.SHOOTING:
            pass
            self.shooterSubsystem.spinUp()
            if self.timer.get() > 3.5:
                self.shooterSubsystem.kickerOn()
                self.delayTime += 20 / 1000
                if self.delayTime >= 1:
                    self.currentState = self.State.DONE
                    self.timer.reset()
            else:
                self.shooterSubsystem.kickerOff()
        elif self.currentState is self.State.DONE:
            self.shooterSubsystem.idle()
            self.shooterSubsystem.kickerOff()
            if self.timer.get() > 1:
                self.currentState = self.State.SCANNING

        self.shooterSubsystem.updateSmartDashboardValues()
Пример #4
0
    def execute(self):
        super().execute()

        if not wpilib.DriverStation.getInstance().isOperatorControl() or self.jsManip.getRawButton(8):
            return None

        self.oneeightycd += 20/1000

        spencerPow = 1.0 if (self.jsLeft.getRawButton(1) or self.jsRight.getRawButton(1)) else 0.75

        power = self.jsLeft.getY()**3 * spencerPow
        spin = self.jsRight.getX() * (1 if config.isPracticeBot else -1)
        if abs(spin) < 0.05:
            spin = 0

        if spencerPow != 1:
            spin *= config.spinFactor
        self.driveSubsystem.drive(power, spin)

        if self.jsLeft.getRawButton(7):
            self.driveSubsystem.resetEncoderCount()

        if self.jsManip.getRawButton(9):
            self.articulateAngle = config.articulateAngleHigh
        elif self.jsManip.getRawButton(10):
            self.articulateAngle = config.articulateAngleLow

        if self.jsManip.getPOV() == 0:
            self.tomahawkSubsystem.set(config.tomahawkPower)
        elif self.jsManip.getPOV() == 180:
            self.tomahawkSubsystem.set(-config.tomahawkPower)
        else:
            self.tomahawkSubsystem.set(0)

        if self.jsManip.getRawButton(6):
            self.shooterSubsystem.kickerOn()
        else:
            self.shooterSubsystem.kickerOff()

        if self.jsManip.getRawButton(7):
            self.shooterSubsystem.articulatePID.disable()
            self.shooterSubsystem.updateArticulate(self.jsManip.getY())
        else:
            self.shooterSubsystem.articulatePID.enable()

        if self.jsManip.getRawButton(1):
            self.shooterSubsystem.spinUp()
        elif self.jsManip.getRawButton(2):
            self.shooterSubsystem.eject()
        elif self.jsManip.getRawButton(3):
            self.shooterSubsystem.intake()
        elif self.jsManip.getRawButton(4):
            self.shooterSubsystem.spinUpBatter()
        elif self.jsManip.getRawButton(8):
            self.seekCenterX = self.seekCenterX
        else:
            self.shooterSubsystem.idle()

        articulatePow = (self.jsManip.getRawAxis(1) + self.jsManip.getRawAxis(3))/-2
        if abs(articulatePow) < 0.05:
            articulatePow = 0
        self.articulateAngle += articulatePow * config.articulateRate
        self.articulateAngle = clamp(self.articulateAngle, config.articulateAngleLow, config.articulateAngleHigh)

        self.shooterSubsystem.setArticulateAngle(self.articulateAngle)

        k = vision.calculateShooterParams(self.shooterSubsystem.getAngle())
        if k is not None:
            wpilib.SmartDashboard.putNumber("Shoot Angle", k[0])
            wpilib.SmartDashboard.putNumber("Distance From Tower", k[2])

        self.shooterSubsystem.updateSmartDashboardValues()
        self.driveSubsystem.updateSmartDashboardValues()