Initial Drive Subsystem and Path Handler

.vscode/settings.json

@@ -56,5 +56,6 @@
     "edu.wpi.first.math.proto.*",
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
-  ]
+  ],
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"
 }

src/main/deploy/choreo/2025-project.chor

@@ -9,22 +9,22 @@
  "config":{
   "frontLeft":{
    "x":{
-    "exp":"11 in",
-    "val":0.2794
+    "exp":"12.125 in",
+    "val":0.307975
    },
    "y":{
-    "exp":"11 in",
-    "val":0.2794
+    "exp":"12.125 in",
+    "val":0.307975
    }
   },
   "backLeft":{
    "x":{
-    "exp":"-11 in",
-    "val":-0.2794
+    "exp":"-12.125 in",
+    "val":-0.307975
    },
    "y":{
-    "exp":"11 in",
-    "val":0.2794
+    "exp":"12.125 in",
+    "val":0.307975
    }
   },
   "mass":{
@@ -40,8 +40,8 @@
    "val":6.5
   },
   "radius":{
-   "exp":"2 in",
-   "val":0.0508
+   "exp":"1.75 in",
+   "val":0.04445
   },
   "vmax":{
    "exp":"6000 RPM",
@@ -52,21 +52,21 @@
    "val":1.2
   },
   "cof":{
-   "exp":"1.5",
-   "val":1.5
+   "exp":"1.1",
+   "val":1.1
   },
   "bumper":{
    "front":{
-    "exp":"16 in",
-    "val":0.4064
+    "exp":"35.875 in",
+    "val":0.911225
    },
    "side":{
-    "exp":"16 in",
-    "val":0.4064
+    "exp":"35.875 in",
+    "val":0.911225
    },
    "back":{
-    "exp":"16 in",
-    "val":0.4064
+    "exp":"35.875 in",
+    "val":0.911225
    }
   },
   "differentialTrackWidth":{

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

@@ -0,0 +1,5 @@
+package frc.robot.commands.auton;
+
+public interface AutoCommandInterface {
+  public void reassignAlliance();
+}

src/main/java/frc/robot/commands/drive/DriveAutonCommand.java

@@ -0,0 +1,106 @@
+package frc.robot.commands.drive;
+
+import choreo.Choreo;
+import choreo.trajectory.SwerveSample;
+import choreo.trajectory.Trajectory;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.commands.auton.AutoCommandInterface;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import java.util.Optional;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+public class DriveAutonCommand extends Command implements AutoCommandInterface {
+  private final DriveSubsystem driveSubsystem;
+  private Trajectory<SwerveSample> trajectory;
+  private final Timer timer = new Timer();
+  private static final Logger logger = LoggerFactory.getLogger(DriveAutonCommand.class);
+  private boolean isTherePath = false;
+  private String trajectoryName;
+  private boolean mirrorTrajectory = false;
+
+  private boolean resetOdometry;
+  private boolean lastPath;
+
+  public DriveAutonCommand(
+      DriveSubsystem driveSubsystem,
+      String trajectoryName,
+      boolean lastPath,
+      boolean resetOdometry) {
+
+    addRequirements(driveSubsystem);
+    this.driveSubsystem = driveSubsystem;
+    this.resetOdometry = resetOdometry;
+    this.lastPath = lastPath;
+    this.trajectoryName = trajectoryName;
+    Optional<Trajectory<SwerveSample>> tempTrajectory = Choreo.loadTrajectory(trajectoryName);
+    if (tempTrajectory.isPresent()) {
+      trajectory = tempTrajectory.get();
+      isTherePath = true;
+    } else {
+      logger.error("Trajectory {} not found", trajectoryName);
+      isTherePath = false;
+    }
+    timer.start();
+  }
+
+  @Override
+  public void reassignAlliance() {
+    mirrorTrajectory = driveSubsystem.shouldFlip();
+  }
+
+  @Override
+  public void initialize() {
+    if (isTherePath) {
+      driveSubsystem.setAutoDebugMsg("Initialize " + trajectoryName);
+      Pose2d initialPose = new Pose2d();
+      initialPose = trajectory.getInitialPose(mirrorTrajectory).get();
+      if (resetOdometry) {
+        driveSubsystem.resetOdometry(initialPose);
+      }
+      driveSubsystem.setEnableHolo(true);
+      // driveSubsystem.recordAutoTrajectory(trajectory);
+      driveSubsystem.resetHolonomicController();
+      driveSubsystem.grapherTrajectoryActive(true);
+      timer.reset();
+      logger.info("Begin Trajectory: {}", trajectoryName);
+      SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
+      driveSubsystem.calculateController(desiredState);
+    }
+  }
+
+  @Override
+  public void execute() {
+    if (isTherePath) {
+      SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
+      driveSubsystem.calculateController(desiredState);
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (!isTherePath) {
+      return false;
+    }
+    return timer.hasElapsed(trajectory.getTotalTime());
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    driveSubsystem.setEnableHolo(false);
+    // driveSubsystem.recordAutoTrajectory(null);
+
+    if (!interrupted && !lastPath) {
+      driveSubsystem.calculateController(
+          trajectory.sampleAt(trajectory.getTotalTime(), mirrorTrajectory).get());
+    } else {
+      driveSubsystem.drive(0, 0, 0);
+    }
+
+    driveSubsystem.grapherTrajectoryActive(false);
+    logger.info("End Trajectory {}: {}", trajectoryName, timer.get());
+    driveSubsystem.setAutoDebugMsg("End " + trajectoryName);
+  }
+}

src/main/java/frc/robot/constants/DriveConstants.java

@@ -1,3 +1,151 @@
 package frc.robot.constants;
 
-public class DriveConstants {}
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
+import com.ctre.phoenix.sensors.SensorVelocityMeasPeriod;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.geometry.Translation2d;
+
+public class DriveConstants {
+  public static final double kDriveGearRatio = 6.5;
+  public static final double kWheelDiameterInches = 4.0;
+  public static final double kMaxSpeedMetersPerSecond = 12.0;
+  public static final double kSpeedStillThreshold = 0.1; // meters per second
+  public static final double kGyroRateStillThreshold = 10.0; // 25  5 degrees per second
+  public static final double kGyroDifferentThreshold = 5.0; // 5 degrees
+  public static final int kGyroDifferentCount = 3;
+
+  public static final double kRobotLength = 22.0;
+  public static final double kRobotWidth = 22.0;
+  public static final double kFieldMaxX = 690.0;
+
+  public static final double kPOmega = 4.5;
+  public static final double kIOmega = 0.0;
+  public static final double kDOmega = 0.0;
+  public static final double kMaxVelOmega =
+      (kMaxSpeedMetersPerSecond / Math.hypot(kRobotWidth / 2.0, kRobotLength / 2.0)) / 2.0;
+  public static final double kMaxAccelOmega = 5.0;
+
+  public static final double kPHolonomic = 3.0; // was 3
+  public static final double kIHolonomic = 0.0000;
+  public static final double kDHolonomic = 0.00; // kPHolonomic/100
+
+  public static Translation2d[] getWheelLocationMeters() {
+    final double x = kRobotLength / 2.0; // front-back, was ROBOT_LENGTH
+    final double y = kRobotWidth / 2.0; // left-right, was ROBOT_WIDTH
+    Translation2d[] locs = new Translation2d[4];
+    locs[0] = new Translation2d(x, y); // left front
+    locs[1] = new Translation2d(x, -y); // right front
+    locs[2] = new Translation2d(-x, y); // left rear
+    locs[3] = new Translation2d(-x, -y); // right rear
+    return locs;
+  }
+
+  // temp stuff
+  public static final int kTempAvgCount = 25;
+  public static final double kTripTemp = 1300;
+  public static final double kRecoverTemp = 1290;
+  public static final double kNotifyTemp = 1295;
+
+  public static TalonSRXConfiguration
+      getAzimuthTalonConfig() { // will be changed to a TalonFXConfiguration
+    // constructor sets encoder to Quad/CTRE_MagEncoder_Relative
+    TalonSRXConfiguration azimuthConfig = new TalonSRXConfiguration();
+
+    azimuthConfig.primaryPID.selectedFeedbackCoefficient = 1.0;
+    azimuthConfig.auxiliaryPID.selectedFeedbackSensor = FeedbackDevice.None;
+
+    azimuthConfig.forwardLimitSwitchSource = LimitSwitchSource.Deactivated;
+    azimuthConfig.reverseLimitSwitchSource = LimitSwitchSource.Deactivated;
+
+    azimuthConfig.continuousCurrentLimit = 10;
+    azimuthConfig.peakCurrentDuration = 0;
+    azimuthConfig.peakCurrentLimit = 0;
+
+    azimuthConfig.slot0.kP = 15.0;
+    azimuthConfig.slot0.kI = 0.0;
+    azimuthConfig.slot0.kD = 150.0;
+    azimuthConfig.slot0.kF = 1.0;
+    azimuthConfig.slot0.integralZone = 0;
+    azimuthConfig.slot0.allowableClosedloopError = 0;
+    azimuthConfig.slot0.maxIntegralAccumulator = 0;
+
+    azimuthConfig.motionCruiseVelocity = 800;
+    azimuthConfig.motionAcceleration = 10_000;
+    azimuthConfig.velocityMeasurementWindow = 64;
+    azimuthConfig.velocityMeasurementPeriod = SensorVelocityMeasPeriod.Period_100Ms;
+    azimuthConfig.voltageCompSaturation = 12;
+    azimuthConfig.voltageMeasurementFilter = 32;
+    azimuthConfig.neutralDeadband = 0.04;
+    return azimuthConfig;
+  }
+
+  public static TalonFXConfiguration getDriveTalonConfig() {
+    TalonFXConfiguration driveConfig = new TalonFXConfiguration();
+
+    CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
+    currentConfig.SupplyCurrentLimit = 60;
+
+    currentConfig.StatorCurrentLimit = 140;
+
+    currentConfig.SupplyCurrentLimitEnable = true;
+    currentConfig.StatorCurrentLimitEnable = true;
+
+    driveConfig.CurrentLimits = currentConfig;
+
+    Slot0Configs slot0Config = new Slot0Configs();
+    slot0Config.kP = 0.5; // 0.16 using phoenix 6 migrate
+    slot0Config.kI = 0.5; // 0.0002 using phoenix 6 migrate
+    slot0Config.kD = 0.0;
+    slot0Config.kV = 0.12; // 0.047 using phoenix 6 migrate
+    driveConfig.Slot0 = slot0Config;
+
+    MotorOutputConfigs motorConfigs = new MotorOutputConfigs();
+    motorConfigs.DutyCycleNeutralDeadband = 0.01;
+    motorConfigs.NeutralMode = NeutralModeValue.Brake;
+    driveConfig.MotorOutput = motorConfigs;
+
+    return driveConfig;
+  }
+
+  public static final int kPigeonCanID = 1000; // fix later
+
+  public static Pigeon2Configuration getPigeon2Configuration() {
+    Pigeon2Configuration config = new Pigeon2Configuration();
+
+    config.MountPose.MountPoseYaw = -90.0;
+    config.MountPose.MountPoseRoll = 0.0;
+    config.MountPose.MountPosePitch = 0.0;
+
+    config.GyroTrim.GyroScalarX = 0.0;
+    config.GyroTrim.GyroScalarY = 0.0;
+    config.GyroTrim.GyroScalarZ = -2.12;
+
+    return config;
+  }
+
+  public static CurrentLimitsConfigs getSafeDriveLimits() {
+    CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
+    currentConfig.SupplyCurrentLimit = 30;
+    currentConfig.SupplyCurrentLimitEnable = true;
+    currentConfig.StatorCurrentLimitEnable = false;
+    return currentConfig;
+  }
+
+  public static CurrentLimitsConfigs getNormDriveLimits() {
+    CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
+    currentConfig.SupplyCurrentLimit = 30;
+
+    currentConfig.StatorCurrentLimit = 140;
+
+    currentConfig.SupplyCurrentLimitEnable = true;
+    currentConfig.StatorCurrentLimitEnable = true;
+    return currentConfig;
+  }
+}

src/main/java/frc/robot/constants/RobotConstants.java

@@ -1,3 +1,5 @@
 package frc.robot.constants;
 
-public class RobotConstants {}
+public class RobotConstants {
+  public static final int kTalonConfigTimeout = 10; // ms
+}

src/main/java/frc/robot/constants/RobotStateConstants.java

@@ -1,3 +1,5 @@
 package frc.robot.constants;
 
-public class RobotStateConstants {}
+public class RobotStateConstants {
+  public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0};
+}

src/main/java/frc/robot/constants/TagServoingConstants.java

@@ -0,0 +1,5 @@
+package frc.robot.constants;
+
+public class TagServoingConstants {
+  public static final double kAngleCloseEnough = 3.0;
+}