LazerCAN, Solo Driving, & PDH Logging (#139)

* gp gaaejioerp hop ghzdf * LazerCAN works! (+ solo driver bindings) * add pdh logging * Update PDH names --------- Co-authored-by: Alena <[email protected]>
FRCTeam3255 · Nov 19, 2024 · 6005a7d · 6005a7d
1 parent 62ed075
commit 6005a7d
Show file tree

Hide file tree

Showing 6 changed files with 244 additions and 14 deletions.
diff --git 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<Voltage> 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<Distance> 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
@@ -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
@@ -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<Command> 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
@@ -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
@@ -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
@@ -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"
+            ]
+        }
+    ]
+}