Merge branch 'champs' of https://github.com/team4909/2024-Crescendo i…

…nto champs

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

@@ -7,23 +7,14 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Filesystem;
-import edu.wpi.first.wpilibj.RobotController;
 import java.io.IOException;
 import java.nio.file.Path;
 import java.util.function.BooleanSupplier;
 
 public final class Constants {
   public static final Mode kCurrentMode = Mode.kReal;
-  public static final RobotName kRobot;
-
-  static {
-    final String rioSerialNumber = RobotController.getSerialNumber();
-    if (rioSerialNumber.equals(RobotName.kViper.rioSerialNumber)) kRobot = RobotName.kBlackMamba;
-    else if (rioSerialNumber.equals(RobotName.kBlackMamba.rioSerialNumber))
-      kRobot = RobotName.kBlackMamba;
-    else kRobot = RobotName.kBlackMamba;
-  }
 
+  // Whether or not the current robot is Viper or Black Mamba
   public static final boolean kIsViper = false;
   public static final boolean kIsSim = Constants.kCurrentMode.equals(Mode.kSim);
   public static final String kDrivetrainCanBus = "CANivore1";
@@ -58,15 +49,4 @@ public static enum Mode {
     kSim,
     kReplay
   }
-
-  public static enum RobotName {
-    kViper("032380FD"),
-    kBlackMamba("032243C9");
-
-    public final String rioSerialNumber;
-
-    private RobotName(String rioSerialNumber) {
-      this.rioSerialNumber = rioSerialNumber;
-    }
-  }
 }

src/main/java/frc/robot/PoseEstimation.java

@@ -16,6 +16,7 @@
 import java.util.function.BooleanSupplier;
 import org.littletonrobotics.junction.AutoLogOutput;
 
+// why is this in our code? Shou;dn't this be in a library we use?
 public class PoseEstimation {
 
   private static PoseEstimation m_instance;

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

@@ -17,6 +17,7 @@
 import frc.robot.arm.Arm;
 import frc.robot.arm.Arm.ArmSetpoints;
 import frc.robot.climber.Climber;
+import frc.robot.drivetrain.DriveToNote;
 import frc.robot.drivetrain.DriveToPose;
 import frc.robot.drivetrain.Drivetrain;
 import frc.robot.drivetrain.WheelRadiusCharacterization;

src/main/java/frc/robot/drivetrain/DriveToNote.java

@@ -0,0 +1,53 @@
+package frc.robot.drivetrain;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.intake.Intake;
+import frc.robot.lights.Lights;
+import frc.robot.vision.GamePieceDetection;
+
+public class DriveToNote extends Command {
+private Drivetrain m_drivetrain;
+private Lights m_lights;
+private GamePieceDetection m_gamePieceDetection;
+
+private PIDController m_thetaController;
+
+private double m_horizontalOffset;
+private double m_verticalOffset;
+
+public DriveToNote(Drivetrain drivetrain, Lights lights, GamePieceDetection gamePieceDetection, Intake intake) {
+m_drivetrain = drivetrain;
+m_lights = lights;
+m_gamePieceDetection = gamePieceDetection;
+
+m_thetaController = new PIDController(5.0, 0.0, 0.0);
+
+m_horizontalOffset = m_gamePieceDetection.getHorizontalOffsetDeg();
+m_verticalOffset = m_gamePieceDetection.getVerticalOffsetDeg();
+
+m_thetaController.setTolerance(1);
+}
+
+@Override
+public void initialize() {}
+
+@Override
+public void execute() {
+m_horizontalOffset = m_gamePieceDetection.getHorizontalOffsetDeg();
+m_verticalOffset = m_gamePieceDetection.getVerticalOffsetDeg();
+m_drivetrain.runVelocity(
+new ChassisSpeeds(0.0, 0.0, m_thetaController.calculate(m_horizontalOffset, 0)));
+// m_lights.setBlink(Color.kMidnightBlue);
+}
+
+@Override
+public void end(boolean interrupted) {}
+
+@Override
+public boolean isFinished() {
+return false;
+}
+}

src/main/java/frc/robot/drivetrain/Drivetrain.java

@@ -46,6 +46,8 @@ public class Drivetrain extends SubsystemBase {
 
   private static final double kTrackwidthMeters = Units.inchesToMeters(20.75);
   private static double kWheelbaseMeters = Units.inchesToMeters(15.75);
+
+  // @todo What order are these in? Is it Front Left then Clockwise
   private static final Translation2d[] m_modulePositions =
       new Translation2d[] {
         new Translation2d(kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0),
@@ -55,6 +57,7 @@ public class Drivetrain extends SubsystemBase {
       };
   public static final SwerveDriveKinematics swerveKinematics =
       new SwerveDriveKinematics(m_modulePositions);
+
   public static final double kDriveBaseRadius =
       Math.hypot(kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0);
   private static final LoggedTunableNumber speakerRangeMeters =
@@ -156,8 +159,7 @@ public void periodic() {
       SwerveModulePosition[] newModulePositions = new SwerveModulePosition[m_modules.length];
       SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4];
       for (int moduleIndex = 0; moduleIndex < m_modules.length; moduleIndex++) {
-        newModulePositions[moduleIndex] =
-            m_modules[moduleIndex].getOdometryPositions()[updateIndex];
+        newModulePositions[moduleIndex] = m_modules[moduleIndex].getOdometryPositions()[updateIndex];
         moduleDeltas[moduleIndex] =
             new SwerveModulePosition(
                 newModulePositions[moduleIndex].distanceMeters
@@ -170,6 +172,7 @@ public void periodic() {
       if (m_imuInputs.connected) {
         m_gyroRotation = m_imuInputs.odometryYawPositions[updateIndex];
       } else {
+        //when does this code run? Was this added when we had the pidgeon issue at worlds?
         final Twist2d twist = swerveKinematics.toTwist2d(moduleDeltas);
         m_gyroRotation = m_gyroRotation.plus(new Rotation2d(twist.dtheta));
       }
@@ -244,7 +247,8 @@ public Command joystickDrive(
    * drive, no path following)
    */
   public Command blankDrive() {
-    return this.run(() -> runVelocity(new ChassisSpeeds(0.0, 0.0, 0.0)));
+    return this.run(() -> 
+    runVelocity(new ChassisSpeeds(0.0, 0.0, 0.0)));
   }
 
   public Command zeroGyro() {

src/main/java/frc/robot/vision/GamePieceDetection.java

@@ -32,4 +32,12 @@ public void periodic() {
     Logger.recordOutput("GamePieceDetection/ValidTarget", hasValidTarget.getAsBoolean());
     Logger.recordOutput("GamePieceDetection/HorizontalErrorDeg", horizontalErrorDeg.getAsDouble());
   }
+
+  public double getHorizontalOffsetDeg() {
+    return horizontalErrorDeg.getAsDouble();
+  }
+
+  public double getVerticalOffsetDeg() {
+    return m_inputs.targetVerticalOffsetDegrees;
+  }
 }

src/main/java/frc/robot/vision/Vision.java

@@ -126,7 +126,7 @@ public void periodic() {
                         .ifPresent(
                             tagPose ->
                                 m_lastDetectionTimeIds.put(
-                                    target.getFiducialId(), Timer.getFPGATimestamp()));
+                                    target.getFiducialId(), Timer.getFPGATimestamp())); //shouldn't this be the timestamp of capture on the photonvision processor and not our timestamp
                   });
               m_allEstimatedPosesToLog.add(estimatedPose);
               m_newVisionUpdates.add(