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