Skip to content

Subsystem improvements and health checks for testing. #9

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
fishnos wants to merge 3 commits into
base: comp-dev
Choose a base branch
from
Open

Subsystem improvements and health checks for testing. #9

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
bbd671e
initial commit
fishnos Feb 7, 2026
11916ff
overall robot subsystem functionality checker
fishnos Feb 7, 2026
961644c
discretize swerve properly and fix a few logic errors. ready for merge
fishnos Feb 12, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
130 changes: 130 additions & 0 deletions src/main/java/frc/robot/Dashboard.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,130 @@
package frc.robot;

import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import frc.robot.subsystems.Superstructure;
import frc.robot.subsystems.swerve.SwerveDrive;

public class Dashboard {
private static Field2d robotField;
private static SwerveDrive swerveDrive;

private static StringPublisher superstructureCurrentState;
private static StringPublisher superStructureDesiredState;
private static DoublePublisher matchTimePublisher;

private static SendableChooser<Command> autoChooser;
private static SendableBuilderImpl autoBuilder;

private static boolean initialized = false;

private static void initialize() {
if (initialized) return;

NetworkTableInstance inst = NetworkTableInstance.getDefault();
NetworkTable table = inst.getTable("Dashboard");

// Create publishers once
superstructureCurrentState = table.getStringTopic("Superstructure/CurrentState").publish();
superStructureDesiredState = table.getStringTopic("Superstructure/DesiredState").publish();
matchTimePublisher = table.getDoubleTopic("MatchTime").publish();

// Initialize Field2d
NetworkTable fieldTable = table.getSubTable("Field");
robotField = new Field2d();

SendableBuilderImpl fieldBuilder = new SendableBuilderImpl();
fieldBuilder.setTable(fieldTable);
robotField.initSendable(fieldBuilder);
fieldBuilder.startListeners();

// Initialize swerve drive sendable
swerveDrive = SwerveDrive.getInstance();
NetworkTable swerveTable = table.getSubTable("Swerve Drive");

Sendable swerveSendable = new Sendable() {
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("SwerveDrive");

builder.addDoubleProperty("Front Left Angle",
() -> swerveDrive.getSwerveModulePositions()[0].angle.getRadians(), null);
builder.addDoubleProperty("Front Left Velocity",
() -> swerveDrive.getSwerveModuleStates()[0].speedMetersPerSecond, null);

builder.addDoubleProperty("Front Right Angle",
() -> swerveDrive.getSwerveModulePositions()[1].angle.getRadians(), null);
builder.addDoubleProperty("Front Right Velocity",
() -> swerveDrive.getSwerveModuleStates()[1].speedMetersPerSecond, null);

builder.addDoubleProperty("Back Left Angle",
() -> swerveDrive.getSwerveModulePositions()[2].angle.getRadians(), null);
builder.addDoubleProperty("Back Left Velocity",
() -> swerveDrive.getSwerveModuleStates()[2].speedMetersPerSecond, null);

builder.addDoubleProperty("Back Right Angle",
() -> swerveDrive.getSwerveModulePositions()[3].angle.getRadians(), null);
builder.addDoubleProperty("Back Right Velocity",
() -> swerveDrive.getSwerveModuleStates()[3].speedMetersPerSecond, null);

builder.addDoubleProperty("Robot Angle",
() -> RobotState.getInstance().getAngleToFloor().getRadians(), null);
}
};

SendableBuilderImpl swerveBuilder = new SendableBuilderImpl();
swerveBuilder.setTable(swerveTable);
swerveSendable.initSendable(swerveBuilder);
swerveBuilder.startListeners();

// Auto chooser
autoChooser = new SendableChooser<Command>();
autoChooser.setDefaultOption("None", new InstantCommand());

NetworkTable autoTable = table.getSubTable("AutoChooser");
autoBuilder = new SendableBuilderImpl();
autoBuilder.setTable(autoTable);
autoChooser.initSendable(autoBuilder);
autoBuilder.startListeners();
autoBuilder.update();

initialized = true;
}

public static void addAutoChooserOption(String key, Command command) {
autoChooser.addOption(key, command);
}

public static Command getCurrentAutoCommand() {
return autoChooser.getSelected();
}

public static void updateData() {
initialize();

// Superstructure state logging
Superstructure superstructure = Superstructure.getInstance();
superstructureCurrentState.set(superstructure.getCurrentState().name());
superStructureDesiredState.set(superstructure.getDesiredState().name());

// Update match time
matchTimePublisher.set(DriverStation.getMatchTime());

// Update the field with current robot pose
robotField.setRobotPose(RobotState.getInstance().getEstimatedPose());

if (autoBuilder != null) {
autoBuilder.update();
}
}
}
9 changes: 5 additions & 4 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,8 +87,8 @@ public void robotInit() {
DriverStationSim.notifyNewData();
}

break;
break;

case REPLAY:
// Replaying a log, set up replay source
setUseTiming(false); // Run as fast as possible
Expand Down Expand Up @@ -128,6 +128,7 @@ public void robotPeriodic() {
// robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
Dashboard.updateData();
}

/** This function is called once each time the robot enters Disabled mode. */
Expand Down Expand Up @@ -161,15 +162,15 @@ private void linkFollowPathLogging() {
Logger.recordOutput(pair.getFirst(), pair.getSecond());
});
}

/**
* This autonomous runs the autonomous command selected by your
* {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_robotContainer.autonomousInit();

if (Constants.agentMode) {
m_autonomousCommand = null;
} else {
Expand Down
131 changes: 61 additions & 70 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,32 +3,20 @@
import org.littletonrobotics.junction.Logger;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import frc.robot.constants.AlignmentConstants;
import frc.robot.commands.check.CheckSystemCommand;
// import frc.robot.commands.autos.tower.ScoreL1;
import frc.robot.constants.Constants;
import frc.robot.lib.BLine.FollowPath;
import frc.robot.lib.BLine.Path;
import frc.robot.lib.BLine.Path.EventTrigger;
import frc.robot.lib.BLine.Path.PathConstraints;
import frc.robot.lib.BLine.Path.Waypoint;
import frc.robot.lib.input.XboxController;
import frc.robot.lib.util.ballistics.ProjectileVisualizer;
import frc.robot.subsystems.Superstructure;
import frc.robot.subsystems.Superstructure.DesiredState;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.swerve.SwerveDrive;
// import frc.robot.subsystems.vision.Vision;
import frc.robot.subsystems.vision.Vision;

public class RobotContainer {
public static RobotContainer instance = null;
Expand All @@ -51,7 +39,6 @@ public static RobotContainer getInstance() {
@SuppressWarnings("unused")
private final ProjectileVisualizer projectileVisualizer = ProjectileVisualizer.getInstance();


private RobotContainer() {
registerEventTriggers();

Expand All @@ -62,18 +49,16 @@ private RobotContainer() {
// Configure teleop input suppliers for SwerveDrive FSM
// Using normalized inputs (-1 to 1) with deadband applied
swerveDrive.setTeleopInputSuppliers(
() -> -MathUtil.applyDeadband(xboxDriver.getLeftY(), Constants.OperatorConstants.LEFT_Y_DEADBAND),
() -> -MathUtil.applyDeadband(xboxDriver.getLeftX(), Constants.OperatorConstants.LEFT_X_DEADBAND),
() -> -MathUtil.applyDeadband(xboxDriver.getRightX(), Constants.OperatorConstants.RIGHT_X_DEADBAND)
);
() -> -MathUtil.applyDeadband(xboxDriver.getLeftY(), Constants.OperatorConstants.LEFT_Y_DEADBAND),
() -> -MathUtil.applyDeadband(xboxDriver.getLeftX(), Constants.OperatorConstants.LEFT_X_DEADBAND),
() -> -MathUtil.applyDeadband(xboxDriver.getRightX(), Constants.OperatorConstants.RIGHT_X_DEADBAND));

// Set default state to TELEOP
swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.TELEOP);

// Set default superstructure state to HOME
superstructure.setDesiredState(Superstructure.DesiredState.HOME);


configureBindings();
}

Expand All @@ -89,71 +74,77 @@ private void registerEventTriggers() {
private void configureBindings() {
// Path toClimb = new Path(new Waypoint(1,3, new Rotation2d(0)));
// xboxDriver.getXButton().onTrue(
// new InstantCommand(() -> currentPath = toClimb).andThen(
// new InstantCommand(() -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH)).andThen(
// new WaitUntilCommand(() -> swerveDrive.getCurrentSystemState() == SwerveDrive.CurrentSystemState.IDLE)).andThen(
// new InstantCommand(() -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.TELEOP))
// )
// )
// new InstantCommand(() -> currentPath = toClimb).andThen(
// new InstantCommand(() ->
// swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH)).andThen(
// new WaitUntilCommand(() -> swerveDrive.getCurrentSystemState() ==
// SwerveDrive.CurrentSystemState.IDLE)).andThen(
// new InstantCommand(() ->
// swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.TELEOP))
// )
// )
// );

// xboxDriver.getXButton().onFalse(new InstantCommand(() -> robotState.resetPose(new Pose2d(0,0, new Rotation2d(0)))));
// xboxDriver.getXButton().onFalse(new InstantCommand(() ->
// robotState.resetPose(new Pose2d(0,0, new Rotation2d(0)))));

// xboxDriver.getAButton().onTrue(new InstantCommand(() -> {
// ProjectileVisualizer.addProjectile(
// 0, // vx
// 0, // vy
// 10, // exitVelocity
// new Pose3d(0, 0, 2, new Rotation3d(0, Math.PI/4, 0)), // position & rotation
// 3 // hub height
// );
// })
// ProjectileVisualizer.addProjectile(
// 0, // vx
// 0, // vy
// 10, // exitVelocity
// new Pose3d(0, 0, 2, new Rotation3d(0, Math.PI/4, 0)), // position & rotation
// 3 // hub height
// );
// })
// );

xboxDriver.getAButton().onTrue(
new InstantCommand(() -> superstructure.setDesiredState(DesiredState.SHOOTING))
);
new InstantCommand(() -> superstructure.setDesiredState(DesiredState.SHOOTING)));
xboxDriver.getBButton().onTrue(
new InstantCommand(() -> superstructure.setDesiredState(DesiredState.TRACKING))
);
new InstantCommand(() -> superstructure.setDesiredState(DesiredState.TRACKING)));

SmartDashboard.putData("System Check", new CheckSystemCommand());

// xboxDriver.getAButton().onTrue(
// new ConditionalCommand(
// new SequentialCommandGroup(
// followPath(
// new Path(
// new PathConstraints()
// .setMaxVelocityMetersPerSec(AlignmentConstants.Tower.INTERMEDIARY_MAX_VELOCITY_METERS_PER_SEC)
// .setEndTranslationToleranceMeters(AlignmentConstants.Tower.INTERMEDIARY_TRANSLATION_TOLERANCE_METERS)
// .setEndRotationToleranceDeg(AlignmentConstants.Tower.INTERMEDIARY_ROTATION_TOLERANCE_DEG),
// new Waypoint(AlignmentConstants.Tower.Left.INTERMEDIATE_WAYPOINT)),
// false
// ),
// followPath(
// new Path(
// new PathConstraints().setMaxVelocityMetersPerSec(AlignmentConstants.Tower.APPROACH_MAX_VELOCITY_METERS_PER_SEC),
// new Waypoint(AlignmentConstants.Tower.Left.FINAL_WAYPOINT)),
// false
// )
// ),
// new InstantCommand(() -> {}),
// () -> AlignmentConstants.Tower.isWithinBounds(robotState.getEstimatedPose(), AlignmentConstants.Tower.Left.BOUNDS)
// )
// new ConditionalCommand(
// new SequentialCommandGroup(
// followPath(
// new Path(
// new PathConstraints()
// .setMaxVelocityMetersPerSec(AlignmentConstants.Tower.INTERMEDIARY_MAX_VELOCITY_METERS_PER_SEC)
// .setEndTranslationToleranceMeters(AlignmentConstants.Tower.INTERMEDIARY_TRANSLATION_TOLERANCE_METERS)
// .setEndRotationToleranceDeg(AlignmentConstants.Tower.INTERMEDIARY_ROTATION_TOLERANCE_DEG),
// new Waypoint(AlignmentConstants.Tower.Left.INTERMEDIATE_WAYPOINT)),
// false
// ),
// followPath(
// new Path(
// new
// PathConstraints().setMaxVelocityMetersPerSec(AlignmentConstants.Tower.APPROACH_MAX_VELOCITY_METERS_PER_SEC),
// new Waypoint(AlignmentConstants.Tower.Left.FINAL_WAYPOINT)),
// false
// )
// ),
// new InstantCommand(() -> {}),
// () -> AlignmentConstants.Tower.isWithinBounds(robotState.getEstimatedPose(),
// AlignmentConstants.Tower.Left.BOUNDS)
// )
// );

// Test snap-to-angle bindings
// xboxDriver.getAButton().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.DesiredState.BUMP)));
// xboxDriver.getAButton().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.DesiredState.HOME)));
// xboxDriver.getAButton().onTrue(new InstantCommand(() ->
// superstructure.setDesiredState(Superstructure.DesiredState.BUMP)));
// xboxDriver.getAButton().onFalse(new InstantCommand(() ->
// superstructure.setDesiredState(Superstructure.DesiredState.HOME)));
}

private Command followPath(Path path, boolean shouldResetPose) {
return
new InstantCommand(() -> {
swerveDrive.setCurrentPath(path, shouldResetPose);
})
.andThen(setSwerveDriveState(SwerveDrive.DesiredSystemState.FOLLOW_PATH)).
andThen(new WaitUntilCommand(
() -> swerveDrive.getCurrentSystemState() == SwerveDrive.CurrentSystemState.IDLE));
return new InstantCommand(() -> {
swerveDrive.setCurrentPath(path, shouldResetPose);
})
.andThen(setSwerveDriveState(SwerveDrive.DesiredSystemState.FOLLOW_PATH)).andThen(new WaitUntilCommand(
() -> swerveDrive.getCurrentSystemState() == SwerveDrive.CurrentSystemState.IDLE));
}

private Command setSwerveDriveState(SwerveDrive.DesiredSystemState state) {
Expand Down
Loading