Swerve pedro drivetrain

Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/AutoConstants.java

@@ -21,6 +21,8 @@
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.sixteen750.Setup.HardwareNames;
 import org.firstinspires.ftc.sixteen750.helpers.CustomAdafruitIMU;
+import org.firstinspires.ftc.sixteen750.swerveutil.CoaxialSwerveConstants;
+import org.firstinspires.ftc.sixteen750.swerveutil.CoaxialSwerveDrive;
 
 @Configurable
 public class AutoConstants {

Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/SwerveCalibrationOpMode.java

@@ -0,0 +1,110 @@
+package org.firstinspires.ftc.sixteen750.opmodes;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.AnalogInput;
+import org.firstinspires.ftc.sixteen750.swerveutil.AbsoluteAnalogEncoder;
+
+/**
+ * Calibration OpMode for Swerve Drive Absolute Encoders
+ *
+ * This OpMode helps you find the zero offsets for your swerve module encoders.
+ *
+ * INSTRUCTIONS:
+ * 1. Manually align all 4 swerve modules to point straight forward
+ * 2. Run this OpMode
+ * 3. Read the encoder values from telemetry
+ * 4. Copy these values into your CoaxialSwerveConstants as encoder offsets
+ *
+ * The offsets ensure that when your code thinks the modules are at 0°,
+ * they're actually pointing forward.
+ */
+@TeleOp(name = "Swerve Encoder Calibration", group = "Calibration")
+public class SwerveCalibrationOpMode extends LinearOpMode {
+
+    // Change these to match your robot configuration
+    private static final String FL_ENCODER = "frontLeftEncoder";
+    private static final String FR_ENCODER = "frontRightEncoder";
+    private static final String BL_ENCODER = "backLeftEncoder";
+    private static final String BR_ENCODER = "backRightEncoder";
+
+    @Override
+    public void runOpMode() {
+        // Initialize encoders
+        AbsoluteAnalogEncoder flEncoder = new AbsoluteAnalogEncoder(
+                hardwareMap.get(AnalogInput.class, FL_ENCODER)
+        );
+        AbsoluteAnalogEncoder frEncoder = new AbsoluteAnalogEncoder(
+                hardwareMap.get(AnalogInput.class, FR_ENCODER)
+        );
+        AbsoluteAnalogEncoder blEncoder = new AbsoluteAnalogEncoder(
+                hardwareMap.get(AnalogInput.class, BL_ENCODER)
+        );
+        AbsoluteAnalogEncoder brEncoder = new AbsoluteAnalogEncoder(
+                hardwareMap.get(AnalogInput.class, BR_ENCODER)
+        );
+
+        telemetry.addLine("========================================");
+        telemetry.addLine("SWERVE ENCODER CALIBRATION");
+        telemetry.addLine("========================================");
+        telemetry.addLine();
+        telemetry.addLine("INSTRUCTIONS:");
+        telemetry.addLine("1. Manually rotate all modules to point");
+        telemetry.addLine("   straight FORWARD on your robot");
+        telemetry.addLine("2. Press START when aligned");
+        telemetry.addLine("3. Copy the offset values to your");
+        telemetry.addLine("   CoaxialSwerveConstants class");
+        telemetry.update();
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+            // Read current positions
+            double flPos = flEncoder.getRawAngle();
+            double frPos = frEncoder.getRawAngle();
+            double blPos = blEncoder.getRawAngle();
+            double brPos = brEncoder.getRawAngle();
+
+            // Read raw voltages for debugging
+            double flVolt = flEncoder.getVoltage();
+            double frVolt = frEncoder.getVoltage();
+            double blVolt = blEncoder.getVoltage();
+            double brVolt = brEncoder.getVoltage();
+
+            // Display information
+            telemetry.addLine("========================================");
+            telemetry.addLine("ENCODER POSITIONS (radians)");
+            telemetry.addLine("========================================");
+            telemetry.addData("Front Left", "%.4f rad (%.1f°)", flPos, Math.toDegrees(flPos));
+            telemetry.addData("Front Right", "%.4f rad (%.1f°)", frPos, Math.toDegrees(frPos));
+            telemetry.addData("Back Left", "%.4f rad (%.1f°)", blPos, Math.toDegrees(blPos));
+            telemetry.addData("Back Right", "%.4f rad (%.1f°)", brPos, Math.toDegrees(brPos));
+
+            telemetry.addLine();
+            telemetry.addLine("========================================");
+            telemetry.addLine("COPY THESE VALUES TO YOUR CONSTANTS");
+            telemetry.addLine("========================================");
+            telemetry.addLine(String.format(
+                    ".withEncoderOffsets(%.4f, %.4f, %.4f, %.4f)",
+                    flPos, frPos, blPos, brPos
+            ));
+
+            telemetry.addLine();
+            telemetry.addLine("========================================");
+            telemetry.addLine("RAW VOLTAGES (for debugging)");
+            telemetry.addLine("========================================");
+            telemetry.addData("Front Left", "%.3fV", flVolt);
+            telemetry.addData("Front Right", "%.3fV", frVolt);
+            telemetry.addData("Back Left", "%.3fV", blVolt);
+            telemetry.addData("Back Right", "%.3fV", brVolt);
+
+            telemetry.addLine();
+            telemetry.addLine("========================================");
+            telemetry.addLine("Make sure all modules point FORWARD");
+            telemetry.addLine("before copying the offset values!");
+            telemetry.addLine("========================================");
+
+            telemetry.update();
+        }
+    }
+}

Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/AbsoluteAnalogEncoder.java

@@ -0,0 +1,117 @@
+package org.firstinspires.ftc.sixteen750.swerveutil;
+
+import com.bylazar.configurables.annotations.Configurable;
+import com.qualcomm.robotcore.hardware.AnalogInput;
+
+@Configurable
+public class AbsoluteAnalogEncoder {
+    public static double DEFAULT_RANGE = 3.3;
+
+    private final AnalogInput encoder;
+    private double offset;
+    private double analogRange;
+    private boolean inverted;
+
+    // Low-pass filter for smoothing
+    private double filteredVoltage;
+    private double alpha = 0.3; // Smoothing factor (0.0-1.0). Lower = smoother but more lag
+    private boolean filterInitialized = false;
+
+    public AbsoluteAnalogEncoder(AnalogInput enc){
+        this(enc, DEFAULT_RANGE);
+    }
+
+    public AbsoluteAnalogEncoder(AnalogInput enc, double aRange){
+        encoder = enc;
+        analogRange = aRange;
+        offset = 0;
+        inverted = false;
+    }
+
+    /**
+     * Set the zero offset for this encoder
+     * @param off offset in radians
+     * @return this encoder for chaining
+     */
+    public AbsoluteAnalogEncoder zero(double off){
+        offset = off;
+        return this;
+    }
+
+    /**
+     * Set whether this encoder reads inverted
+     * @param inv true if inverted
+     * @return this encoder for chaining
+     */
+    public AbsoluteAnalogEncoder setInverted(boolean inv){
+        inverted = inv;
+        return this;
+    }
+
+    /**
+     * Set the smoothing factor for the low-pass filter
+     * @param smoothing value between 0.0 (max smoothing, more lag) and 1.0 (no smoothing)
+     * @return this encoder for chaining
+     */
+    public AbsoluteAnalogEncoder setSmoothing(double smoothing){
+        alpha = Math.max(0.0, Math.min(1.0, smoothing)); // Clamp to [0, 1]
+        return this;
+    }
+
+    /**
+     * Get the current position of the encoder in radians [0, 2π]
+     * with offset applied and filtering
+     * @return position in radians
+     */
+    public double getCurrentPosition() {
+        double rawVoltage = getVoltage();
+
+        // Initialize filter on first read
+        if (!filterInitialized) {
+            filteredVoltage = rawVoltage;
+            filterInitialized = true;
+        }
+
+        // Apply low-pass filter to reduce jitter
+        filteredVoltage = (alpha * rawVoltage) + ((1 - alpha) * filteredVoltage);
+
+        // Convert voltage to ratio [0, 1]
+        double ratio = filteredVoltage / analogRange;
+        if (!inverted) {
+            ratio = 1 - ratio;
+        }
+
+        // Convert to radians [0, 2π] and apply offset
+        double pos = Angle.norm((ratio * Math.PI * 2) - offset);
+
+        return pos;
+    }
+
+    /**
+     * Get the raw angle without offset or filtering
+     * Useful for calibration
+     * @return raw angle in radians [0, 2π]
+     */
+    public double getRawAngle() {
+        double ratio = getVoltage() / analogRange;
+        if (!inverted) {
+            ratio = 1 - ratio;
+        }
+        return ratio * Math.PI * 2;
+    }
+
+    /**
+     * Get the raw voltage from the encoder
+     * @return voltage in volts
+     */
+    public double getVoltage(){
+        return encoder.getVoltage();
+    }
+
+    /**
+     * Reset the filter (useful when encoder might have jumped)
+     */
+    public void resetFilter() {
+        filterInitialized = false;
+    }
+}

Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/Angle.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.sixteen750.swerveutil;
+
+public final class Angle {
+    private static final double PI = Math.PI;
+    private static final double TAU = PI * 2;
+
+    private Angle() {
+        // Prevent instantiation
+    }
+
+    /**
+     * Returns angle clamped to [0, 2pi].
+     *
+     * @param angle angle measure in radians
+     */
+    public static double norm(double angle) {
+        double modifiedAngle = angle % TAU;
+        modifiedAngle = (modifiedAngle + TAU) % TAU;
+        return modifiedAngle;
+    }
+
+    /**
+     * Returns angleDelta clamped to [-pi, pi].
+     *
+     * @param angleDelta angle delta in radians
+     */
+    public static double normDelta(double angleDelta) {
+        double modifiedAngleDelta = norm(angleDelta);
+
+        if (modifiedAngleDelta > PI) {
+            modifiedAngleDelta -= TAU;
+        }
+
+        return modifiedAngleDelta;
+    }
+}