diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 13e454ce..17a10218 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -67,7 +67,6 @@ import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem; import frc.robot.subsystems.shooter.ShooterPivotSubsystem; import frc.robot.subsystems.superstructure.LightBarStatus; -import frc.robot.subsystems.superstructure.SuperstructureSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.util.ConditionalWaitCommand; import frc.robot.util.GRTUtil; @@ -90,7 +89,6 @@ public class RobotContainer { private final FieldManagementSubsystem fmsSubsystem; private final LightBarSubsystem lightBarSubsystem; - private final SuperstructureSubsystem superstructureSubsystem; private final SendableChooser autonPathChooser; private final AutonBuilder autonBuilder; @@ -127,12 +125,6 @@ public class RobotContainer { ChoreoTrajectory trajectory; - private PIDController xPID; - private PIDController yPID; - - private final GenericEntry xError; - private final GenericEntry yError; - private final ShuffleboardTab swerveCrauton; private double shooterPivotSetPosition = Units.degreesToRadians(18); @@ -148,14 +140,12 @@ public class RobotContainer { */ public RobotContainer() { fmsSubsystem = new FieldManagementSubsystem(); - lightBarSubsystem = new LightBarSubsystem(); - superstructureSubsystem = new SuperstructureSubsystem(lightBarSubsystem, fmsSubsystem); swerveSubsystem = new SwerveSubsystem(fmsSubsystem::isRedAlliance); swerveSubsystem.setVerbose(false); // SET THIS TO true FOR TUNING VALUES intakePivotSubsystem = new IntakePivotSubsystem(); - intakeRollerSubsystem = new IntakeRollerSubsystem(lightBarSubsystem); + intakeRollerSubsystem = new IntakeRollerSubsystem(); shooterPivotSubsystem = new ShooterPivotSubsystem( swerveSubsystem::getRobotPosition, @@ -166,20 +156,17 @@ public RobotContainer() { fmsSubsystem::isRedAlliance ); + lightBarSubsystem = new LightBarSubsystem(intakeRollerSubsystem, + shooterFlywheelSubsystem, + fmsSubsystem); elevatorSubsystem = new ElevatorSubsystem(); climbSubsystem = new ClimbSubsystem(); noteDetector = new NoteDetectionWrapper(NOTE_CAMERA); - xPID = new PIDController(4, 0, 0); - yPID = new PIDController(4, 0, 0); - swerveCrauton = Shuffleboard.getTab("Auton"); - xError = swerveCrauton.add("xError", 0).withPosition(8, 0).getEntry(); - yError = swerveCrauton.add("yError", 0).withPosition(9, 0).getEntry(); - if (DriverStation.getJoystickName(0).equals("Controller (Xbox One For Windows)")) { driveController = new XboxDriveController(); } else { @@ -202,7 +189,8 @@ public RobotContainer() { elevatorSubsystem, climbSubsystem, swerveSubsystem, - lightBarSubsystem, fmsSubsystem + lightBarSubsystem, + fmsSubsystem ); autonPathChooser = new SendableChooser<>(); @@ -253,9 +241,6 @@ private void configureBindings() { driveController.getRotatePower()); } - xError.setValue(xPID.getPositionError()); - yError.setValue(yPID.getPositionError()); - }, swerveSubsystem)); /* Pressing the button resets the field axes to the current robot axes. */ diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java index 36c11adc..2360aa3d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java @@ -30,8 +30,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.leds.LightBarSubsystem; -import frc.robot.subsystems.superstructure.LightBarStatus; import frc.robot.util.TrackingTimer; /** The subsystem that controls the rollers on the intake. */ @@ -55,15 +53,13 @@ public class IntakeRollerSubsystem extends SubsystemBase { private NetworkTableEntry rockwellSensorEntry; private NetworkTableEntry ampSenSorEntry; - private final LightBarSubsystem lightBarSubsystem; - private Timer colorResetTimer; private TrackingTimer sensorTimer = new TrackingTimer(); /** * Subsystem controls the front, middle, and integration rollers for the intake. */ - public IntakeRollerSubsystem(LightBarSubsystem lightBarSubsystem) { + public IntakeRollerSubsystem() { integrationMotor = new TalonSRX(IntakeConstants.INTEGRATION_MOTOR_ID); frontMotors = new CANSparkMax(IntakeConstants.FRONT_MOTOR_ID, MotorType.kBrushless); frontMotors.setIdleMode(IdleMode.kBrake); @@ -81,8 +77,6 @@ public IntakeRollerSubsystem(LightBarSubsystem lightBarSubsystem) { ntFrontPublisher = ntTable.getBooleanTopic("FrontSensor").publish(); ntBackPublisher = ntTable.getBooleanTopic("BackSensor").publish(); - this.lightBarSubsystem = lightBarSubsystem; - // colorResetTimer = new Timer(); // colorResetTimer.start(); } @@ -151,8 +145,5 @@ public void periodic() { ntFrontPublisher.set(getFrontSensorReached()); prevFrontSensorValue = getFrontSensorValue(); - if (getFrontSensorValue()) { - lightBarSubsystem.setLightBarStatus(LightBarStatus.HOLDING_NOTE, 2); - } } } diff --git a/src/main/java/frc/robot/subsystems/leds/LightBarSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LightBarSubsystem.java index 27072d17..0e4325f5 100644 --- a/src/main/java/frc/robot/subsystems/leds/LightBarSubsystem.java +++ b/src/main/java/frc/robot/subsystems/leds/LightBarSubsystem.java @@ -3,7 +3,11 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.LEDConstants; +import frc.robot.subsystems.FieldManagementSubsystem; +import frc.robot.subsystems.intake.IntakeRollerSubsystem; +import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem; import frc.robot.subsystems.superstructure.LightBarStatus; +import frc.robot.subsystems.superstructure.MatchStatus; import frc.robot.util.OpacityColor; import frc.robot.util.TrackingTimer; @@ -42,11 +46,21 @@ public class LightBarSubsystem extends SubsystemBase { private static final OpacityColor GREEN_COLOR = new OpacityColor(0, 254, 0); // used for shooter spin-up private static final OpacityColor BLUE_COLOR = new OpacityColor(0, 0, 255); // used for auto-align indicator private static final OpacityColor WHITE_COLOR = new OpacityColor(255, 255, 255); + + private final IntakeRollerSubsystem intakeRollerSubsystem; + private final ShooterFlywheelSubsystem shooterFlywheelSubsystem; + private final FieldManagementSubsystem fieldManagementSubsystem; /** Subsystem to manage a short strip of LEDs on the robot, used for robot->driver and driver->HP signaling. * */ - public LightBarSubsystem() { + public LightBarSubsystem(IntakeRollerSubsystem intakeRollerSubsystem, + ShooterFlywheelSubsystem shooterFlywheelSubsystem, + FieldManagementSubsystem fieldManagementSubsystem) { + + this.intakeRollerSubsystem = intakeRollerSubsystem; + this.shooterFlywheelSubsystem = shooterFlywheelSubsystem; + this.fieldManagementSubsystem = fieldManagementSubsystem; ledStrip = new LEDStrip(LEDConstants.LED_PWM_PORT, LEDConstants.LED_LENGTH); @@ -78,7 +92,19 @@ public LightBarSubsystem() { */ public void periodic() { - if (ledTimer.advanceIfElapsed(0.05)) { + if (intakeRollerSubsystem.getFrontSensorValue()) { + setLightBarStatus(LightBarStatus.HOLDING_NOTE, 2); + } + + if (fieldManagementSubsystem.getMatchStatus() == MatchStatus.AUTON) { + setLightBarStatus(LightBarStatus.AUTON, 0); + } else if (fieldManagementSubsystem.getMatchStatus() == MatchStatus.ENDGAME) { + setLightBarStatus(LightBarStatus.ENDGAME, 0); + } else { + setLightBarStatus(LightBarStatus.DORMANT, 0); + } + + if (ledTimer.advanceIfElapsed(0.07)) { rainbowOffset += inc; rainbowOffset = rainbowOffset % (LEDConstants.LED_LENGTH * 360); @@ -89,7 +115,6 @@ public void periodic() { bounceOffset -= (inc * bounceDirection); // undo the increment that oversteps the array length bounceDirection *= -1; // switch the bounce direction } - } switch (matchStatus) { @@ -132,7 +157,7 @@ public void periodic() { break; } - if (mechResetLEDTimer.hasElapsed(2.5)) { + if (mechResetLEDTimer.hasElapsed(1.0)) { mechStatus = LightBarStatus.DORMANT; mechResetLEDTimer.stop(); mechResetLEDTimer.reset(); @@ -154,7 +179,6 @@ public void periodic() { ledStrip.addLayer(mechLayer); ledStrip.setBuffer(1); - } /** @@ -174,8 +198,6 @@ public void setLightBarStatus(LightBarStatus status, int statusType) { } else { System.out.println("[LightBarSubsystem] Invalid lightBar statusType provided."); } - - // this.status = status; } /** Gets the status of the light bar. diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureSubsystem.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureSubsystem.java deleted file mode 100644 index 85d561b1..00000000 --- a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureSubsystem.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.subsystems.superstructure; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.FieldManagementSubsystem; -import frc.robot.subsystems.leds.LightBarSubsystem; - -public class SuperstructureSubsystem extends SubsystemBase { - - private final FieldManagementSubsystem fmsSubsystem; - private final LightBarSubsystem lightBarSubsystem; - - public SuperstructureSubsystem(LightBarSubsystem lightBarSubsystem, FieldManagementSubsystem fmsSubsystem){ - this.fmsSubsystem = fmsSubsystem; - this.lightBarSubsystem = lightBarSubsystem; - } - - @Override - public void periodic() { - if (fmsSubsystem.getMatchStatus() == MatchStatus.AUTON) { - lightBarSubsystem.setLightBarStatus(LightBarStatus.AUTON, 0); - } else if (fmsSubsystem.getMatchStatus() == MatchStatus.ENDGAME) { - lightBarSubsystem.setLightBarStatus(LightBarStatus.ENDGAME, 0); - } else { - lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 0); - } - } -} \ No newline at end of file