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