diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 40574cda..afb50f94 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,6 +33,7 @@ import frc.robot.settings.Constants.Field; import frc.robot.commands.IndexCommand; +import frc.robot.commands.IndicatorLights; import frc.robot.settings.Constants; import frc.robot.settings.Constants.ClimberConstants; import frc.robot.settings.Constants.DriveConstants; @@ -56,6 +57,7 @@ import frc.robot.subsystems.IndexerSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.Limelight; +import frc.robot.subsystems.RobotState; import frc.robot.subsystems.ShooterSubsystem; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -168,7 +170,7 @@ private void climbSpotChooserInit() { SmartDashboard.putData(climbSpotChooser); } private void driveTrainInst() { - driveTrain = new DrivetrainSubsystem(lights, lightsExist); + driveTrain = new DrivetrainSubsystem(); defaultDriveCommand = new Drive( driveTrain, () -> driverController.getL1Button(), @@ -208,6 +210,7 @@ private void limelightInit() { } private void lightsInst() { lights = new Lights(Constants.LED_COUNT-1); + lights.setDefaultCommand(new IndicatorLights(lights)); } @@ -338,7 +341,11 @@ public void teleopPeriodic() { driveTrain.calculateSpeakerAngle(); if(useDetectorLimelight) { SmartDashboard.putNumber("Is Note Seen?", limelight.getNeuralDetectorValues().ta); + RobotState.getInstance().IsNoteSeen = limelight.getNeuralDetectorValues().isResultValid; } + SmartDashboard.putBoolean("is note seen", RobotState.getInstance().IsNoteSeen); + SmartDashboard.putBoolean("shooter in range", RobotState.getInstance().ShooterInRange); + SmartDashboard.putBoolean("shooter ready", RobotState.getInstance().ShooterReady); } public void disabledPeriodic() { diff --git a/src/main/java/frc/robot/commands/IndexCommand.java b/src/main/java/frc/robot/commands/IndexCommand.java index 86bb0091..4b40926f 100644 --- a/src/main/java/frc/robot/commands/IndexCommand.java +++ b/src/main/java/frc/robot/commands/IndexCommand.java @@ -15,6 +15,7 @@ import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.IndexerSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.RobotState; import frc.robot.subsystems.ShooterSubsystem; @@ -72,20 +73,30 @@ public void execute() { shooter.shootThing(ShooterConstants.SHOOTER_AMP_POWER); } } - if (shootIfReadySupplier.getAsBoolean()) { - if((angleShooterSubsytem.calculateSpeakerAngleDifference()