From bcbf978a5e12d14de696323a60eec747a4e9e33a Mon Sep 17 00:00:00 2001 From: iefomit Date: Tue, 17 Mar 2026 19:35:42 -0700 Subject: [PATCH] reducing clutter, testing dashboard --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 12 ++++++++++++ .../commands/drive_comm/DefaultDriveCommand.java | 1 - .../java/frc/robot/subsystems/Intake/Intake.java | 3 +++ .../robot/subsystems/drivetrain/Drivetrain.java | 8 ++++++++ .../frc/robot/subsystems/shooter/Shooter.java | 8 +++++++- .../java/frc/robot/subsystems/turret/Turret.java | 15 +++++++++------ 7 files changed, 40 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 21352cc..93fb78b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -121,6 +121,7 @@ public class Robot extends LoggedRobot { CommandScheduler.getInstance().run(); robotContainer.logComponents(); + robotContainer.periodic(); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d8b1922..50743b4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -87,6 +87,8 @@ public class RobotContainer { // display the current robot id on smartdashboard SmartDashboard.putString("RobotID", robotId.toString()); + SmartDashboard.putNumber("Match Time", 0.0); + // Filling the SendableChooser on SmartDashboard // autoChooserInit(); @@ -307,4 +309,14 @@ public class RobotContainer { // Subsystem Pose3ds }); } + + /** Updates SmartDashboard values that need to be refreshed every loop */ + public void periodic() { + // update match timer + double matchTime = DriverStation.getMatchTime(); + if (matchTime > 0) { + SmartDashboard.putNumber("Match Time", matchTime); + } + SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); + } } diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java index ed873af..727a983 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -42,7 +42,6 @@ public class DefaultDriveCommand extends Command { trenchAssistPid.setIntegratorRange(-1, 1); SmartDashboard.putNumber("0 degrees snap location", 0); - SmartDashboard.putNumber("", 0); } @Override diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index f4bfdee..786a382 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -182,6 +182,9 @@ public class Intake extends SubsystemBase implements IntakeIO{ Logger.recordOutput("Intake/Setpoint", setpointInches); robotExtension.setLength(inchExtension); + SmartDashboard.putNumber("Intake Extension (in)", inchExtension); + SmartDashboard.putBoolean("Intake Extended", inchExtension > 1.0); + if(calibrating){ leftMotor.set(-0.1); rightMotor.set(-0.1); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 7a8b54b..cb77ac2 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -24,6 +24,8 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.FieldConstants; @@ -111,6 +113,8 @@ public class Drivetrain extends SubsystemBase { private Rotation2d rawGyroRotation = new Rotation2d(); + private final Field2d field = new Field2d(); + /** * Creates a new Swerve Style Drivetrain. */ @@ -171,6 +175,8 @@ public class Drivetrain extends SubsystemBase { }); // PPLibTelemetry.enableCompetitionMode(); + + SmartDashboard.putData("Field", field); } public void close() { @@ -210,6 +216,8 @@ public class Drivetrain extends SubsystemBase { } Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses()); updateOdometryVision(); + + field.setRobotPose(getPose()); } // DRIVE diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 5e751e2..230a38f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -81,7 +81,13 @@ public class Shooter extends SubsystemBase implements ShooterIO { Logger.recordOutput("Shooter/realVelocity", shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER); Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed); - SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOON" : "lost"); + // SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOON" : "lost"); + double actualWheelVelocity = shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER; + SmartDashboard.putNumber("Shooter Speed Error (mps)", shooterTargetSpeed - actualWheelVelocity); + + boolean autoWon = HubActive.wonAuto(); + SmartDashboard.putBoolean("Auto Won", autoWon); + SmartDashboard.putString("Auto Result", autoWon ? "WON" : "LOST"); } /** diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 19d212d..550f6c8 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -107,6 +108,14 @@ public class Turret extends SubsystemBase implements TurretIO{ SmartDashboard.putData("Start turret calibration", new InstantCommand(()-> calibrate())); SmartDashboard.putData("Stop turret calibration", new InstantCommand(()-> stopCalibrating())); + SendableChooser turretTestChooser = new SendableChooser<>(); + turretTestChooser.setDefaultOption("Turn to 0", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0))); + turretTestChooser.addOption("Turn to -90", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0))); + turretTestChooser.addOption("Turn to 90", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0))); + turretTestChooser.addOption("Turn to 200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0))); + turretTestChooser.addOption("Turn to -200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0))); + SmartDashboard.putData("Turret Test Positions", turretTestChooser); + double leftPosition = encoderLeft.getAbsolutePosition().getValueAsDouble(); double leftAbs = wrapUnit(leftPosition - TurretConstants.LEFT_ENCODER_OFFSET); @@ -126,12 +135,6 @@ public class Turret extends SubsystemBase implements TurretIO{ //motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO); motor.setPosition(0.0); - - SmartDashboard.putData("Turn to 0", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0);})); - SmartDashboard.putData("Turn to -90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0);})); - SmartDashboard.putData("Turn to 90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0);})); - SmartDashboard.putData("Turn to 200", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0);})); - SmartDashboard.putData("Turn to -200", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0);})); } /* ---------------- Public API ---------------- */ -- 2.39.5