diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json index f14ae16..11b6b56 100644 --- a/src/main/deploy/elastic-layout.json +++ b/src/main/deploy/elastic-layout.json @@ -2,6 +2,123 @@ "version": 1.0, "grid_size": 34, "tabs": [ + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Robot State", + "x": 0.0, + "y": 0.0, + "width": 374.0, + "height": 170.0, + "type": "Large Text Display", + "properties": { + "topic": "Robot/m_robotContainer/robotStateToString", + "period": 0.6, + "data_type": "string" + } + }, + { + "title": "Match Period", + "x": 374.0, + "y": 170.0, + "width": 714.0, + "height": 136.0, + "type": "Large Text Display", + "properties": { + "topic": "Robot/m_robotContainer/loggedTelemetryInstance/getMatchPeriod", + "period": 0.6, + "data_type": "string" + } + }, + { + "title": "Is our shift?", + "x": 0.0, + "y": 170.0, + "width": 374.0, + "height": 374.0, + "type": "Boolean Box", + "properties": { + "topic": "Robot/m_robotContainer/isOurShiftTrigger", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "X" + } + }, + { + "title": "Our Shift First", + "x": 1088.0, + "y": 0.0, + "width": 408.0, + "height": 170.0, + "type": "Boolean Box", + "properties": { + "topic": "Robot/m_robotContainer/isOurShiftFirstTrigger", + "period": 0.06, + "data_type": "boolean", + "true_color": 4294967295, + "false_color": 4279571733, + "true_icon": "Checkmark", + "false_icon": "None" + } + }, + { + "title": "Match Time", + "x": 374.0, + "y": 0.0, + "width": 714.0, + "height": 170.0, + "type": "Match Time", + "properties": { + "topic": "Robot/m_robotContainer/loggedTelemetryInstance/getMatchTime", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "remaining period time", + "x": 374.0, + "y": 306.0, + "width": 714.0, + "height": 238.0, + "type": "Match Time", + "properties": { + "topic": "Robot/m_robotContainer/loggedTelemetryInstance/getMatchPeriodTime", + "period": 0.033, + "data_type": "double", + "time_display_mode": "Seconds Only", + "red_start_time": 0, + "yellow_start_time": 5 + } + }, + { + "title": "Is our shift?", + "x": 1088.0, + "y": 170.0, + "width": 408.0, + "height": 374.0, + "type": "Boolean Box", + "properties": { + "topic": "Robot/m_robotContainer/isOurShiftTrigger", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "X" + } + } + ] + } + }, { "name": "Disabled", "grid_layout": { @@ -67,7 +184,7 @@ } }, { - "title": "Auto Chooser", + "title": "Auto Choser", "x": 0.0, "y": 0.0, "width": 374.0, @@ -114,7 +231,7 @@ "robot_width": 0.85, "robot_length": 0.85, "show_other_objects": true, - "show_trajectories": true, + "show_trajectories": false, "field_rotation": 0.0, "robot_color": 4294198070, "trajectory_color": 4294967295, @@ -126,7 +243,7 @@ "x": 510.0, "y": 0.0, "width": 238.0, - "height": 340.0, + "height": 170.0, "type": "Large Text Display", "properties": { "topic": "Robot/m_robotContainer/robotStateToString", @@ -136,9 +253,9 @@ }, { "title": "Driving State", - "x": 0.0, + "x": 510.0, "y": 170.0, - "width": 510.0, + "width": 238.0, "height": 170.0, "type": "Large Text Display", "properties": { @@ -147,6 +264,23 @@ "data_type": "string" } }, + { + "title": "All Zeroed", + "x": 0.0, + "y": 170.0, + "width": 510.0, + "height": 170.0, + "type": "Boolean Box", + "properties": { + "topic": "Robot/bothSubsystemsZeroed", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, { "title": "Match Time", "x": 0.0, @@ -162,63 +296,96 @@ "red_start_time": 15, "yellow_start_time": 30 } + }, + { + "title": "Ready to shoot", + "x": 0.0, + "y": 340.0, + "width": 748.0, + "height": 204.0, + "type": "Boolean Box", + "properties": { + "topic": "Robot/m_robotContainer/readyToShootTrigger", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "climbed?", + "x": 748.0, + "y": 340.0, + "width": 782.0, + "height": 204.0, + "type": "Boolean Box", + "properties": { + "topic": "Robot/m_robotContainer/climbingL1Trigger", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } } ] } }, { - "name": "Teleoperated", + "name": "Climbing", "grid_layout": { "layouts": [], "containers": [ { - "title": "Robot State", + "title": "Climber Camera", "x": 0.0, "y": 0.0, - "width": 374.0, - "height": 578.0, - "type": "Large Text Display", + "width": 646.0, + "height": 544.0, + "type": "Camera Stream", "properties": { - "topic": "Robot/m_robotContainer/robotStateToString", - "period": 0.6, - "data_type": "string" + "topic": "/CameraPublisher/USB Camera 0", + "period": 0.06, + "rotation_turns": 0, + "compression": 40, + "fps": 30 } }, { - "title": "Match Time", - "x": 374.0, + "title": "Climbed L1", + "x": 646.0, "y": 0.0, - "width": 782.0, - "height": 170.0, - "type": "Match Time", + "width": 850.0, + "height": 272.0, + "type": "Boolean Box", "properties": { - "topic": "Robot/m_robotContainer/loggedTelemetryInstance/getMatchTime", + "topic": "Robot/m_robotContainer/climbingL1Trigger", "period": 0.06, - "data_type": "double", - "time_display_mode": "Minutes and Seconds", - "red_start_time": 15, - "yellow_start_time": 30 + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "X" } }, { - "title": "Field", - "x": 374.0, - "y": 170.0, - "width": 782.0, - "height": 408.0, - "type": "Field", + "title": "Climbed L2 or L3", + "x": 646.0, + "y": 272.0, + "width": 850.0, + "height": 272.0, + "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Field", + "topic": "Robot/m_robotContainer/climbingL2_L3Trigger", "period": 0.06, - "field_game": "Rebuilt", - "robot_width": 0.85, - "robot_length": 0.85, - "show_other_objects": true, - "show_trajectories": true, - "field_rotation": 0.0, - "robot_color": 4294198070, - "trajectory_color": 4294967295, - "show_robot_outside_widget": true + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "X" } } ] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 97b2743..fe26e84 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,20 +14,23 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.DeferredCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.DeviceIDs.controllerIDs; import frc.robot.commands.AddVisionMeasurement; import frc.robot.commands.ResetPose; -import frc.robot.commands.states.ShootingOnFly; import frc.robot.commands.states.ShootingOnPreset; import frc.robot.constants.ChoreoTraj; +import frc.robot.constants.ConstRumble; import frc.robot.constants.ConstAuto; import frc.robot.constants.ConstField; import frc.robot.constants.ConstSystem; @@ -140,6 +143,16 @@ public class RobotContainer { conDriver.btn_RightBumper), Set.of(driverStateMachineInstance)); + // public final Trigger + public final Trigger isOurShiftFirstTrigger = new Trigger( + () -> telemetryInstance.ourShiftFirst()); + public final Trigger isOurShiftTrigger = new Trigger( + () -> telemetryInstance.isHubActive()); + public final Trigger hubSwitchingTrigger = new Trigger( + () -> telemetryInstance.hubsIsSwitching()); + public final Trigger climbingL1Trigger = new Trigger( + () -> stateMachineInstance.getRobotState() == RobotState.CLIMBING); + public RobotContainer() { conDriver.setLeftDeadband(constControllers.DRIVER_LEFT_STICK_DEADBAND); @@ -457,4 +470,25 @@ public Command addVisionMeasurement() { public static boolean isPracticeBot() { return RobotController.getSerialNumber().equals(ConstSystem.PRACTICE_BOT_RIO_SERIAL_NUMBER); } + + public void configFeedback() { + + hubSwitchingTrigger + .whileTrue( + Commands.run(() -> conDriver.setRumble(RumbleType.kRightRumble, + ConstRumble.SHIFT_CHANGE_RUMBLE), telemetryInstance)) + .onFalse(Commands.runOnce(() -> conDriver.setRumble(RumbleType.kRightRumble, + ConstRumble.RUMBLE_OFF), telemetryInstance)); + + isOurShiftFirstTrigger + .whileTrue(Commands.run(() -> { + double t = Timer.getFPGATimestamp(); // seconds since FPGA boot + boolean on = ((int) Math.floor(t) % 2) == 0; // toggle every 1 second + conDriver.setRumble(RumbleType.kLeftRumble, + on ? ConstRumble.OUR_SHIFT_FIRST_RUMBLE : ConstRumble.RUMBLE_OFF); + }, telemetryInstance)) + .onFalse(Commands.runOnce(() -> conDriver.setRumble(RumbleType.kLeftRumble, ConstRumble.RUMBLE_OFF), + telemetryInstance)); + // Add feedback bindings here if needed + } } diff --git a/src/main/java/frc/robot/constants/ConstRumble.java b/src/main/java/frc/robot/constants/ConstRumble.java new file mode 100644 index 0000000..351b1bb --- /dev/null +++ b/src/main/java/frc/robot/constants/ConstRumble.java @@ -0,0 +1,15 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.constants; + +/** Add your docs here. */ +public class ConstRumble { + + public static final double TEST_RUMBLE = 1; + public static final double READY_TO_SHOOT_RUMBLE = 0.5; + public static final double SHIFT_CHANGE_RUMBLE = 1; + public static final double RUMBLE_OFF = 0.0; + public static final double OUR_SHIFT_FIRST_RUMBLE = 1; +} diff --git a/src/main/java/frc/robot/subsystems/Telemetry.java b/src/main/java/frc/robot/subsystems/Telemetry.java index b0e6699..e7fe373 100644 --- a/src/main/java/frc/robot/subsystems/Telemetry.java +++ b/src/main/java/frc/robot/subsystems/Telemetry.java @@ -4,27 +4,209 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Value; + +import java.util.Currency; +import java.util.Optional; + import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; +import edu.wpi.first.wpilibj.RobotController; @Logged public class Telemetry extends SubsystemBase { - /** Creates a new telemetry. */ + /** Creates a new Telemetry. */ + double timeBefore = 6; + double shift1Time = 105; + double autoTime = 140; + double transitionShiftTime = 130; + double shift2Time = 80; + double shift3Time = 55; + double shift4Time = 30; + double endgameTime = 0; + + private String gameData = ""; + public Telemetry() { } @Override public void periodic() { // This method will be called once per scheduler run + if (gameData.isEmpty()) { + gameData = DriverStation.getGameSpecificMessage(); + } + + } + + public boolean redIsNotFirst() { + if (gameData.isEmpty()) { + return true; + } + boolean redInactiveFirst = false; + switch (gameData.charAt(0)) { + case 'R' -> redInactiveFirst = true; + case 'B' -> redInactiveFirst = false; + default -> { + // If we have invalid game data, assume hub is active. + return true; + } + } + return redInactiveFirst; } public double getMatchTime() { return DriverStation.getMatchTime(); } + public double getMatchPeriodTime() { + double matchTime = getMatchTime(); + if (matchTime > autoTime) { + return matchTime - autoTime; + } else if (matchTime > transitionShiftTime) { + return matchTime - transitionShiftTime; + } else if (matchTime > shift1Time) { + return matchTime - shift1Time; + } else if (matchTime > shift2Time) { + return matchTime - shift2Time; + } else if (matchTime > shift3Time) { + return matchTime - shift3Time; + } else if (matchTime > shift4Time) { + return matchTime - shift4Time; + } else if (matchTime > endgameTime) { + return matchTime; + } else { + return 0; + } + } + + public enum MatchPeriods { + AUTO, + TRANSITION_SHIFT, + SHIFT_1, + SHIFT_2, + SHIFT_3, + SHIFT_4, + ENDGAME, + POST_MATCH + } + + public String getMatchPeriod() { + if (getMatchTime() > autoTime) { + return MatchPeriods.AUTO.toString(); + } else if (getMatchTime() > transitionShiftTime) { + return MatchPeriods.TRANSITION_SHIFT.toString(); + } else if (getMatchTime() > shift1Time) { + return MatchPeriods.SHIFT_1.toString(); + } else if (getMatchTime() > shift2Time) { + return MatchPeriods.SHIFT_2.toString(); + } else if (getMatchTime() > shift3Time) { + return MatchPeriods.SHIFT_3.toString(); + } else if (getMatchTime() > shift4Time) { + return MatchPeriods.SHIFT_4.toString(); + } else if (getMatchTime() > endgameTime) { + return MatchPeriods.ENDGAME.toString(); + } else { + return MatchPeriods.POST_MATCH.toString(); + } + } + + public boolean hubsIsSwitching() { + double matchTime = getMatchTime(); + if ((matchTime < transitionShiftTime + timeBefore && matchTime > transitionShiftTime) + || (matchTime < shift1Time + timeBefore && matchTime > shift1Time) + || (matchTime < shift2Time + timeBefore && matchTime > shift2Time) + || (matchTime < shift3Time + timeBefore && matchTime > shift3Time) + || (matchTime < shift4Time + timeBefore && matchTime > shift4Time)) { + return true; + } + return false; + } + + public boolean isHubActive() { + Optional alliance = DriverStation.getAlliance(); + // If we have no alliance, we cannot be enabled, therefore no hub. + if (alliance.isEmpty()) { + return false; + } + // Hub is always enabled in autonomous. + if (DriverStation.isAutonomousEnabled()) { + return true; + } + // At this point, if we're not teleop enabled, there is no hub. + if (!DriverStation.isTeleopEnabled()) { + return false; + } + + // We're teleop enabled, compute. + double matchTime = DriverStation.getMatchTime(); + // If we have no game data, we cannot compute, assume hub is active, as its + // likely early in teleop. + if (gameData.isEmpty()) { + return true; + } + boolean redInactiveFirst = false; + switch (gameData.charAt(0)) { + case 'R' -> redInactiveFirst = true; + case 'B' -> redInactiveFirst = false; + default -> { + // If we have invalid game data, assume hub is active. + return true; + } + } + + // Shift was is active for blue if red won auto, or red if blue won auto. + boolean shift1Active = switch (alliance.get()) { + case Red -> !redInactiveFirst; + case Blue -> redInactiveFirst; + }; + + if (matchTime > transitionShiftTime) { + // Transition shift, hub is active. + return true; + } else if (matchTime > shift1Time) { + // Shift 1 + return shift1Active; + } else if (matchTime > shift2Time) { + // Shift 2 + return !shift1Active; + } else if (matchTime > shift3Time) { + // Shift 3 + return shift1Active; + } else if (matchTime > shift4Time) { + // Shift 4 + return !shift1Active; + } else { + // End game, hub always active. + return true; + } + } + + public boolean ourShiftFirst() { + Optional alliance = DriverStation.getAlliance(); + + if (alliance.isEmpty()) { + return false; + } + if (gameData.isEmpty()) { + return false; + } + + if (((alliance.get().equals(Alliance.Red) && redIsNotFirst() == false) + || + (alliance.get().equals(Alliance.Blue) && redIsNotFirst() == true)) + && getMatchTime() < autoTime + && getMatchTime() > transitionShiftTime) { + return true; + } + return false; + } + public Voltage batteryVoltage() { return RobotController.getMeasureBatteryVoltage(); } @@ -32,4 +214,5 @@ public Voltage batteryVoltage() { public boolean isBrownedOut() { return RobotController.isBrownedOut(); } -} + +} \ No newline at end of file