Merge pull request #110 from midtownrobotics/dev

Dev

.SysId/sysid.json

@@ -0,0 +1 @@
+{}

.gitignore

@@ -1,6 +1,10 @@
 # This gitignore has been specially created by the WPILib team.
 # If you remove items from this file, intellisense might break.
 
+# Panjabi MC IS the G.O.A.T.
+
+BuildConstants.java
+
 ### C++ ###
 # Prerequisites
 *.d

src/main/java/frc/robot/BuildConstants.java

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2024-Offseason";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 200;
-  public static final String GIT_SHA = "0fd5eba2e63c561a4c2921b9c8ff214a9dd32ac6";
-  public static final String GIT_DATE = "2024-10-19 16:45:45 GMT";
-  public static final String GIT_BRANCH = "GRITS-Ankit-5am";
-  public static final String BUILD_DATE = "2024-10-19 18:52:32 GMT";
-  public static final long BUILD_UNIX_TIME = 1729363952616L;
+  public static final int GIT_REVISION = 204;
+  public static final String GIT_SHA = "d7243b7c1c97d3381ea8662f67d91dc0669ab0bb";
+  public static final String GIT_DATE = "2024-10-26 01:54:30 GMT";
+  public static final String GIT_BRANCH = "63-update-revctre-classes";
+  public static final String BUILD_DATE = "2024-10-28 23:30:20 GMT";
+  public static final long BUILD_UNIX_TIME = 1730158220273L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc/robot/Constants.java

@@ -13,6 +13,7 @@ public final class Constants {
     public static final double JOYSTICK_THRESHOLD = 0.1;
     public static final double CONTROL_LIMITER = 1;
 	public static final double THEORETICAL_RESTING_VOLTAGE = 12;
+    public static final double RUMBLE_DURATION = 1;
 
   public static enum Mode {
     REAL,

src/main/java/frc/robot/LimelightHelpers.java

@@ -20,8 +20,8 @@
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.TimestampedDoubleArray;
-import frc.robot.LimelightHelpers.LimelightResults;
-import frc.robot.LimelightHelpers.PoseEstimate;
+// import frc.robot.LimelightHelpers.LimelightResults;
+// import frc.robot.LimelightHelpers.PoseEstimate;
 import java.io.IOException;
 import java.net.HttpURLConnection;
 import java.net.MalformedURLException;
@@ -604,32 +604,32 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr
         pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials);
   }
 
-  private static RawFiducial[] getRawFiducials(String limelightName) {
-    var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials");
-    var rawFiducialArray = entry.getDoubleArray(new double[0]);
-    int valsPerEntry = 7;
-    if (rawFiducialArray.length % valsPerEntry != 0) {
-      return new RawFiducial[0];
-    }
-
-    int numFiducials = rawFiducialArray.length / valsPerEntry;
-    RawFiducial[] rawFiducials = new RawFiducial[numFiducials];
-
-    for (int i = 0; i < numFiducials; i++) {
-      int baseIndex = i * valsPerEntry;
-      int id = (int) extractArrayEntry(rawFiducialArray, baseIndex);
-      double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1);
-      double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2);
-      double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3);
-      double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4);
-      double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5);
-      double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6);
-
-      rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
-    }
-
-    return rawFiducials;
-  }
+  // private static RawFiducial[] getRawFiducials(String limelightName) {
+  //   var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials");
+  //   var rawFiducialArray = entry.getDoubleArray(new double[0]);
+  //   int valsPerEntry = 7;
+  //   if (rawFiducialArray.length % valsPerEntry != 0) {
+  //     return new RawFiducial[0];
+  //   }
+
+  //   int numFiducials = rawFiducialArray.length / valsPerEntry;
+  //   RawFiducial[] rawFiducials = new RawFiducial[numFiducials];
+
+  //   for (int i = 0; i < numFiducials; i++) {
+  //     int baseIndex = i * valsPerEntry;
+  //     int id = (int) extractArrayEntry(rawFiducialArray, baseIndex);
+  //     double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1);
+  //     double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2);
+  //     double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3);
+  //     double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4);
+  //     double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5);
+  //     double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6);
+
+  //     rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
+  //   }
+
+  //   return rawFiducials;
+  // }
 
   public static RawDetection[] getRawDetections(String limelightName) {
     var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections");

src/main/java/frc/robot/Robot.java

@@ -14,7 +14,6 @@
 import com.pathplanner.lib.commands.PathfindingCommand;
 
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -88,17 +87,6 @@ public void autonomousInit() {
     if (autonCommand != null) {
       autonCommand.schedule();
     }
-    String currentAuton = m_robotContainer.getAutonFactory().getAutonCommandString();
-    double desiredHeading = 0;
-    if (currentAuton.toLowerCase().contains("amp")) {
-      desiredHeading = 60;
-    } else if (currentAuton.toLowerCase().contains("source")) {
-      desiredHeading = -60;
-    }
-    if (Alliance.Red.equals(DriverStation.getAlliance().get())) {
-      desiredHeading *= -1;
-    }
-    m_robotContainer.getDrivetrain().resetHeading(desiredHeading);
   }
 
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -9,8 +9,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.networktables.NetworkTableInstance;
-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.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -118,6 +116,7 @@ private void configureBindings() {
               if (drivingMode != null && drivingMode.get() != null && drivingMode.get().equals("robot")) {
                 pigeonValue = 0;
               }
+
               drivetrain.setDriverDesired(
                   ChassisSpeeds.fromFieldRelativeSpeeds(
                       driverX, driverY, driverRot, Rotation2d.fromDegrees(pigeonValue)));
@@ -143,8 +142,13 @@ private void configureBindings() {
             },
             climber));
 
-    driver.a().onTrue(new InstantCommand(() -> drivetrain.resetHeading()));
-    // Note: These probably do not need to require drivetrain
+    driver
+        .a()
+        .onTrue(
+          new InstantCommand(
+            () -> drivetrain.resetHeading()
+            ));
+
     driver
         .x()
         .whileTrue(
@@ -169,7 +173,10 @@ private void configureBindings() {
     driver
         .leftTrigger()
         .whileTrue(
-            new StartEndCommand(() -> drivetrain.setBoost(true), () -> drivetrain.setBoost(false)));
+            new StartEndCommand(
+                () -> drivetrain.setBoost(true), 
+                () -> drivetrain.setBoost(false)
+                ));
 
     driver
         .b()
@@ -241,19 +248,26 @@ private void configureBindings() {
     //             intake,
     //             shooter));
     operator
-    .leftTrigger()
-    .whileTrue(
-        new StartEndCommand(
-            () -> {
-              intake.setState(IntakeState.VOMITING);
-              shooter.setState(ShooterState.VOMITING);
-            },
-            () -> {
-              shooter.setState(ShooterState.IDLE);
-              intake.setState(IntakeState.IDLE);
-            },
-            intake,
-            shooter));
+        .leftBumper()
+        .whileTrue(
+            new StartEndCommand(
+                () -> {intake.setState(IntakeState.VOMITING);
+                       shooter.setState(ShooterState.VOMITING);},
+                () -> {intake.setState(IntakeState.IDLE);
+                       shooter.setState(ShooterState.IDLE);},
+                intake,
+                shooter));
+
+    operator
+        .leftTrigger()
+        .whileTrue(
+            new StartEndCommand(
+                () -> {intake.setState(IntakeState.VOMITING);
+                       shooter.setState(ShooterState.VOMITING);},
+                () -> {intake.setState(IntakeState.IDLE);
+                       shooter.setState(ShooterState.IDLE);},
+                intake,
+                shooter));
 
     operator
         .rightTrigger()
@@ -354,6 +368,17 @@ public void initializeSubsystems() {
 
     shooter = new Shooter(flywheelIO, pivotIO, feederIO, limelight);
 
+    // Beam break
+    BeamBreakIO beamBreakIO;
+
+    if (Constants.getMode() == Constants.Mode.REAL) {
+      beamBreakIO = new BeamBreakIODIO(IntakePorts.beamBreak);
+    } else {
+      beamBreakIO = new BeamBreakIOSim();
+    }
+
+    beamBreak = new BeamBreak(beamBreakIO);
+
     // Intake
 
     RollerIO rollerIO;
@@ -364,35 +389,20 @@ public void initializeSubsystems() {
       rollerIO = new RollerIOSim();
     }
 
-    intake = new Intake(rollerIO);
+    intake = new Intake(rollerIO, beamBreak);
 
     // Climber
 
     ClimberIO climberIO;
 
     if (Constants.getMode() == Constants.Mode.REAL) {
-      climberIO =
-          new ClimberIONeo(Ports.ClimberPorts.leftClimberID, Ports.ClimberPorts.rightClimberID);
+      climberIO = new ClimberIONeo(Ports.ClimberPorts.leftClimberID, Ports.ClimberPorts.rightClimberID);
     } else {
       climberIO = new ClimberIOSim();
     }
 
     climber = new Climber(operator, climberIO);
 
-    // BeamBreak
-
-    BeamBreakIO beamBreakIO;
-
-    if (Constants.getMode() == Constants.Mode.REAL) {
-      beamBreakIO = new BeamBreakIODIO(IntakePorts.beamBreak);
-    } else {
-      beamBreakIO = new BeamBreakIOSim();
-    }
-
-    beamBreak =
-        new BeamBreak(
-            beamBreakIO, robotState, Ports.driverControllerPort, Ports.operatorControllerPort);
-
     // Drivetrain
 
     // if (Constants.USE_KRAKEN_DRIVETRAIN.get()) { // default value is false which means neo is
@@ -408,13 +418,11 @@ public void initializeSubsystems() {
     }
 
     // Robot State
-    robotState = new RobotState(shooter, climber, intake, drivetrain);
-    beamBreak.setRobotState(robotState);
+    robotState = new RobotState(shooter, climber, intake, drivetrain, beamBreak, Ports.driverControllerPort, Ports.operatorControllerPort);
 
     drivingMode = new LoggedDashboardChooser<String>("Driving Mode");
     drivingMode.addDefaultOption("Field Relative", "field");
     drivingMode.addOption("Robot Relative", "robot");
-
   }
 
   public RobotContainer() {

src/main/java/frc/robot/RobotState.java

@@ -1,7 +1,7 @@
 package frc.robot;
 
+import frc.robot.subsystems.BeamBreak.BeamBreak;
 import org.littletonrobotics.junction.Logger;
-
 import frc.robot.Constants.ShooterConstants;
 import frc.robot.subsystems.Climber.Climber;
 import frc.robot.subsystems.Drivetrain.Drivetrain;
@@ -10,8 +10,13 @@
 import frc.robot.subsystems.Intake.Intake.IntakeState;
 import frc.robot.subsystems.Shooter.Shooter;
 import frc.robot.subsystems.Shooter.Shooter.ShooterState;
+import frc.robot.utils.IOProtectionXboxController;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 
 public class RobotState {
+  public BeamBreak beamBreak;
+  public IOProtectionXboxController driver; 
+  public IOProtectionXboxController operator;
   public Shooter shooter;
   public Intake intake;
   public Drivetrain drive;
@@ -39,7 +44,14 @@ public enum State {
   // private final LoggedDashboardChooser<State> stateChooser = new LoggedDashboardChooser<>("Robot
   // State");
 
-  public RobotState(Shooter shooter, Climber climber, Intake intake, Drivetrain drive) {
+  public RobotState(Shooter shooter, 
+                    Climber climber, 
+                    Intake intake, 
+                    Drivetrain drive, 
+                    BeamBreak beamBreak, 
+                    int driverPort, 
+                    int operatorPort) {
+
     // stateChooser.addOption("AMP", State.AMP);
     // stateChooser.addOption("AMP_REVVING", State.AMP_REVVING);
     // stateChooser.addOption("SUBWOOFER", State.SUBWOOFER);
@@ -53,6 +65,9 @@ public RobotState(Shooter shooter, Climber climber, Intake intake, Drivetrain dr
     this.shooter = shooter;
     this.intake = intake;
     this.drive = drive;
+    this.beamBreak = beamBreak;
+    this.driver = new IOProtectionXboxController(driverPort);
+    this.operator = new IOProtectionXboxController(operatorPort);
   }
 
   public State currentState = State.IDLE;
@@ -137,7 +152,7 @@ public void updateState() {
         break;
       case INTAKING:
         shooter.setState(ShooterState.INTAKING);
-        intake.setState(IntakeState.INTAKING);
+        intake.setState(IntakeState.INTAKING); 
         break;
       case NOTE_HELD:
         shooter.setState(ShooterState.IDLE);
@@ -156,6 +171,8 @@ public void updateState() {
       default:
         break;
     }
+
+    setControllerRumble(); 
   }
 
   public ShooterState getShooterState() {
@@ -177,4 +194,16 @@ public void onTeleopInit() {
     shooter.setState(ShooterState.IDLE);
     intake.setState(IntakeState.IDLE);
   }
-}
+
+  public void setControllerRumble(){
+    if (beamBreak.isBroken() && 
+        edu.wpi.first.wpilibj.RobotState.isTeleop() && 
+        Constants.RUMBLE_DURATION > beamBreak.getBrokenTime()) {
+      driver.setRumble(RumbleType.kBothRumble, 1);
+      operator.setRumble(RumbleType.kBothRumble, 1);
+    } else {
+      driver.setRumble(RumbleType.kBothRumble, 0);
+      operator.setRumble(RumbleType.kBothRumble, 0); 
+    }
+  }
+}

src/main/java/frc/robot/commands/auton/AlignWithSpeaker.java

@@ -1,7 +1,6 @@
 package frc.robot.commands.auton;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.RobotState;
 import frc.robot.Constants.AutonConstants;
 import frc.robot.subsystems.Drivetrain.Drivetrain;
 import frc.robot.subsystems.Drivetrain.Drivetrain.DriveState;