diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fd96ee8..673d680 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,6 +26,7 @@ import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.frcteam3255.components.swerve.SN_SwerveConstants; +import au.grapplerobotics.LaserCan.TimingBudget; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -50,6 +51,16 @@ public final class Constants { public static final Measure MAX_VOLTAGE = Units.Volts.of(12); + public static final String[] PDH_DEVICES = { + "Back Right Steer (0)", "Back Right Drive (1)", "Radio Auxiliary Power (2)", "GrappleCAN (3)", "(4)", "LEDS (5)", + "Transfer Roller (6)", "Shooter Pivot(7)", + "Back Left Steer (8)", "Back Left Drive (9)", + "Front Left Steer (10)", "Front Left Drive (11)", "(12)", "Drainpipe (13)", "Elevator (14)", "Shooter Right (15)", + "Shooter Left (16)", "Intake (17)", "Front Right Steer (18)", "Front Right Drive (19)", + "Swerve CANCoders & Pigeon (20)", "Radio POE (21)", "RoboRIO Power (22)", "(23)" }; + + public static final boolean ENABLE_PDH_LOGGING = true; + public static class constControllers { public static final double DRIVER_LEFT_STICK_DEADBAND = 0.05; public static final boolean SILENCE_JOYSTICK_WARNINGS = true; @@ -59,6 +70,8 @@ public static class constControllers { public static final double DRIVER_GP_COLLECTED_RUMBLE = 0.3; public static final double OPERATOR_GP_COLLECTED_RUMBLE = 0.3; + + public static final boolean SOLO_DRIVER = false; } public static class constDrivetrain { @@ -619,6 +632,9 @@ public static class constIntake { public static class constTransfer { public static final boolean NOTE_SENSOR_INVERT = true; + public static final Measure PIECE_DETECTED_DIST_THRESH = Units.Millimeters.of(320); + public static final TimingBudget TIMING_BUDGET = TimingBudget.TIMING_BUDGET_20MS; + public static final InvertedValue MOTOR_INVERT = InvertedValue.Clockwise_Positive; public static final NeutralModeValue FEEDER_NEUTRAL_MODE = NeutralModeValue.Brake; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ac0ce8f..e771b69 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -50,6 +50,7 @@ public void robotInit() { public void robotPeriodic() { CommandScheduler.getInstance().run(); RobotContainer.AddVisionMeasurement().schedule(); + RobotContainer.logPDHValues(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fda8b04..79cf62b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; @@ -67,8 +69,9 @@ public class RobotContainer { public final static StateMachine subStateMachine = new StateMachine(subClimber, subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter); + private final Trigger falseTrigger = new Trigger(() -> false); private final Trigger gamePieceStoredTrigger = new Trigger(() -> subTransfer.getGamePieceStored()); - private final Trigger gamePieceCollectedTrigger = new Trigger(() -> subIntake.getGamePieceCollected()); + private final Trigger gamePieceCollectedTrigger = falseTrigger; private final BooleanSupplier readyToShootOperator = (() -> (subDrivetrain.isDrivetrainFacingSpeaker() || subDrivetrain.isDrivetrainFacingShuffle()) @@ -94,15 +97,29 @@ public class RobotContainer { SendableChooser autoChooser = new SendableChooser<>(); + private static PowerDistribution PDH = new PowerDistribution(1, ModuleType.kRev); + public RobotContainer() { conDriver.setLeftDeadband(constControllers.DRIVER_LEFT_STICK_DEADBAND); - subDrivetrain - .setDefaultCommand( - new Drive(subDrivetrain, subStateMachine, conDriver.axis_LeftY, conDriver.axis_LeftX, conDriver.axis_RightX, - conDriver.btn_LeftBumper, conDriver.btn_RightBumper, conDriver.btn_RightTrigger, - conDriver.btn_Y, conDriver.btn_B, conDriver.btn_A, - conDriver.btn_X, conDriver.btn_LeftTrigger)); + if (constControllers.SOLO_DRIVER) { + subDrivetrain + .setDefaultCommand( + new Drive(subDrivetrain, subStateMachine, conDriver.axis_LeftY, conDriver.axis_LeftX, + conDriver.axis_RightX, + conDriver.btn_LeftBumper, falseTrigger, falseTrigger, + falseTrigger, falseTrigger, falseTrigger, + falseTrigger, falseTrigger)); + + } else { + subDrivetrain + .setDefaultCommand( + new Drive(subDrivetrain, subStateMachine, conDriver.axis_LeftY, conDriver.axis_LeftX, + conDriver.axis_RightX, + conDriver.btn_LeftBumper, conDriver.btn_RightBumper, conDriver.btn_RightTrigger, + conDriver.btn_Y, conDriver.btn_B, conDriver.btn_A, + conDriver.btn_X, conDriver.btn_LeftTrigger)); + } // - Manual Triggers - gamePieceStoredTrigger @@ -153,7 +170,11 @@ public RobotContainer() { SmartDashboard.putNumber("Preload Only Delay", 0); - configureDriverBindings(conDriver); + if (constControllers.SOLO_DRIVER) { + configureSoloDriverBindings(conDriver); + } else { + configureDriverBindings(conDriver); + } configureOperatorBindings(conOperator); configureTestBindings(conTestOperator); @@ -171,6 +192,80 @@ private void configureDriverBindings(SN_XboxController controller) { .unless(() -> comIntakeSource.getIntakeSourceGamePiece())); } + private void configureSoloDriverBindings(SN_XboxController controller) { + // Reset Pose + controller.btn_Start.onTrue( + Commands.runOnce(() -> subDrivetrain.resetPoseToPose(constField.getFieldPositions().get()[6].toPose2d()))); + + // -- States -- + // Intake + controller.btn_LeftTrigger + .whileTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.INTAKING))) + .onFalse(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.NONE)) + .unless(gamePieceStoredTrigger)); + + // Shoot + controller.btn_RightTrigger.whileTrue( + Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.SHOOTING))) + .onFalse(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.NONE)) + .unless(gamePieceStoredTrigger)); + + // Prep with vision + controller.btn_RightBumper.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_VISION))) + .onTrue(Commands + .deferredProxy( + () -> subStateMachine.tryState(RobotState.PREP_VISION))); + + // Ejecting + controller.btn_Back.whileTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.EJECTING))) + .onFalse(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.NONE))); + + // Prep spike + controller.btn_X.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SPIKE))) + .onTrue( + Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.PREP_SPIKE))); + + controller.btn_B.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_AMP))) + .onTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.PREP_AMP))); + + controller.btn_Y.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SUB_BACKWARDS))) + .onTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.PREP_SUB_BACKWARDS))); + + // "Unalive Shooter" + controller.btn_A.onTrue( + Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.PREP_NONE)) + .alongWith(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_NONE)))) + .onFalse(Commands.runOnce(() -> subShooter.setShootingNeutralOutput())); + + // Prep subwoofer + controller.btn_South.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SPEAKER))) + .onTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.PREP_SPEAKER))); + + // Prep wing + controller.btn_North.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_WING))) + .onTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.PREP_WING))); + + // Prep shuffle + controller.btn_West.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SHUFFLE))) + .onTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.PREP_SHUFFLE))); + + // Game Piece Override + controller.btn_East.onTrue(Commands.runOnce(() -> subTransfer.setGamePieceCollected(true)) + .alongWith(Commands.runOnce(() -> subStateMachine.setRobotState(RobotState.STORE_FEEDER)))); + + } + private void configureOperatorBindings(SN_XboxController controller) { // -- States -- // Intake @@ -316,6 +411,26 @@ public static Command AddVisionMeasurement() { .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming).ignoringDisable(true); } + // -- PDH -- + /** + * Updates the values supplied to the PDH to SmartDashboard. Should be called + * periodically. + */ + public static void logPDHValues() { + SmartDashboard.putNumber("PDH/Input Voltage", PDH.getVoltage()); + SmartDashboard.putBoolean("PDH/Is Switchable Channel Powered", PDH.getSwitchableChannel()); + SmartDashboard.putNumber("PDH/Total Current", PDH.getTotalCurrent()); + SmartDashboard.putNumber("PDH/Total Power", PDH.getTotalPower()); + SmartDashboard.putNumber("PDH/Total Energy", PDH.getTotalEnergy()); + + if (Constants.ENABLE_PDH_LOGGING) { + for (int i = 0; i < Constants.PDH_DEVICES.length; i++) { + SmartDashboard.putNumber("PDH/" + Constants.PDH_DEVICES[i] + " Current", PDH.getCurrent(i)); + } + } + } + + // -- LEDS -- public void setDisabledLEDs() { subLEDs.setLEDAnimation(constLEDs.DISABLED_COLOR_1, 0); subLEDs.setLEDAnimation(constLEDs.DISABLED_COLOR_2, 1); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 39ae974..760d544 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -62,7 +62,7 @@ public static class mapClimber { // MOTORS: 50 -> 59 public static class mapTransfer { public static final int TRANSFER_MOTOR_CAN = 50; - public static final int NOTE_SENSOR_DIO = 0; + public static final int NOTE_SENSOR_CAN = 51; } public static class mapLEDs { diff --git a/src/main/java/frc/robot/subsystems/Transfer.java b/src/main/java/frc/robot/subsystems/Transfer.java index 88dfdb7..5e1a52e 100644 --- a/src/main/java/frc/robot/subsystems/Transfer.java +++ b/src/main/java/frc/robot/subsystems/Transfer.java @@ -8,7 +8,11 @@ import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.wpilibj.DigitalInput; +import au.grapplerobotics.ConfigurationFailedException; +import au.grapplerobotics.LaserCan; +import au.grapplerobotics.LaserCan.Measurement; +import au.grapplerobotics.LaserCan.TimingBudget; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.constTransfer; @@ -17,14 +21,14 @@ public class Transfer extends SubsystemBase { TalonFX feederMotor; TalonFXConfiguration feederConfig = new TalonFXConfiguration(); - DigitalInput noteSensor; + LaserCan noteSensor; boolean hasGamePiece; /** Creates a new Transfer. */ public Transfer() { feederMotor = new TalonFX(mapTransfer.TRANSFER_MOTOR_CAN, "rio"); - noteSensor = new DigitalInput(mapTransfer.NOTE_SENSOR_DIO); + noteSensor = new LaserCan(mapTransfer.NOTE_SENSOR_CAN); configure(); } @@ -40,6 +44,13 @@ public void configure() { feederConfig.CurrentLimits.SupplyTimeThreshold = constTransfer.CURRENT_TIME_THRESH; feederMotor.getConfigurator().apply(feederConfig); + + try { + noteSensor.setTimingBudget(constTransfer.TIMING_BUDGET); + } catch (ConfigurationFailedException e) { + System.out.println("LaserCAN could not be initialized :<"); + + } } public void setFeederSpeed(double speed) { @@ -51,8 +62,16 @@ public void setFeederNeutralOutput() { } public boolean getGamePieceStored() { - boolean noteSensorValue = (constTransfer.NOTE_SENSOR_INVERT) ? !noteSensor.get() : noteSensor.get(); - return (noteSensorValue || hasGamePiece); + Measurement measurement = noteSensor.getMeasurement(); + if (hasGamePiece) { + return hasGamePiece; + } + + if (measurement != null) { + return measurement.distance_mm <= constTransfer.PIECE_DETECTED_DIST_THRESH.in(Units.Millimeters); + } + + return false; } public void setGamePieceCollected(boolean isCollected) { @@ -63,5 +82,10 @@ public void setGamePieceCollected(boolean isCollected) { public void periodic() { // This method will be called once per scheduler run SmartDashboard.putBoolean("Game Piece Stored", getGamePieceStored()); + if (noteSensor.getMeasurement() != null) { + SmartDashboard.putNumber("Transfer/Laser Can/Distance", noteSensor.getMeasurement().distance_mm); + SmartDashboard.putNumber("Transfer/Laser Can/Ambient Light", noteSensor.getMeasurement().ambient); + SmartDashboard.putNumber("Transfer/Laser Can/Timing Budget", noteSensor.getMeasurement().budget_ms); + } } } diff --git a/vendordeps/libgrapplefrc2024.json b/vendordeps/libgrapplefrc2024.json new file mode 100644 index 0000000..cef1bde --- /dev/null +++ b/vendordeps/libgrapplefrc2024.json @@ -0,0 +1,74 @@ +{ + "fileName": "libgrapplefrc2024.json", + "name": "libgrapplefrc", + "version": "2024.3.1", + "frcYear": "2024", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ + "https://storage.googleapis.com/grapple-frc-maven" + ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2024.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2024.3.1" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2024.3.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2024.3.1", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2024.3.1", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file