Adding a mechanism to zero the modules without an absolute encoder

Reviewed-on: #1

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2026.1.1"
+    id "edu.wpi.first.GradleRIO" version "2026.2.1"
 }
 
 java {

src/main/deploy/pathplanner/paths/Example Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/settings.json

@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 74.088,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.048,
+  "driveGearing": 5.143,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}

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

@@ -4,9 +4,13 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.constants.AutoConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.Drivetrain;
 
@@ -15,11 +19,15 @@ public class RobotContainer {
 
   private CommandXboxController driver;
 
+  private SendableChooser<Command> autoChooser;
+
   public RobotContainer() {
     drivetrain = new Drivetrain();
 
     driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 
+    autoChooser = AutoBuilder.buildAutoChooser();
+
     configureBindings();
   }
 
@@ -35,6 +43,10 @@ public class RobotContainer {
   }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    if(AutoConstants.kAutoConfigOk) {
+      return autoChooser.getSelected();
+    } else {
+      return new PrintCommand("Robot Config loading failed, autonomous disabled");
+    }
   }
 }

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

@@ -0,0 +1,62 @@
+package frc.robot.constants;
+
+import java.io.IOException;
+
+import org.json.simple.parser.ParseException;
+
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.path.PathConstraints;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
+// TODO This is all hold over from 2025, does any of it need to change?
+public class AutoConstants {
+    public static final double kMaxSpeedMetersPerSecond = 5;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 4;
+    public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
+    public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI;
+
+    public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5;
+
+    public static final double kPXYController = 3.5;
+    public static final double kPThetaController = 5;
+
+    public static final double kAlignPXYController = 2;
+    public static final double kAlignPThetaController = 5;
+
+    // Constraint for the motion profiled robot angle controller
+    public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularAccelerationRadiansPerSecondSquared);
+
+    public static final TrapezoidProfile.Constraints kAlignThetaControllerConstraints = new TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularAccelerationRadiansPerSecondSquared);
+
+    // TODO This is a constant being managed like a static rewriteable variable
+    public static RobotConfig kRobotConfig;
+    public static boolean kAutoConfigOk;
+
+    public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
+        new PIDConstants(kPXYController, 0, 0),
+        new PIDConstants(kPThetaController, 0, 0)
+    );
+
+    public static final PathConstraints kOnTheFlyConstraints = new PathConstraints(
+        kMaxSpeedMetersPerSecond, 
+        kMaxAccelerationMetersPerSecondSquared, 
+        kMaxAngularSpeedRadiansPerSecond, 
+        kMaxAngularAccelerationRadiansPerSecondSquared
+    );
+
+    static {
+        try {
+            kRobotConfig = RobotConfig.fromGUISettings();
+            kAutoConfigOk = true;
+        } catch (IOException | ParseException e) {
+            System.err.println("FAILED TO READ ROBOTCONFIG, WAS THE CONFIG SET UP IN PATHPLANNER?");
+            e.printStackTrace();
+            kAutoConfigOk = false;
+        }
+    }
+}

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

@@ -0,0 +1,17 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.geometry.Transform3d;
+
+public class PhotonConstants {
+    public static final String kCamera1Name = "pv1";
+    public static final String kCamera2Name = "pv2";
+
+    // TODO Need actual values for all of this
+    public static final Transform3d kCamera1RobotToCam = new Transform3d();
+    public static final Transform3d kCamera2RobotToCam = new Transform3d();
+
+    public static final double kCamera1HeightMeters = 0;
+    public static final double kCamera1PitchRadians = 0;
+    public static final double kCamera2HeightMeters = 0;
+    public static final double kCamera2PitchRadians = 0;
+}

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

@@ -1,14 +1,19 @@
 package frc.robot.subsystems;
 
+import java.util.Optional;
+import java.util.OptionalDouble;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.junction.Logger;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathPlannerPath;
 import com.studica.frc.AHRS;
 import com.studica.frc.AHRS.NavXComType;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -16,10 +21,14 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
+import frc.robot.utilities.PhotonVision;
 import frc.robot.utilities.SwerveModule;
 
 public class Drivetrain extends SubsystemBase {
@@ -32,6 +41,9 @@ public class Drivetrain extends SubsystemBase {
 
     private SwerveDrivePoseEstimator estimator;
 
+    private PhotonVision camera1;
+    private PhotonVision camera2;
+
     public Drivetrain() {
         frontLeft = new SwerveModule(
             "FrontLeft", 
@@ -79,6 +91,25 @@ public class Drivetrain extends SubsystemBase {
             },
             new Pose2d()
         );
+
+        if(AutoConstants.kAutoConfigOk) {
+            AutoBuilder.configure(
+                this::getPose, 
+                this::resetOdometry, 
+                this::getCurrentChassisSpeeds, 
+                (speeds, feedforwards) -> driveWithChassisSpeeds(speeds), 
+                AutoConstants.kPPDriveController, 
+                AutoConstants.kRobotConfig,
+                () -> {
+                    Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+                        if (alliance.isPresent()) {
+                            return alliance.get() == DriverStation.Alliance.Red;
+                        }
+                        return false;
+                }, 
+                this
+            );
+        }
     }
 
     @Override
@@ -102,6 +133,42 @@ public class Drivetrain extends SubsystemBase {
         Logger.recordOutput("Drivetrain/Heading", getHeading());
     }
 
+    // TODO check both cameras
+    public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
+        if (camera1 == null) {
+            return new PrintCommand("Camera 1 not available");
+        }
+
+        // TODO The process variable is different here than what these constants are used for, may need to use something different
+        PIDController controller = new PIDController(
+            AutoConstants.kPThetaController, 
+            0, 
+            0
+        );
+
+        return runOnce(controller::reset).andThen(
+            drive(
+                xSpeed, 
+                ySpeed, 
+                () -> {
+                    OptionalDouble tagYaw = camera1.getTagYawByID(tagID);
+
+                    return (tagYaw.isEmpty() ? 0 : controller.calculate(tagYaw.getAsDouble(), 0));
+                },
+                () -> false
+           )
+        );
+    }
+
+    public Command drivePathPlannerPath(PathPlannerPath path) {
+        if(AutoConstants.kAutoConfigOk) {
+            return AutoBuilder.followPath(path);
+        } else {
+            return new PrintCommand("Robot Config loading failed, on the fly PathPlanner disabled");
+        }
+        
+    }
+
     public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative) {
         // TODO Inversions? Specific Alliance code?
         return run(() -> {
@@ -130,6 +197,19 @@ public class Drivetrain extends SubsystemBase {
         rearRight.resetEncoders();
     }
 
+    public void resetOdometry(Pose2d pose) {
+        estimator.resetPosition(
+            Rotation2d.fromDegrees(getGyroValue()),
+            new SwerveModulePosition[] {
+                frontLeft.getPosition(),
+                frontRight.getPosition(),
+                rearLeft.getPosition(),
+                rearRight.getPosition()
+            },
+            pose
+        );
+    }
+
     public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) {
         double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
         double xSpeedDelivered = 0;

src/main/java/frc/robot/utilities/SwerveModule.java

@@ -1,4 +1,4 @@
-package frc.robot.utiltiies;
+package frc.robot.utilities;
 
 import org.littletonrobotics.junction.Logger;
 
@@ -43,13 +43,42 @@ public class SwerveModule {
 
     private String moduleName;
 
+    private boolean isAbsoluteEncoderDisabled;
+
+    /**
+     * Builds the swerve module but with the Absolute Encoder disabled.
+     * 
+     * This constructor assumes you zeroed the swerve modules (faced all the bevel gears to the left)
+     * before booting up the robot.
+     * 
+     * @param moduleName The module name, Front Left, Front Right, etc.
+     * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
+     * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
+     */
+    public SwerveModule(String moduleName, int drivingCANID, int turningCANID) {
+        this(moduleName, drivingCANID, turningCANID, -1, -1);
+    }
+
+    /**
+     * Builds the swerve module with the normal features
+     * 
+     * @param moduleName The module name, Front Left, Front Right, etc.
+     * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
+     * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
+     * @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder
+     * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module 
+     */
     public SwerveModule(String moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
+        isAbsoluteEncoderDisabled = (analogEncoderID == -1) || (analogEncoderOffset < 0);
+
         drive = new TalonFX(drivingCANID);
         turning = new SparkMax(turningCANID, MotorType.kBrushless);
 
         turningRelativeEncoder = turning.getEncoder();
 
+        if(!isAbsoluteEncoderDisabled) {
             turningAbsoluteEncoder = new AnalogEncoder(analogEncoderID, 2 * Math.PI, analogEncoderOffset);
+        }
         
         turningClosedLoopController = turning.getClosedLoopController();
 
@@ -65,7 +94,12 @@ public class SwerveModule {
             PersistMode.kPersistParameters
         );
 
+        if(isAbsoluteEncoderDisabled){
+            turningRelativeEncoder.setPosition(0);
+        } else {
             turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
+        }
+        
         drive.setPosition(0);
 
         this.moduleName = "Drivetrain/Modules/" + moduleName;
@@ -118,6 +152,10 @@ public class SwerveModule {
     }
 
     public void zeroTurningEncoder() {
+        if(isAbsoluteEncoderDisabled) {
+            turningRelativeEncoder.setPosition(0);
+        } else {
             turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
         }
+    }
 }