diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 814ac38e..84c94a93 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -52,6 +53,14 @@ public class Robot extends LoggedRobot { private Field2d _autonomousTrajectory = new Field2d(); private List _shownPaths = new ArrayList<>(); + // Shuffleboard Boolean that shows whether battery voltage is above 12.8V. + public boolean isBatteryVoltageAbove() { + return RobotController.getBatteryVoltage() >=12.8; + } + + // Shuffleboard Boolean that shows whether an auto is chosen in autonomous + public boolean _isAutoChosen; + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -155,6 +164,10 @@ public void robotPeriodic() { Logger.recordOutput("NTClients/Names", clientNames.toArray(new String[clientNames.size()])); Logger.recordOutput("NTClients/Addresses", clientAddresses.toArray(new String[clientAddresses.size()])); + // SHUFFLEBOARD + // This shows whether the battery is above 12.8V. + SmartDashboard.putBoolean("Is Battery Above 12.8V?", isBatteryVoltageAbove()); + } /** This function is called once when the robot is disabled. */ @@ -201,7 +214,14 @@ public void autonomousInit() { // schedule the autonomous command (example) if (_autonomousCommand != null) { _autonomousCommand.schedule(); + _isAutoChosen = true; + } else { + _isAutoChosen = false; } + + // SHUFFLEBOARD + // This shows whether an auto is chosen. + SmartDashboard.putBoolean("Is An Auto Selected?", _isAutoChosen); } /** This function is called periodically during autonomous. */ diff --git a/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java b/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java index ef662e17..638ba932 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.Drive; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index cff43ed1..72e2af72 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -173,6 +173,10 @@ public double getPivotAngle() { return _intakeInputs._pivotEncoderPositionDegrees; } + public boolean isPivotEncoderAtZero() { + return _intakeInputs._pivotEncoderPositionDegrees==0; + } + /** * Returns the target position * @return intakePosition as an angle (double) @@ -202,6 +206,7 @@ public void periodic() { SmartDashboard.putNumber("Intake Current Pivot Angle", _intakeInputs._pivotEncoderPositionDegrees); SmartDashboard.putNumber("Intake Desired Pivot Angle", _intakePosition.getAngle()); SmartDashboard.putBoolean("Intake Has Note", _hasNote); + SmartDashboard.putBoolean("Is Intake Encoder At Zero", isPivotEncoderAtZero()); // VISUALIZATION double angle = _intakeInputs._pivotEncoderPositionDegrees; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index e5625a0f..9fec9a8f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.util.LoggedTunableNumber; @@ -183,6 +184,11 @@ public double getCurrentPositionInDegrees() { // } } + // checks if shooter encoder is at 0 + public boolean isShooterEncoderAtZero() { + return _shooterInputs._angleEncoderPositionDegrees == 0; + } + /** * The current zone the shooter thinks it is in. * @@ -334,5 +340,10 @@ public void periodic() { Logger.recordOutput("Shooter/TargetShooterAngle", _targetShooterAngle); Logger.recordOutput("Shooter/AnglePIDCalculatedOutput", calcAnglePID()); Logger.recordOutput("Shooter/isAutoShootEnabled",this.isAutoShootEnabled()); + + // SHUFFLEBOARD VALUES + SmartDashboard.putBoolean("Is Shooter Encoder at 0?", isShooterEncoderAtZero()); + + } }