diff --git a/src/main/java/frc2020/Robot.java b/src/main/java/frc2020/Robot.java index 3ddc6dc..9d22bf0 100644 --- a/src/main/java/frc2020/Robot.java +++ b/src/main/java/frc2020/Robot.java @@ -1,43 +1,20 @@ package frc2020; -import lib.geometry.Pose2d; -import lib.geometry.Rotation2d; -import lib.geometry.Translation2d; -import lib.wpilib.TimedRobot; -import lib.util.*; -//import lib.vision.AimingParameters; -import lib.loops.*; -import lib.subsystems.*; -import lib.SubsystemManager; -import edu.wpi.first.cameraserver.CameraServer; +import java.util.Optional; + import edu.wpi.first.wpilibj.Compressor; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Units; -import frc2020.subsystems.Drive; -import frc2020.subsystems.Intake; -import frc2020.subsystems.Inventory; -import frc2020.subsystems.Feeder; -import frc2020.subsystems.LED; -import frc2020.subsystems.Limelight; -import frc2020.subsystems.Pixy; -import frc2020.subsystems.RobotStateEstimator; -import frc2020.subsystems.Hood; -import frc2020.subsystems.Shooter; -import frc2020.subsystems.Superstructure; -import frc2020.subsystems.SwerveModule; import frc2020.auto.AutoModeExecutor; import frc2020.auto.modes.AutoModeBase; -import frc2020.hmi.HMI; -import frc2020.paths.TrajectoryRegistry; -import frc2020.statemachines.ClimberStateMachine; -import frc2020.statemachines.SuperstructureStateMachine; -import frc2020.statemachines.SuperstructureStateMachine.SystemState; -import frc2020.states.LEDState; - -import java.util.Optional; +import lib.SubsystemManager; +//import lib.vision.AimingParameters; +import lib.loops.Looper; +import lib.util.CrashTracker; +import lib.wpilib.IRobotContainer; +import lib.wpilib.TimedRobot; +/** + * Do not edit this class unless necessary. Edit {@link RobotContainer} instead. + */ public class Robot extends TimedRobot { private final Looper mEnabledLooper = new Looper(); @@ -45,34 +22,12 @@ public class Robot extends TimedRobot { private final SubsystemManager mSubsystemManager = SubsystemManager.getInstance(); - //Subsystems - - private final Drive mDrive = Drive.getInstance(); - - private final RobotStateEstimator mRobotStateEstimator = RobotStateEstimator.getInstance(); - - private final Intake mIntake = Intake.getInstance(); - private final Hood mHood = Hood.getInstance(); - private final Shooter mShooter = Shooter.getInstance(); - private final Feeder mFeeder = Feeder.getInstance(); - - private final Inventory mInventory = Inventory.getInstance(); - private final Superstructure mSuperstructure = Superstructure.getInstance(); - - private final Limelight mLimelight = Limelight.getInstance(); - private final LED mLED = LED.getInstance(); - private final Pixy mPixy = Pixy.getInstance(); - - private final Compressor mCompressor; - - private final HMI mHMI = HMI.getInstance(); - private AutoModeSelector mAutoModeSelector = new AutoModeSelector(); private AutoModeExecutor mAutoModeExecutor; - private TrajectoryRegistry mTrajectoryRegistry = TrajectoryRegistry.getInstance(); + private final Compressor mCompressor; - private boolean mCoastDrive = true; + private IRobotContainer mRobotContainer = new RobotContainer(); Robot() { CrashTracker.logRobotConstruction(); @@ -84,36 +39,15 @@ public void robotInit() { try { CrashTracker.logRobotInit(); - mSubsystemManager.setSubsystems( - mRobotStateEstimator, - mDrive, - mDrive.getSwerveModules()[0], - mDrive.getSwerveModules()[1], - mDrive.getSwerveModules()[2], - mDrive.getSwerveModules()[3], - mIntake, - mHood, - mShooter, - mFeeder, - mInventory, - mSuperstructure, - mLimelight, - mLED, - mPixy - ); + mSubsystemManager.setSubsystems(mRobotContainer.getSubsystems()); mSubsystemManager.registerEnabledLoops(mEnabledLooper); mSubsystemManager.registerDisabledLoops(mDisabledLooper); mAutoModeSelector.updateModeCreator(); - mTrajectoryRegistry.load("Slalom", "Barrel", "Bounce1", "RedA", "RedB"); - - if(!SmartDashboard.containsKey("Disable Shooter")) { - SmartDashboard.putBoolean("Disable Shooter", false); - } - - //CameraServer.getInstance().startAutomaticCapture(); + // ROBOT CONTAINER METHOD + mRobotContainer.robotInit(); } catch (Throwable t) { CrashTracker.logThrowableCrash(t); throw t; @@ -126,8 +60,6 @@ public void disabledInit() { mEnabledLooper.stop(); mCompressor.stop(); - mDrive.setBraked(true); - mHood.forceDisable(); // Reset all auto mode state. if (mAutoModeExecutor != null) { @@ -137,9 +69,10 @@ public void disabledInit() { mAutoModeSelector.updateModeCreator(); mAutoModeExecutor = new AutoModeExecutor(); - mDisabledLooper.start(); + // ROBOT CONTAINER METHOD + mRobotContainer.disabledInit(); - SmartDashboard.putBoolean("autoInit", false); + mDisabledLooper.start(); } catch (Throwable t) { CrashTracker.logThrowableCrash(t); throw t; @@ -151,8 +84,11 @@ public void autonomousInit() { try { CrashTracker.logAutoInit(); enabledInit(); + + // ROBOT CONTAINER METHOD + mRobotContainer.autonomousInit(); + mAutoModeExecutor.start(); - SmartDashboard.putBoolean("autoInit", true); } catch (Throwable t) { CrashTracker.logThrowableCrash(t); throw t; @@ -166,7 +102,9 @@ public void teleopInit() { mAutoModeExecutor.stop(); } - SmartDashboard.putBoolean("autoInit", true); + // ROBOT CONTAINER METHOD + mRobotContainer.teleopInit(); + enabledInit(); } catch (Throwable t) { CrashTracker.logThrowableCrash(t); @@ -178,12 +116,9 @@ public void enabledInit() { mDisabledLooper.stop(); mCompressor.start(); - mDrive.setBraked(true); - - mSuperstructure.setDisabled(false); - mDrive.setDisabled(false); - - mCoastDrive = false; + + // ROBOT CONTAINER METHOD + mRobotContainer.enabledInit(); mEnabledLooper.start(); } @@ -192,8 +127,9 @@ public void enabledInit() { public void testInit() { try { CrashTracker.logTestInit(); - testTimer.reset(); - testTimer.start(); + + // ROBOT CONTAINER METHOD + mRobotContainer.testInit(); enabledInit(); } catch (Throwable t) { CrashTracker.logThrowableCrash(t); @@ -207,38 +143,22 @@ public void robotPeriodic() { //Output to dashboard mSubsystemManager.outputToSmartDashboard(); mAutoModeSelector.outputToSmartDashboard(); + + // ROBOT CONTAINER METHOD + mRobotContainer.robotPeriodic(); } catch (Throwable t) { CrashTracker.logThrowableCrash(t); throw t; } } - public static boolean disableShooter = false; @Override public void disabledPeriodic() { try { mAutoModeSelector.updateModeCreator(); - //Reset Field to Vehicle - mRobotStateEstimator.resetOdometry(); - - mSuperstructure.resetStateMachine(); - - intakeOn = false; - aimManual = false; - enableFlywheel = false; - - disableShooter = SmartDashboard.getBoolean("Disable Shooter", false); - - if(mDrive.getLinearVelocity() < Units.inchesToMeters(5)) { - mCoastDrive = true; - } - mDrive.setBraked(!mCoastDrive); - for(SwerveModule swerveModule : mDrive.getSwerveModules()) { - swerveModule.resetOffset(); - } - - mHood.forceDisable(); + // ROBOT CONTAINER METHOD + mRobotContainer.disabledPeriodic(); Optional autoMode = mAutoModeSelector.getAutoMode(); if (autoMode.isPresent() && autoMode.get() != mAutoModeExecutor.getAutoMode()) { @@ -253,301 +173,24 @@ public void disabledPeriodic() { @Override public void autonomousPeriodic() { - mDrive.setBraked(true); + // ROBOT CONTAINER METHOD + mRobotContainer.testPeriodic(); } @Override public void teleopPeriodic() { try { - manualControl(); + // ROBOT CONTAINER METHOD + mRobotContainer.teleopPeriodic(); } catch (Throwable t) { CrashTracker.logThrowableCrash(t); throw t; } } - //Degrees that all shot altitudes change by when the operator wants to increments or decrements it - private static double deltaAltitudeOffset = 1; - - private LatchedBoolean lToggleCollect = new LatchedBoolean(); - private LatchedBoolean lBlowOff = new LatchedBoolean(); - private LatchedBoolean lAltitudeInc = new LatchedBoolean(); - private LatchedBoolean lAltitudeDec = new LatchedBoolean(); - - private boolean intakeOn = false; - private boolean aimManual = false; - private boolean enableFlywheel = false; - - double lastTimestamp = Timer.getFPGATimestamp(); - public void manualControl() { - double timestamp = Timer.getFPGATimestamp(); - double dt = timestamp - lastTimestamp; - lastTimestamp = timestamp; - - //Update latched buttons - boolean toggleCollect = lToggleCollect.update(mHMI.getToggleCollect()); - boolean blowOff = lBlowOff.update(!mHMI.getBlow()); - boolean altitudeInc = lAltitudeInc.update(mHMI.getAltitudeInc()); - boolean altitudeDec = lAltitudeDec.update(mHMI.getAltitudeDec()); - - //Shooting location - ShootingLocation.Location shootingLocation = mHMI.getShootingLocation(); - boolean isShootingLocation = shootingLocation != ShootingLocation.Location.NONE; - if(isShootingLocation) { - mSuperstructure.setWantedShootingLocation(shootingLocation); - } else if(mHMI.getClearShot()) { - mSuperstructure.setWantedShootingLocation(ShootingLocation.Location.NONE); - } - - //Drive - boolean search = mSuperstructure.systemStateIsLimelight() && - (mHood.getAngle() > 45.0 || shootingLocation == ShootingLocation.Location.GREEN); - mDrive.setTeleOpInputs( - mHMI.getThrottle(), - mHMI.getStrafe(), - mHMI.getSteer(), - false, - mHMI.getDriver().getAButton(), - mHMI.getDriver().getYButton() || mSuperstructure.getSystemState() == SystemState.SHOOT, - mHMI.getDriver().getLeftTriggerBoolean()); - if(mHMI.getDriver().getBButton()) mRobotStateEstimator.resetOdometry(); - - if(isShootingLocation) { - enableFlywheel = true; - } else if(mHMI.getClearShot()) { - mSuperstructure.setWantIdle(); - enableFlywheel = false; - aimManual = false; - } - - // ----- Superstructure Wanted Action ------ - // ----- Intake ----- - // Automatically toggle intake off once there are 3 balls - if(mSuperstructure.getSystemState() == SuperstructureStateMachine.SystemState.INTAKE_FINISH && - intakeOn - ) { - mSuperstructure.setWantIdle(); - intakeOn = false; - } - if(toggleCollect && !enableFlywheel) { - if(intakeOn) { - mSuperstructure.setWantIdle(); - intakeOn = false; - } else { - mSuperstructure.setWantIntakeOn(); - intakeOn = true; - } - } else { - // ----- Blow ----- - if(mHMI.getBlow()) { - mSuperstructure.setWantBlow(); - } else if(blowOff) { - //This handles what happens when the blow button is released, depending on if intake is toggeled or not. - if(intakeOn) { - //Return to intaking if intake is still toggled on - mSuperstructure.setWantIntakeOn(); - } else { - //Otherwise just turn off blow - mSuperstructure.setWantIdle(); - } - } else { - // ----- Aim ----- - if(enableFlywheel && !mHMI.getAim()) { - mSuperstructure.setWantEnableFlywheel(); - intakeOn = false; - } else if(enableFlywheel && mHMI.getAim()) { - if(!mHMI.getShoot()) { - /*if((mHMI.getTurretManual() != 0 || aimManual) && mHMI.getAim()) { - mSuperstructure.setWantAimManual(); - aimManual = true; - } else */{ - mSuperstructure.setWantAimLimelight(); - } - } else { - // ----- Shoot ----- - mSuperstructure.setWantShoot(); - } - } - } - } - - //Altitude offset - if(altitudeInc) { - //Increment - mSuperstructure.changeAltitudeOffset(deltaAltitudeOffset); - } else if(altitudeDec) { - //Decrement - mSuperstructure.changeAltitudeOffset(-deltaAltitudeOffset); - } - - //Manual brush - if(mHMI.getBrushForward()) { - mSuperstructure.setBrushOverride(false); - } else if(mHMI.getBrushBackward()) { - mSuperstructure.setBrushOverride(true); - } else { - mSuperstructure.stopBrushOverride(); - } - - if(mHMI.getDriver().getPOVData().getDown()) { - mSuperstructure.setIntakeDeployOverride(true); - } else { - mSuperstructure.stopIntakeDeployOverride(); - } - - /* - //Climber - ClimberStateMachine.WantedAction climberWantedAction = ClimberStateMachine.WantedAction.STOP; - if(mHMI.getMastUp()) { - climberWantedAction = ClimberStateMachine.WantedAction.MAST_UP; - } else if(mHMI.getMastDown()) { - climberWantedAction = ClimberStateMachine.WantedAction.MAST_DOWN; - } else if(mHMI.getWinchIn()) { - climberWantedAction = ClimberStateMachine.WantedAction.WINCH_IN; - } else if(mHMI.getWinchOut()) { - climberWantedAction = ClimberStateMachine.WantedAction.WINCH_OUT; - } - mClimber.setWantedAction(climberWantedAction); - - // LEDs - - // TODO clean up - /* - if(mClimber.getPartyMode()) { - mLED.setState(Color.WHITE, 0, true); - } else { - if(mSuperstructure.systemStateIsIntaking()) { - mLED.setState(Color.WHITE, mInventory.getBallCount(), false); - } else { - if(mSuperstructure.getSystemState() == SystemState.ENABLE_FLYWHEEL) { - mLED.setState(Color.BLUE, 5, false); - } else { - if(mSuperstructure.getSystemState() == SystemState.AIM_LIGHTLIGHT && - mSuperstructure.getAtRPM() - ) { - if(Math.abs(mDrive.getAutoSteerError()) <= 0.00827266) { - mLED.setState(Color.GREEN, 5, false); - } else { - mLED.setState(Color.BLUE, 5, false); - } - } - } - } - } - */ - } - - Timer testTimer = new Timer(); - LatchedBoolean testLatch1 = new LatchedBoolean(); - LatchedBoolean testLatch2 = new LatchedBoolean(); - boolean indexing = false; - int count = 0; @Override public void testPeriodic() { - mSuperstructure.setDisabled(true); - mDrive.setDisabled(true); - - mIntake.setIntakeDeploy(true); - - /* - mDrive.setTeleOpInputs( - mHMI.getThrottle(), - mHMI.getStrafe(), - mHMI.getSteer(), - mHMI.getDriver().getRightTriggerBoolean(), - mHMI.getDriver().getAButton(), - mHMI.getDriver().getYButton(), - mHMI.getDriver().getLeftTriggerBoolean() - ); - */ - - /* - if(Math.abs(mHMI.getHoodManual()) >= 0.2) { - mHood.setPercent(mHMI.getHoodManual()); - } else { - mHood.setAngle(40); - } - */ - - //mShooter.setDemand(11); - - //mShooter.setTargetRPM(9000); - //mFeeder.setDemand(12); - //mIntake.setIntake(8); - //mIntake.setIntakeDeploy(false); - - //mIntake.setIntakeDeploy(true); - //mIntake.setIntake(10); - - //var trans = new Translation2d(mHMI.getThrottle(), mHMI.getStrafe()); - //SmartDashboard.putNumber("Joystick Norm", trans.norm()); - - /* - mIntake.setIntake(10); - - var a = testLatch1.update(mInventory.getSensorValues()[0]); - var b = testLatch2.update(!mInventory.getSensorValues()[0]); - if(a) { - count++; - if(count < 3) indexing = true; - } - if(b) { - testTimer.reset(); - } - if((!mInventory.getSensorValues()[0] && testTimer.get() >= 0.100) || count >= 3) { - indexing = false; - } - - if(mHMI.getDriver().getBButton()) { - mIntake.setIndexer(-3); - } else if(indexing && mHMI.getDriver().getXButton()) { - mIntake.setIndexer(3); - } else { - mIntake.setIndexer(0); - } - */ - - - /* - mIntake.setIntakeDeploy(true); - mIntake.setIntake(10); - if(mHMI.getDriver().getAButton()) { - mIntake.setIndexer(8); - } else { - mIntake.setIndexer(0); - } - mFeeder.setDemand(12); - mShooter.setDemand(9.4); - */ - - var g = mHMI.getDriver(); - - /* - int index = 0; - if(g.getXButton()) { - index = 3; - } else if(g.getYButton()) { - index = 0; - } else if(g.getAButton()) { - index = 2; - } else if(g.getBButton()) { - index = 1; - } - - for(int i = 0; i < 4; i++) { - if(i == index) { - mDrive.getSwerveModules()[i].setSteerVoltage(g.getLeftStickX() * 4); - } else { - mDrive.getSwerveModules()[i].setSteerVoltage(0); - } - } - */ - - //mDrive.getSwerveModules()[0].setVoltage(12); - //mDrive.getSwerveModules()[0].setVelocity(80); - - //mDrive.getSwerveModules()[1].setSteerVoltage(5); - //mDrive.getSwerveModules()[3].setSteerVoltage(5); - //mDrive.getSwerveModules()[3].setAngle(g.getLeftStickX() * 180); + // ROBOT CONTAINER METHOD + mRobotContainer.testPeriodic(); } } \ No newline at end of file diff --git a/src/main/java/frc2020/RobotContainer.java b/src/main/java/frc2020/RobotContainer.java new file mode 100644 index 0000000..599f688 --- /dev/null +++ b/src/main/java/frc2020/RobotContainer.java @@ -0,0 +1,304 @@ +package frc2020; + +import java.util.Arrays; +import java.util.List; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Units; +import frc2020.hmi.HMI; +import frc2020.subsystems.*; +import frc2020.statemachines.*; +import frc2020.statemachines.SuperstructureStateMachine.SystemState; +import lib.subsystems.Subsystem; +import lib.util.LatchedBoolean; +import lib.wpilib.IRobotContainer; +import frc2020.paths.TrajectoryRegistry; + +public class RobotContainer implements IRobotContainer { + + private final Drive mDrive = Drive.getInstance(); + + private final RobotStateEstimator mRobotStateEstimator = RobotStateEstimator.getInstance(); + + private final Intake mIntake = Intake.getInstance(); + private final Hood mHood = Hood.getInstance(); + private final Shooter mShooter = Shooter.getInstance(); + private final Feeder mFeeder = Feeder.getInstance(); + + private final Inventory mInventory = Inventory.getInstance(); + private final Superstructure mSuperstructure = Superstructure.getInstance(); + + private final Limelight mLimelight = Limelight.getInstance(); + private final LED mLED = LED.getInstance(); + private final Pixy mPixy = Pixy.getInstance(); + + private final TrajectoryRegistry mTrajectoryRegistry = TrajectoryRegistry.getInstance(); + private final HMI mHMI = HMI.getInstance(); + + private boolean mCoastDrive = true; + + public Subsystem[] getSubsystems() { + var subystems = new Subsystem[] { + mRobotStateEstimator, + mDrive, + mDrive.getSwerveModules()[0], + mDrive.getSwerveModules()[1], + mDrive.getSwerveModules()[2], + mDrive.getSwerveModules()[3], + mIntake, + mHood, + mShooter, + mFeeder, + mInventory, + mSuperstructure, + mLimelight, + mLED, + mPixy + }; + return subystems; + } + + /** + * Robot-wide initialization code should go here. + */ + public void robotInit() { + mTrajectoryRegistry.load("Slalom", "Barrel", "Bounce1", "RedA", "RedB"); + + if(!SmartDashboard.containsKey("Disable Shooter")) { + SmartDashboard.putBoolean("Disable Shooter", false); + } + + //CameraServer.getInstance().startAutomaticCapture(); + } + + /** + * Initialization code for disabled mode should go here. + */ + public void disabledInit() { + mDrive.setBraked(true); + mHood.forceDisable(); + + SmartDashboard.putBoolean("autoInit", false); + } + + /** + * Initialization code for autonomous mode should go here. + */ + public void autonomousInit() { + + } + + /** + * Initialization code for teleop mode should go here. + */ + public void teleopInit() { + + } + + /** + * Initialization code for test mode should go here. + */ + public void testInit() { + + } + + /** + * Initalization code for any mode (other than disabled) should go here. + */ + public void enabledInit() { + SmartDashboard.putBoolean("autoInit", true); + + mDrive.setBraked(true); + + mSuperstructure.setDisabled(false); + mDrive.setDisabled(false); + + mCoastDrive = false; + } + + /** + * Periodic code for all robot modes should go here. + */ + public void robotPeriodic() { + + } + + /** + * Periodic code for disabled mode should go here. + */ + public void disabledPeriodic() { + //Reset Field to Vehicle + mRobotStateEstimator.resetOdometry(); + + mSuperstructure.resetStateMachine(); + + intakeOn = false; + aimManual = false; + enableFlywheel = false; + + boolean disableShooter = SmartDashboard.getBoolean("Disable Shooter", false); + mSuperstructure.setDisableShooter(disableShooter); + + if(mDrive.getLinearVelocity() < Units.inchesToMeters(5)) { + mCoastDrive = true; + } + mDrive.setBraked(!mCoastDrive); + for(SwerveModule swerveModule : mDrive.getSwerveModules()) { + swerveModule.resetOffset(); + } + + mHood.forceDisable(); + } + + /** + * Periodic code for autonomous mode should go here. + */ + public void autonomousPeriodic() { + mDrive.setBraked(true); + } + + /** + * Periodic code for teleop mode should go here. + */ + public void teleopPeriodic() { + manualControl(); + } + + /** + * Periodic code for test mode should go here. + */ + public void testPeriodic() { + + } + + //Degrees that all shot altitudes change by when the operator wants to increments or decrements it + private static double deltaAltitudeOffset = 1; + + private LatchedBoolean lToggleCollect = new LatchedBoolean(); + private LatchedBoolean lBlowOff = new LatchedBoolean(); + private LatchedBoolean lAltitudeInc = new LatchedBoolean(); + private LatchedBoolean lAltitudeDec = new LatchedBoolean(); + + private boolean intakeOn = false; + private boolean aimManual = false; + private boolean enableFlywheel = false; + + double lastTimestamp = Timer.getFPGATimestamp(); + public void manualControl() { + double timestamp = Timer.getFPGATimestamp(); + double dt = timestamp - lastTimestamp; + lastTimestamp = timestamp; + + //Update latched buttons + boolean toggleCollect = lToggleCollect.update(mHMI.getToggleCollect()); + boolean blowOff = lBlowOff.update(!mHMI.getBlow()); + boolean altitudeInc = lAltitudeInc.update(mHMI.getAltitudeInc()); + boolean altitudeDec = lAltitudeDec.update(mHMI.getAltitudeDec()); + + //Shooting location + ShootingLocation.Location shootingLocation = mHMI.getShootingLocation(); + boolean isShootingLocation = shootingLocation != ShootingLocation.Location.NONE; + if(isShootingLocation) { + mSuperstructure.setWantedShootingLocation(shootingLocation); + } else if(mHMI.getClearShot()) { + mSuperstructure.setWantedShootingLocation(ShootingLocation.Location.NONE); + } + + //Drive + boolean search = mSuperstructure.systemStateIsLimelight() && + (mHood.getAngle() > 45.0 || shootingLocation == ShootingLocation.Location.GREEN); + mDrive.setTeleOpInputs( + mHMI.getThrottle(), + mHMI.getStrafe(), + mHMI.getSteer(), + false, + mHMI.getDriver().getAButton(), + mHMI.getDriver().getYButton() || mSuperstructure.getSystemState() == SystemState.SHOOT, + mHMI.getDriver().getLeftTriggerBoolean()); + if(mHMI.getDriver().getBButton()) mRobotStateEstimator.resetOdometry(); + + if(isShootingLocation) { + enableFlywheel = true; + } else if(mHMI.getClearShot()) { + mSuperstructure.setWantIdle(); + enableFlywheel = false; + aimManual = false; + } + + // ----- Superstructure Wanted Action ------ + // ----- Intake ----- + // Automatically toggle intake off once there are 3 balls + if(mSuperstructure.getSystemState() == SuperstructureStateMachine.SystemState.INTAKE_FINISH && + intakeOn + ) { + mSuperstructure.setWantIdle(); + intakeOn = false; + } + if(toggleCollect && !enableFlywheel) { + if(intakeOn) { + mSuperstructure.setWantIdle(); + intakeOn = false; + } else { + mSuperstructure.setWantIntakeOn(); + intakeOn = true; + } + } else { + // ----- Blow ----- + if(mHMI.getBlow()) { + mSuperstructure.setWantBlow(); + } else if(blowOff) { + //This handles what happens when the blow button is released, depending on if intake is toggeled or not. + if(intakeOn) { + //Return to intaking if intake is still toggled on + mSuperstructure.setWantIntakeOn(); + } else { + //Otherwise just turn off blow + mSuperstructure.setWantIdle(); + } + } else { + // ----- Aim ----- + if(enableFlywheel && !mHMI.getAim()) { + mSuperstructure.setWantEnableFlywheel(); + intakeOn = false; + } else if(enableFlywheel && mHMI.getAim()) { + if(!mHMI.getShoot()) { + /*if((mHMI.getTurretManual() != 0 || aimManual) && mHMI.getAim()) { + mSuperstructure.setWantAimManual(); + aimManual = true; + } else */{ + mSuperstructure.setWantAimLimelight(); + } + } else { + // ----- Shoot ----- + mSuperstructure.setWantShoot(); + } + } + } + } + + //Altitude offset + if(altitudeInc) { + //Increment + mSuperstructure.changeAltitudeOffset(deltaAltitudeOffset); + } else if(altitudeDec) { + //Decrement + mSuperstructure.changeAltitudeOffset(-deltaAltitudeOffset); + } + + //Manual brush + if(mHMI.getBrushForward()) { + mSuperstructure.setBrushOverride(false); + } else if(mHMI.getBrushBackward()) { + mSuperstructure.setBrushOverride(true); + } else { + mSuperstructure.stopBrushOverride(); + } + + if(mHMI.getDriver().getPOVData().getDown()) { + mSuperstructure.setIntakeDeployOverride(true); + } else { + mSuperstructure.stopIntakeDeployOverride(); + } + } +} diff --git a/src/main/java/frc2020/subsystems/Superstructure.java b/src/main/java/frc2020/subsystems/Superstructure.java index 6d44872..ffe6808 100644 --- a/src/main/java/frc2020/subsystems/Superstructure.java +++ b/src/main/java/frc2020/subsystems/Superstructure.java @@ -10,6 +10,7 @@ import frc2020.statemachines.SuperstructureStateMachine; import frc2020.statemachines.SuperstructureStateMachine.SystemState; import frc2020.Robot; +import frc2020.RobotContainer; import frc2020.RobotState; import lib.loops.ILooper; import lib.loops.Loop; @@ -38,7 +39,8 @@ public class Superstructure extends Subsystem { private double mBrushOverride = 0; private Optional mHoodAngleOverride = Optional.empty(); - private boolean disabled = false; + private boolean mDisabled = false; + private boolean mDisableShooter = false; private boolean mAtRPM = false; @@ -69,7 +71,7 @@ public void onStart(final double timestamp) { @Override public void onLoop(final double timestamp) { synchronized (Superstructure.this) { - if (!disabled) { + if (!mDisabled) { // This is needed so that the inventory knows if it should increment the ball // counter mInventory.setIntaking(systemStateIsIntaking()); @@ -122,7 +124,7 @@ public void onLoop(final double timestamp) { if (isShooterVoltage && isShooterRPM) { throw new Exception("SuperstructureStateMachine exception: cannot set shooter voltage and RPM at the same time. One or both must be equal to Double.NaN"); } - if(Robot.disableShooter || (!isShooterVoltage && !isShooterRPM)) { + if(mDisableShooter || (!isShooterVoltage && !isShooterRPM)) { mShooter.setDemand(0); } else if(isShooterVoltage) { mShooter.setDemand(newState.shooterVoltage); @@ -154,7 +156,11 @@ public boolean getAtRPM() { } public void setDisabled(boolean disabled) { - this.disabled = disabled; + mDisabled = disabled; + } + + public void setDisableShooter(boolean disableShooter) { + mDisableShooter = disableShooter; } public synchronized void resetStateMachine() { diff --git a/src/main/java/lib/wpilib/IRobotContainer.java b/src/main/java/lib/wpilib/IRobotContainer.java new file mode 100644 index 0000000..937e611 --- /dev/null +++ b/src/main/java/lib/wpilib/IRobotContainer.java @@ -0,0 +1,18 @@ +package lib.wpilib; + +import lib.subsystems.Subsystem; + +public interface IRobotContainer { + Subsystem[] getSubsystems(); + void robotInit(); + void disabledInit(); + void autonomousInit(); + void teleopInit(); + void testInit(); + void enabledInit(); + void robotPeriodic(); + void disabledPeriodic(); + void autonomousPeriodic(); + void teleopPeriodic(); + void testPeriodic(); +}