From f94f78ffd0c9c6dac11278b35790396abdc9316c Mon Sep 17 00:00:00 2001 From: Bryson Halsey Date: Sun, 16 Feb 2025 20:08:49 -0500 Subject: [PATCH] added coast and brake commands to dashboard, along with match timer --- Elastic Json/3467_Elastic_V2.json | 161 +++++++++++++++++- src/main/java/frc/robot/Robot.java | 6 +- .../java/frc/robot/subsystems/Arm/Arm.java | 17 +- .../robot/subsystems/Elevator/Elevator.java | 17 +- 4 files changed, 192 insertions(+), 9 deletions(-) diff --git a/Elastic Json/3467_Elastic_V2.json b/Elastic Json/3467_Elastic_V2.json index 4591c7a..648fc6f 100644 --- a/Elastic Json/3467_Elastic_V2.json +++ b/Elastic Json/3467_Elastic_V2.json @@ -109,7 +109,8 @@ "type": "Camera Stream", "properties": { "topic": "/CameraPublisher/front_left-raw", - "period": 0.06 + "period": 0.06, + "rotation_turns": 0 } }, { @@ -121,7 +122,8 @@ "type": "Camera Stream", "properties": { "topic": "/CameraPublisher/front_right-raw", - "period": 0.06 + "period": 0.06, + "rotation_turns": 0 } }, { @@ -206,7 +208,8 @@ "type": "Camera Stream", "properties": { "topic": "/CameraPublisher/front_left-processed", - "period": 0.06 + "period": 0.06, + "rotation_turns": 0 } }, { @@ -218,7 +221,8 @@ "type": "Camera Stream", "properties": { "topic": "/CameraPublisher/front_right-processed", - "period": 0.06 + "period": 0.06, + "rotation_turns": 0 } }, { @@ -1728,6 +1732,155 @@ } ] } + }, + { + "name": "Pit", + "grid_layout": { + "layouts": [ + { + "title": "ClawRollerLaserCAN", + "x": 950.0, + "y": 300.0, + "width": 300.0, + "height": 150.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Distance", + "x": 0.0, + "y": 0.0, + "width": 150.0, + "height": 150.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/ClawRollerLaserCAN/Distance", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "IntakeLaserCAN", + "x": 650.0, + "y": 300.0, + "width": 300.0, + "height": 150.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Distance", + "x": 0.0, + "y": 0.0, + "width": 150.0, + "height": 150.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/IntakeLaserCAN/Distance", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + ], + "containers": [ + { + "title": "Elevator Coast Command", + "x": 650.0, + "y": 0.0, + "width": 300.0, + "height": 150.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Elevator Coast Command", + "period": 0.06, + "show_type": false + } + }, + { + "title": "Arm Coast Command", + "x": 950.0, + "y": 0.0, + "width": 300.0, + "height": 150.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Arm Coast Command", + "period": 0.06, + "show_type": false + } + }, + { + "title": "Arm Brake Command", + "x": 950.0, + "y": 150.0, + "width": 300.0, + "height": 150.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Arm Brake Command", + "period": 0.06, + "show_type": false + } + }, + { + "title": "Elevator Brake Command", + "x": 650.0, + "y": 150.0, + "width": 300.0, + "height": 150.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Elevator Brake Command", + "period": 0.06, + "show_type": false + } + }, + { + "title": "Triggered", + "x": 950.0, + "y": 450.0, + "width": 300.0, + "height": 100.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/RealOutputs/ClawRollerLaserCAN/Triggered", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Triggered", + "x": 650.0, + "y": 450.0, + "width": 300.0, + "height": 100.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/RealOutputs/IntakeLaserCAN/Triggered", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } } ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7a8a19c..f0344f4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -134,7 +134,7 @@ public void robotPeriodic() // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); - Logger.recordOutput("ZeroedCompPoses", new Pose3d()); + SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime()); } /** This function is called once when the robot is disabled. */ @@ -142,7 +142,7 @@ public void robotPeriodic() public void disabledInit() { m_robotContainer.resetSimulationField(); - Elastic.selectTab(1); + Elastic.selectTab(0); SmartDashboard.putData("Auto Path Preview", m_autoTraj); } @@ -190,7 +190,7 @@ public void teleopInit() if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - Elastic.selectTab(2); + Elastic.selectTab(1); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/subsystems/Arm/Arm.java b/src/main/java/frc/robot/subsystems/Arm/Arm.java index fbfcff0..7c5b680 100644 --- a/src/main/java/frc/robot/subsystems/Arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm/Arm.java @@ -4,6 +4,7 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystem; @@ -41,7 +42,9 @@ public enum State implements TargetState { BARGE(() -> Units.degreesToRotations(30.0), ProfileType.MM_POSITION), TUNING(() -> Units.degreesToRotations(positionTuning.getAsDouble()), ProfileType.MM_POSITION), - CHARACTERIZATION(() -> 0.0, ProfileType.CHARACTERIZATION); + CHARACTERIZATION(() -> 0.0, ProfileType.CHARACTERIZATION), + COAST(() -> 0.0, ProfileType.DISABLED_COAST), + BRAKE(() -> 0.0, ProfileType.DISABLED_BRAKE); private final DoubleSupplier output; private final ProfileType profileType; @@ -60,6 +63,8 @@ public enum State implements TargetState { public Arm(ArmIO io, boolean isSim) { super(ProfileType.MM_POSITION, ArmConstants.kSubSysConstants, io, isSim); + SmartDashboard.putData("Arm Coast Command", setCoastStateCommand()); + SmartDashboard.putData("Arm Brake Command", setBrakeStateCommand()); } /** Constructor */ @@ -68,6 +73,16 @@ public Command setStateCommand(State state) return this.runOnce(() -> this.state = state); } + public Command setCoastStateCommand() + { + return this.runOnce(() -> this.state = State.COAST); + } + + public Command setBrakeStateCommand() + { + return this.runOnce(() -> this.state = State.BRAKE); + } + public boolean atPosition(double tolerance) { return io.atPosition(tolerance); diff --git a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java index 30467fa..4860025 100644 --- a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -45,7 +46,9 @@ public enum State implements TargetState { ALGAE_SCORE(() -> 0.05, ProfileType.MM_POSITION), BARGE(() -> 4.0, ProfileType.MM_POSITION), TUNING(() -> positionTuning.getAsDouble(), ProfileType.MM_POSITION), - CHARACTERIZATION(() -> 0.0, ProfileType.CHARACTERIZATION); + CHARACTERIZATION(() -> 0.0, ProfileType.CHARACTERIZATION), + COAST(() -> 0.0, ProfileType.DISABLED_COAST), + BRAKE(() -> 0.0, ProfileType.DISABLED_BRAKE); private final DoubleSupplier output; private final ProfileType profileType; @@ -66,6 +69,8 @@ public enum State implements TargetState { public Elevator(ElevatorIO io, boolean isSim) { super(ProfileType.MM_POSITION, ElevatorConstants.kSubSysConstants, io, isSim); + SmartDashboard.putData("Elevator Coast Command", setCoastStateCommand()); + SmartDashboard.putData("Elevator Brake Command", setBrakeStateCommand()); } public Command setStateCommand(State state) @@ -73,6 +78,16 @@ public Command setStateCommand(State state) return this.runOnce(() -> this.state = state); } + public Command setCoastStateCommand() + { + return this.runOnce(() -> this.state = State.COAST); + } + + public Command setBrakeStateCommand() + { + return this.runOnce(() -> this.state = State.BRAKE); + } + private Debouncer homedDebouncer = new Debouncer(.25, DebounceType.kRising); public Trigger homedTrigger =