From 9f8c6b5e64041480003ff6797e9798a839b7ccba Mon Sep 17 00:00:00 2001 From: koolpoolo <181432562+koolpoolo@users.noreply.github.com> Date: Mon, 15 Dec 2025 21:28:53 -0800 Subject: [PATCH 1/8] started on implementation of swerve --- .../ftc/sixteen750/AutoConstants.java | 2 + .../opmodes/SwerveCalibrationOpMode.java | 110 ++++++ .../swerveutil/AbsoluteAnalogEncoder.java | 47 +++ .../ftc/sixteen750/swerveutil/Angle.java | 36 ++ .../swerveutil/CoaxialSwerveConstants.java | 209 ++++++++++ .../swerveutil/CoaxialSwerveDrive.java | 366 ++++++++++++++++++ 6 files changed, 770 insertions(+) create mode 100644 Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/SwerveCalibrationOpMode.java create mode 100644 Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/AbsoluteAnalogEncoder.java create mode 100644 Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/Angle.java create mode 100644 Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java create mode 100644 Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/AutoConstants.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/AutoConstants.java index 04f2f7d3..f81059b3 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/AutoConstants.java +++ b/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 { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/SwerveCalibrationOpMode.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/SwerveCalibrationOpMode.java new file mode 100644 index 00000000..5aeec3a8 --- /dev/null +++ b/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.getCurrentPosition(); + double frPos = frEncoder.getCurrentPosition(); + double blPos = blEncoder.getCurrentPosition(); + double brPos = brEncoder.getCurrentPosition(); + + // 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(); + } + } +} diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/AbsoluteAnalogEncoder.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/AbsoluteAnalogEncoder.java new file mode 100644 index 00000000..3623d910 --- /dev/null +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/AbsoluteAnalogEncoder.java @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.sixteen750.swerveutil; + + +import com.bylazar.configurables.annotations.Configurable; +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.technototes.library.hardware.sensor.encoder.Encoder; + +@Configurable +public class AbsoluteAnalogEncoder { + public static double DEFAULT_RANGE = 3.3; + public static boolean VALUE_REJECTION = false; + private final AnalogInput encoder; + private double offset, analogRange;; + private boolean inverted; + + public AbsoluteAnalogEncoder(AnalogInput enc){ + this(enc, DEFAULT_RANGE); + } + public AbsoluteAnalogEncoder(AnalogInput enc, double aRange){ + encoder = enc; + analogRange = aRange; + offset = 0;; + } + public AbsoluteAnalogEncoder zero(double off){ + offset = off; + return this; + } + + public AbsoluteAnalogEncoder setInverted(boolean inv){ + inverted = inv; + return this; + } + + + + private double pastPosition = 1; + public double getCurrentPosition() { + double pos = Angle.norm((!inverted ? 1 - getVoltage() / analogRange : getVoltage() / analogRange) * Math.PI*2 - offset); + //checks for crazy values when the encoder is close to zero + if(!VALUE_REJECTION || Math.abs(Angle.normDelta(pastPosition)) > 0.1 || Math.abs(Angle.normDelta(pos)) < 1) pastPosition = pos; + return pastPosition; + } + public double getVoltage(){ + return encoder.getVoltage(); + } + +} \ No newline at end of file diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/Angle.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/Angle.java new file mode 100644 index 00000000..62679cec --- /dev/null +++ b/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; + } +} diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java new file mode 100644 index 00000000..e95c4a7e --- /dev/null +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java @@ -0,0 +1,209 @@ +package org.firstinspires.ftc.sixteen750.swerveutil; +import com.bylazar.configurables.annotations.Configurable; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.CRServo; + +/** + * Constants class for the Coaxial Swerve Drivetrain + * + * This class contains all the configuration parameters needed for the swerve drivetrain. + * Modify these values to match your robot's physical configuration. + */ +@Configurable +public class CoaxialSwerveConstants { + + // ==================== HARDWARE NAMES ==================== + // Change these to match your robot's configuration file + + public String frontLeftDriveMotorName = "frontLeftDrive"; + public String frontRightDriveMotorName = "frontRightDrive"; + public String rearLeftDriveMotorName = "rearLeftDrive"; + public String rearRightDriveMotorName = "rearRightDrive"; + + public String frontLeftSteeringServoName = "frontLeftSteer"; + public String frontRightSteeringServoName = "frontRightSteer"; + public String rearLeftSteeringServoName = "rearLeftSteer"; + public String rearRightSteeringServoName = "rearRightSteer"; + + // Absolute encoder names for steering feedback + public String frontLeftEncoderName = "frontLeftEncoder"; + public String frontRightEncoderName = "frontRightEncoder"; + public String rearLeftEncoderName = "rearLeftEncoder"; + public String rearRightEncoderName = "rearRightEncoder"; + + // Encoder zero offsets (in radians) + // These should be calibrated by pointing each module forward and recording the encoder value + public double frontLeftEncoderOffset = 0.0; + public double frontRightEncoderOffset = 0.0; + public double rearLeftEncoderOffset = 0.0; + public double rearRightEncoderOffset = 0.0; + + // ==================== MOTOR DIRECTIONS ==================== + // Set these based on how your motors are mounted + + public DcMotorSimple.Direction frontLeftDriveMotorDirection = DcMotorSimple.Direction.FORWARD; + public DcMotorSimple.Direction frontRightDriveMotorDirection = DcMotorSimple.Direction.REVERSE; + public DcMotorSimple.Direction backLeftDriveMotorDirection = DcMotorSimple.Direction.FORWARD; + public DcMotorSimple.Direction backRightDriveMotorDirection = DcMotorSimple.Direction.REVERSE; + + // ==================== SERVO DIRECTIONS ==================== + // Set these based on how your servos are mounted + + public CRServo.Direction frontLeftSteeringServoDirection = CRServo.Direction.FORWARD; + public CRServo.Direction frontRightSteeringServoDirection = CRServo.Direction.FORWARD; + public CRServo.Direction backLeftSteeringServoDirection = CRServo.Direction.FORWARD; + public CRServo.Direction backRightSteeringServoDirection = CRServo.Direction.FORWARD; + + // ==================== PHYSICAL DIMENSIONS ==================== + // All measurements should be in the same unit (inches or meters) + + /** + * Track width: distance between left and right wheels (side to side) + * Measure from the center of one wheel to the center of the opposite wheel + */ + public double trackWidth = 12.0; // inches + + /** + * Wheel base: distance between front and back wheels (front to back) + * Measure from the center of one wheel to the center of the opposite wheel + */ + public double wheelBase = 12.0; // inches + + /** + * Wheel diameter in inches + * Used for calculating distances and velocities + */ + public double wheelDiameter = 4.0; // inches + + // ==================== STEERING CONTROL ==================== + + /** + * Proportional gain for steering control + * Higher values make steering more aggressive but can cause oscillation + * Start with a low value (0.5-1.0) and increase if steering is too slow + */ + public double steeringKp = 1.5; + + /** + * Maximum steering speed (radians per second) + * This limits how fast the modules can rotate + */ + public double steeringSpeed = Math.PI; // 180 degrees per second + + // ==================== MOTOR SPECIFICATIONS ==================== + + /** + * Maximum RPM of your drive motors + * This is used for velocity calculations + * Common FTC motors: + * - HD Hex Motor: 6000 RPM + * - Core Hex Motor: 125 RPM + * - goBILDA 5202/5203: 435 RPM + * - goBILDA 5204: 312 RPM + */ + public double motorMaxRPM = 435.0; + + /** + * Gear ratio from motor to wheel + * If your motor drives the wheel directly, this is 1.0 + * If you have a gear reduction, calculate: motor_teeth / wheel_teeth + * Example: 12 tooth motor gear to 60 tooth wheel gear = 12/60 = 0.2 + */ + public double gearRatio = 1.0; + + // ==================== ENCODER CONFIGURATION ==================== + + /** + * Ticks per revolution for your drive motors + * Common FTC motor encoders: + * - HD Hex Motor: 28 ticks/rev + * - Core Hex Motor: 288 ticks/rev + * - goBILDA motors: 28 ticks/rev (at the motor) + */ + public double motorTicksPerRev = 28.0; + + /** + * Constructor with default values + * You can create instances with different configurations + */ + public CoaxialSwerveConstants() { + // Uses default values defined above + } + + /** + * Constructor with custom track width and wheel base + * Useful for quickly creating configurations for different robot sizes + * + * @param trackWidth distance between left and right wheels + * @param wheelBase distance between front and back wheels + */ + public CoaxialSwerveConstants(double trackWidth, double wheelBase) { + this.trackWidth = trackWidth; + this.wheelBase = wheelBase; + } + + /** + * Builder-style method to set hardware names + * This allows for cleaner configuration code + */ + public CoaxialSwerveConstants withMotorNames(String fl, String fr, String rl, String rr) { + this.frontLeftDriveMotorName = fl; + this.frontRightDriveMotorName = fr; + this.rearLeftDriveMotorName = rl; + this.rearRightDriveMotorName = rr; + return this; + } + + /** + * Builder-style method to set servo names + */ + public CoaxialSwerveConstants withServoNames(String fl, String fr, String rl, String rr) { + this.frontLeftSteeringServoName = fl; + this.frontRightSteeringServoName = fr; + this.rearLeftSteeringServoName = rl; + this.rearRightSteeringServoName = rr; + return this; + } + + /** + * Builder-style method to set encoder names + */ + public CoaxialSwerveConstants withEncoderNames(String fl, String fr, String bl, String br) { + this.frontLeftEncoderName = fl; + this.frontRightEncoderName = fr; + this.rearLeftEncoderName = bl; + this.rearRightEncoderName = br; + return this; + } + + /** + * Builder-style method to set encoder offsets + * Use this after calibrating your modules + */ + public CoaxialSwerveConstants withEncoderOffsets(double fl, double fr, double bl, double br) { + this.frontLeftEncoderOffset = fl; + this.frontRightEncoderOffset = fr; + this.rearRightEncoderOffset = bl; + this.rearRightEncoderOffset = br; + return this; + } + + /** + * Builder-style method to set physical dimensions + */ + public CoaxialSwerveConstants withDimensions(double trackWidth, double wheelBase, double wheelDiameter) { + this.trackWidth = trackWidth; + this.wheelBase = wheelBase; + this.wheelDiameter = wheelDiameter; + return this; + } + + /** + * Builder-style method to set steering parameters + */ + public CoaxialSwerveConstants withSteeringParams(double kp, double maxSpeed) { + this.steeringKp = kp; + this.steeringSpeed = maxSpeed; + return this; + } +} \ No newline at end of file diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java new file mode 100644 index 00000000..52ef013e --- /dev/null +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java @@ -0,0 +1,366 @@ +package org.firstinspires.ftc.sixteen750.swerveutil; +import com.pedropathing.Drivetrain; +import com.pedropathing.math.Vector; +import com.pedropathing.math.MathFunctions; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import org.firstinspires.ftc.sixteen750.swerveutil.AbsoluteAnalogEncoder; +import org.firstinspires.ftc.sixteen750.swerveutil.Angle; +/** + * Coaxial Swerve Drivetrain Implementation + * + * This drivetrain uses 4 modules, each with: + * - A drive motor (moves the wheel forward/backward) + * - A steering servo (rotates the module to point in any direction) + * + * The "coaxial" design means the drive and steering mechanisms share the same axis. + */ +public class CoaxialSwerveDrive extends Drivetrain { + + // Hardware components + private DcMotorEx[] driveMotors; // 4 motors for driving each wheel + private CRServo[] steeringServos; // 4 servos for steering each module + private AbsoluteAnalogEncoder[] steeringEncoders; // 4 absolute encoders for module angles + + // Module positions relative to robot center (for calculating rotation) + private Vector[] modulePositions; + + // Current target angles for each module (in radians) + private double[] targetAngles; + + // Current angles of each module (in radians) + private double[] currentAngles; + + // Constants + private CoaxialSwerveConstants constants; + + // Voltage sensor for battery voltage monitoring + private VoltageSensor voltageSensor; + + // Voltage tracking + private double currentVoltage = 12.0; + + /** + * Constructor for the Coaxial Swerve Drivetrain + * + * @param hardwareMap the FTC hardware map to get motors and servos + * @param constants the constants object containing configuration parameters + */ + public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants constants) { + this.constants = constants; + this.maxPowerScaling = 1.0; + this.voltageCompensation = false; + this.nominalVoltage = 12.0; + + // Get voltage sensor from the Control Hub + // This is typically named "Control Hub" or "Expansion Hub 1" + try { + voltageSensor = hardwareMap.voltageSensor.iterator().next(); + } catch (Exception e) { + voltageSensor = null; + } + + // Initialize hardware arrays + driveMotors = new DcMotorEx[4]; + steeringServos = new CRServo[4]; + steeringEncoders = new AbsoluteAnalogEncoder[4]; + targetAngles = new double[4]; + currentAngles = new double[4]; + + // Get hardware from the hardware map + driveMotors[0] = hardwareMap.get(DcMotorEx.class, constants.frontLeftDriveMotorName); + driveMotors[1] = hardwareMap.get(DcMotorEx.class, constants.frontRightDriveMotorName); + driveMotors[2] = hardwareMap.get(DcMotorEx.class, constants.rearLeftEncoderName); + driveMotors[3] = hardwareMap.get(DcMotorEx.class, constants.rearRightEncoderName); + + steeringServos[0] = hardwareMap.get(CRServo.class, constants.frontLeftSteeringServoName); + steeringServos[1] = hardwareMap.get(CRServo.class, constants.frontRightSteeringServoName); + steeringServos[2] = hardwareMap.get(CRServo.class, constants.rearLeftSteeringServoName); + steeringServos[3] = hardwareMap.get(CRServo.class, constants.rearLeftDriveMotorName); + + // Initialize absolute encoders for steering feedback + steeringEncoders[0] = new AbsoluteAnalogEncoder( + hardwareMap.get(AnalogInput.class, constants.frontLeftEncoderName)) + .zero(constants.frontLeftEncoderOffset); + steeringEncoders[1] = new AbsoluteAnalogEncoder( + hardwareMap.get(AnalogInput.class, constants.frontRightEncoderName)) + .zero(constants.frontRightEncoderOffset); + steeringEncoders[2] = new AbsoluteAnalogEncoder( + hardwareMap.get(AnalogInput.class, constants.rearLeftSteeringServoName)) + .zero(constants.rearLeftEncoderOffset); + steeringEncoders[3] = new AbsoluteAnalogEncoder( + hardwareMap.get(AnalogInput.class, constants.rearLeftDriveMotorName)) + .zero(constants.rearRightEncoderOffset); + + // Set motor directions + driveMotors[0].setDirection(constants.frontLeftDriveMotorDirection); + driveMotors[1].setDirection(constants.frontRightDriveMotorDirection); + driveMotors[2].setDirection(constants.backLeftDriveMotorDirection); + driveMotors[3].setDirection(constants.backRightDriveMotorDirection); + + // Set servo directions + steeringServos[0].setDirection(constants.frontLeftSteeringServoDirection); + steeringServos[1].setDirection(constants.frontRightSteeringServoDirection); + steeringServos[2].setDirection(constants.backLeftSteeringServoDirection); + steeringServos[3].setDirection(constants.backRightSteeringServoDirection); + + // Initialize module positions (relative to robot center) + // These represent the physical location of each module on the robot + modulePositions = new Vector[4]; + modulePositions[0] = new Vector(constants.trackWidth / 2, constants.wheelBase / 2); // Front Left + modulePositions[1] = new Vector(constants.trackWidth / 2, -constants.wheelBase / 2); // Front Right + modulePositions[2] = new Vector(-constants.trackWidth / 2, constants.wheelBase / 2); // Back Left + modulePositions[3] = new Vector(-constants.trackWidth / 2, -constants.wheelBase / 2); // Back Right + + // Set motor modes + for (DcMotorEx motor : driveMotors) { + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + } + + /** + * Calculates the drive powers for each swerve module. + * + * This is the core method that determines: + * 1. What direction each module should point + * 2. How fast each module should drive + * + * @param correctivePower translational correction vector (for path following) + * @param headingPower rotational power for turning + * @param pathingPower forward movement vector along the path + * @param robotHeading current robot heading in radians + * @return array of 8 values: [drive0, steer0, drive1, steer1, drive2, steer2, drive3, steer3] + */ + @Override + public double[] calculateDrive(Vector correctivePower, Vector headingPower, Vector pathingPower, double robotHeading) { + // Combine all the input vectors into a single desired movement vector + Vector desiredTranslation = new Vector( + correctivePower.getXComponent() + pathingPower.getXComponent(), + correctivePower.getYComponent() + pathingPower.getYComponent() + ); + + // Get the desired rotation (heading power magnitude determines rotation speed) + double desiredRotation = headingPower.getMagnitude() * Math.signum(headingPower.getXComponent()); + + // Convert field-relative movement to robot-relative + // This rotates the desired movement vector by the robot's current heading + double cos = Math.cos(-robotHeading); + double sin = Math.sin(-robotHeading); + double rotatedX = desiredTranslation.getXComponent() * cos - desiredTranslation.getYComponent() * sin; + double rotatedY = desiredTranslation.getXComponent() * sin + desiredTranslation.getYComponent() * cos; + + // Array to store module speeds and angles + double[] moduleSpeeds = new double[4]; + double[] moduleAngles = new double[4]; + + // Calculate the desired state for each swerve module + for (int i = 0; i < 4; i++) { + // Calculate the rotational contribution for this module + // Rotation creates a tangent vector perpendicular to the module's position + double rotationX = -modulePositions[i].getYComponent() * desiredRotation; + double rotationY = modulePositions[i].getXComponent() * desiredRotation; + + // Combine translation and rotation + double moduleVectorX = rotatedX + rotationX; + double moduleVectorY = rotatedY + rotationY; + + // Calculate the speed (magnitude) and angle (direction) for this module + moduleSpeeds[i] = Math.sqrt(moduleVectorX * moduleVectorX + moduleVectorY * moduleVectorY); + moduleAngles[i] = Math.atan2(moduleVectorY, moduleVectorX); + + // Optimize the module angle to avoid rotating more than 90 degrees + // If the module needs to rotate >90°, we can reverse the drive direction instead + double angleDifference = moduleAngles[i] - currentAngles[i]; + + // Normalize angle difference to [-PI, PI] + while (angleDifference > Math.PI) angleDifference -= 2 * Math.PI; + while (angleDifference < -Math.PI) angleDifference += 2 * Math.PI; + + // If we need to turn more than 90 degrees, flip the angle and reverse speed + if (Math.abs(angleDifference) > Math.PI / 2) { + moduleAngles[i] += Math.PI; + moduleSpeeds[i] *= -1; + + // Normalize the angle + while (moduleAngles[i] > Math.PI) moduleAngles[i] -= 2 * Math.PI; + while (moduleAngles[i] < -Math.PI) moduleAngles[i] += 2 * Math.PI; + } + + // Store the target angle for this module + targetAngles[i] = moduleAngles[i]; + } + + // Normalize wheel speeds so none exceed 1.0 + double maxSpeed = 0; + for (double speed : moduleSpeeds) { + if (Math.abs(speed) > maxSpeed) { + maxSpeed = Math.abs(speed); + } + } + + if (maxSpeed > 1.0) { + for (int i = 0; i < 4; i++) { + moduleSpeeds[i] /= maxSpeed; + } + } + + // Apply max power scaling + for (int i = 0; i < 4; i++) { + moduleSpeeds[i] *= maxPowerScaling; + } + + // Calculate steering servo powers based on angle error + double[] steeringPowers = new double[4]; + for (int i = 0; i < 4; i++) { + // Get actual current angle from absolute encoder + currentAngles[i] = steeringEncoders[i].getCurrentPosition(); + + // Calculate angle error using the Angle utility for proper wrapping + double angleError = Angle.normDelta(targetAngles[i] - currentAngles[i]); + + // PID control for steering (currently just P) + steeringPowers[i] = MathFunctions.clamp(angleError * constants.steeringKp, -1, 1); + } + + // Apply voltage compensation if enabled + if (voltageCompensation && voltageSensor != null) { + currentVoltage = voltageSensor.getVoltage(); + double voltageScale = nominalVoltage / currentVoltage; + for (int i = 0; i < 4; i++) { + moduleSpeeds[i] *= voltageScale; + steeringPowers[i] *= voltageScale; + } + } + + // Return interleaved array: [drive0, steer0, drive1, steer1, ...] + return new double[] { + moduleSpeeds[0], steeringPowers[0], + moduleSpeeds[1], steeringPowers[1], + moduleSpeeds[2], steeringPowers[2], + moduleSpeeds[3], steeringPowers[3] + }; + } + + /** + * Runs the drivetrain hardware with the calculated powers. + * + * @param drivePowers array containing [drive0, steer0, drive1, steer1, drive2, steer2, drive3, steer3] + */ + @Override + public void runDrive(double[] drivePowers) { + if (drivePowers.length != 8) { + throw new IllegalArgumentException("Drive powers array must have length 8 for coaxial swerve"); + } + + // Set drive motor powers + driveMotors[0].setPower(drivePowers[0]); + driveMotors[1].setPower(drivePowers[2]); + driveMotors[2].setPower(drivePowers[4]); + driveMotors[3].setPower(drivePowers[6]); + + // Set steering servo powers + steeringServos[0].setPower(drivePowers[1]); + steeringServos[1].setPower(drivePowers[3]); + steeringServos[2].setPower(drivePowers[5]); + steeringServos[3].setPower(drivePowers[7]); + } + + @Override + public void updateConstants() { + // Update module positions if constants changed + modulePositions[0] = new Vector(constants.trackWidth / 2, constants.wheelBase / 2); + modulePositions[1] = new Vector(constants.trackWidth / 2, -constants.wheelBase / 2); + modulePositions[2] = new Vector(-constants.trackWidth / 2, constants.wheelBase / 2); + modulePositions[3] = new Vector(-constants.trackWidth / 2, -constants.wheelBase / 2); + } + + @Override + public void breakFollowing() { + // Stop all motors and servos + for (DcMotorEx motor : driveMotors) { + motor.setPower(0); + } + for (CRServo servo : steeringServos) { + servo.setPower(0); + } + } + + @Override + public void startTeleopDrive() { + startTeleopDrive(true); + } + + @Override + public void startTeleopDrive(boolean brakeMode) { + DcMotor.ZeroPowerBehavior behavior = brakeMode ? + DcMotor.ZeroPowerBehavior.BRAKE : DcMotor.ZeroPowerBehavior.FLOAT; + + for (DcMotorEx motor : driveMotors) { + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + motor.setZeroPowerBehavior(behavior); + } + } + + @Override + public double xVelocity() { + // Calculate average velocity in X direction from all modules + // This would typically use encoder velocities + double sum = 0; + for (int i = 0; i < 4; i++) { + sum += driveMotors[i].getVelocity() * Math.cos(currentAngles[i]); + } + return sum / 4.0; + } + + @Override + public double yVelocity() { + // Calculate average velocity in Y direction from all modules + double sum = 0; + for (int i = 0; i < 4; i++) { + sum += driveMotors[i].getVelocity() * Math.sin(currentAngles[i]); + } + return sum / 4.0; + } + + @Override + public void setXVelocity(double xMovement) { + // Not typically used in swerve drive (use calculateDrive instead) + // This is a simplified implementation + } + + @Override + public void setYVelocity(double yMovement) { + // Not typically used in swerve drive (use calculateDrive instead) + // This is a simplified implementation + } + + @Override + public double getVoltage() { + // Read current battery voltage from the voltage sensor + if (voltageSensor != null) { + currentVoltage = voltageSensor.getVoltage(); + return currentVoltage; + } + // Return nominal voltage if sensor unavailable + return nominalVoltage; + } + + @Override + public String debugString() { + StringBuilder sb = new StringBuilder(); + sb.append("Coaxial Swerve Drivetrain Debug:\n"); + for (int i = 0; i < 4; i++) { + sb.append(String.format("Module %d: Speed=%.2f, Angle=%.2f°, Target=%.2f°\n", + i, driveMotors[i].getPower(), + Math.toDegrees(currentAngles[i]), + Math.toDegrees(targetAngles[i]))); + } + sb.append(String.format("Voltage: %.2fV\n", currentVoltage)); + return sb.toString(); + } +} \ No newline at end of file From 87cbbfddb5007f0746926702869ba9d335f7e12c Mon Sep 17 00:00:00 2001 From: koolpoolo <181432562+koolpoolo@users.noreply.github.com> Date: Thu, 18 Dec 2025 22:52:45 -0800 Subject: [PATCH 2/8] refined ts --- .../swerveutil/CoaxialSwerveConstants.java | 91 ++++++------ .../swerveutil/CoaxialSwerveDrive.java | 137 ++++++++++++------ .../swerveutil/SlewRateLimiter.java | 107 ++++++++++++++ 3 files changed, 253 insertions(+), 82 deletions(-) create mode 100644 Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/SlewRateLimiter.java diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java index e95c4a7e..d6eabebe 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java @@ -1,5 +1,5 @@ package org.firstinspires.ftc.sixteen750.swerveutil; -import com.bylazar.configurables.annotations.Configurable; + import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.CRServo; @@ -9,9 +9,10 @@ * This class contains all the configuration parameters needed for the swerve drivetrain. * Modify these values to match your robot's physical configuration. */ -@Configurable public class CoaxialSwerveConstants { + + // ==================== HARDWARE NAMES ==================== // Change these to match your robot's configuration file @@ -38,21 +39,27 @@ public class CoaxialSwerveConstants { public double rearLeftEncoderOffset = 0.0; public double rearRightEncoderOffset = 0.0; + // encoder inverted status + public boolean isFrontLeftEncoderInverted = false; + public boolean isFrontRightEncoderInverted = false; + public boolean isRearLeftEncoderInverted = false; + public boolean isRearRightEncoderInverted = false; + // ==================== MOTOR DIRECTIONS ==================== // Set these based on how your motors are mounted public DcMotorSimple.Direction frontLeftDriveMotorDirection = DcMotorSimple.Direction.FORWARD; public DcMotorSimple.Direction frontRightDriveMotorDirection = DcMotorSimple.Direction.REVERSE; - public DcMotorSimple.Direction backLeftDriveMotorDirection = DcMotorSimple.Direction.FORWARD; - public DcMotorSimple.Direction backRightDriveMotorDirection = DcMotorSimple.Direction.REVERSE; + public DcMotorSimple.Direction rearLeftDriveMotorDirection = DcMotorSimple.Direction.FORWARD; + public DcMotorSimple.Direction rearRightDriveMotorDirection = DcMotorSimple.Direction.REVERSE; // ==================== SERVO DIRECTIONS ==================== // Set these based on how your servos are mounted public CRServo.Direction frontLeftSteeringServoDirection = CRServo.Direction.FORWARD; public CRServo.Direction frontRightSteeringServoDirection = CRServo.Direction.FORWARD; - public CRServo.Direction backLeftSteeringServoDirection = CRServo.Direction.FORWARD; - public CRServo.Direction backRightSteeringServoDirection = CRServo.Direction.FORWARD; + public CRServo.Direction rearLeftSteeringServoDirection = CRServo.Direction.FORWARD; + public CRServo.Direction rearRightSteeringServoDirection = CRServo.Direction.FORWARD; // ==================== PHYSICAL DIMENSIONS ==================== // All measurements should be in the same unit (inches or meters) @@ -69,12 +76,6 @@ public class CoaxialSwerveConstants { */ public double wheelBase = 12.0; // inches - /** - * Wheel diameter in inches - * Used for calculating distances and velocities - */ - public double wheelDiameter = 4.0; // inches - // ==================== STEERING CONTROL ==================== /** @@ -83,6 +84,7 @@ public class CoaxialSwerveConstants { * Start with a low value (0.5-1.0) and increase if steering is too slow */ public double steeringKp = 1.5; + public double steeringKd = 0; /** * Maximum steering speed (radians per second) @@ -90,38 +92,36 @@ public class CoaxialSwerveConstants { */ public double steeringSpeed = Math.PI; // 180 degrees per second - // ==================== MOTOR SPECIFICATIONS ==================== + // ==================== SLEW RATE LIMITING ==================== /** - * Maximum RPM of your drive motors - * This is used for velocity calculations - * Common FTC motors: - * - HD Hex Motor: 6000 RPM - * - Core Hex Motor: 125 RPM - * - goBILDA 5202/5203: 435 RPM - * - goBILDA 5204: 312 RPM + * Enable slew rate limiting for drive motors + * Prevents sudden acceleration/deceleration */ - public double motorMaxRPM = 435.0; + public boolean useDriveSlewRateLimiting = true; /** - * Gear ratio from motor to wheel - * If your motor drives the wheel directly, this is 1.0 - * If you have a gear reduction, calculate: motor_teeth / wheel_teeth - * Example: 12 tooth motor gear to 60 tooth wheel gear = 12/60 = 0.2 + * Maximum drive power change per second + * Example: 2.0 means power can change from 0 to 1.0 in 0.5 seconds + * Higher values = more responsive but jerkier + * Lower values = smoother but less responsive */ - public double gearRatio = 1.0; + public double driveMaxAcceleration = 4.0; // 0 to 1.0 in 0.25 seconds - // ==================== ENCODER CONFIGURATION ==================== + /** + * Enable slew rate limiting for steering servos + * Prevents sudden steering movements + */ + public boolean useSteeringSlewRateLimiting = true; /** - * Ticks per revolution for your drive motors - * Common FTC motor encoders: - * - HD Hex Motor: 28 ticks/rev - * - Core Hex Motor: 288 ticks/rev - * - goBILDA motors: 28 ticks/rev (at the motor) + * Maximum steering power change per second + * This smooths out steering servo movements */ - public double motorTicksPerRev = 28.0; + public double steeringMaxAcceleration = 8.0; // 0 to 1.0 in 0.125 seconds + public double steeringDeadband = Math.toRadians(2); + public double steeringMinPower = 0.05; /** * Constructor with default values * You can create instances with different configurations @@ -168,11 +168,11 @@ public CoaxialSwerveConstants withServoNames(String fl, String fr, String rl, St /** * Builder-style method to set encoder names */ - public CoaxialSwerveConstants withEncoderNames(String fl, String fr, String bl, String br) { + public CoaxialSwerveConstants withEncoderNames(String fl, String fr, String rl, String rr) { this.frontLeftEncoderName = fl; this.frontRightEncoderName = fr; - this.rearLeftEncoderName = bl; - this.rearRightEncoderName = br; + this.rearLeftEncoderName = rl; + this.rearRightEncoderName = rr; return this; } @@ -180,11 +180,18 @@ public CoaxialSwerveConstants withEncoderNames(String fl, String fr, String bl, * Builder-style method to set encoder offsets * Use this after calibrating your modules */ - public CoaxialSwerveConstants withEncoderOffsets(double fl, double fr, double bl, double br) { + public CoaxialSwerveConstants withEncoderOffsets(double fl, double fr, double rl, double rr) { this.frontLeftEncoderOffset = fl; this.frontRightEncoderOffset = fr; - this.rearRightEncoderOffset = bl; - this.rearRightEncoderOffset = br; + this.rearLeftEncoderOffset = rl; + this.rearRightEncoderOffset = rr; + return this; + } + public CoaxialSwerveConstants withEncoderInvertation(boolean fl, boolean fr, boolean rl, boolean rr) { + this.isFrontLeftEncoderInverted = fl; + this.isFrontRightEncoderInverted = fr; + this.isRearLeftEncoderInverted = rl; + this.isRearRightEncoderInverted = rr; return this; } @@ -194,16 +201,18 @@ public CoaxialSwerveConstants withEncoderOffsets(double fl, double fr, double bl public CoaxialSwerveConstants withDimensions(double trackWidth, double wheelBase, double wheelDiameter) { this.trackWidth = trackWidth; this.wheelBase = wheelBase; - this.wheelDiameter = wheelDiameter; return this; } /** * Builder-style method to set steering parameters */ - public CoaxialSwerveConstants withSteeringParams(double kp, double maxSpeed) { + public CoaxialSwerveConstants withSteeringParams(double kp, double kd, double maxSpeed, double db, double minPower) { this.steeringKp = kp; + this.steeringKd = kd; this.steeringSpeed = maxSpeed; + this.steeringDeadband = db; + this.steeringMinPower = minPower; return this; } } \ No newline at end of file diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java index 52ef013e..bd608e22 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java @@ -1,4 +1,6 @@ package org.firstinspires.ftc.sixteen750.swerveutil; +import android.annotation.SuppressLint; + import com.pedropathing.Drivetrain; import com.pedropathing.math.Vector; import com.pedropathing.math.MathFunctions; @@ -10,6 +12,7 @@ import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.sixteen750.swerveutil.AbsoluteAnalogEncoder; import org.firstinspires.ftc.sixteen750.swerveutil.Angle; + /** * Coaxial Swerve Drivetrain Implementation * @@ -41,8 +44,12 @@ public class CoaxialSwerveDrive extends Drivetrain { // Voltage sensor for battery voltage monitoring private VoltageSensor voltageSensor; + // Slew rate limiters for smooth acceleration + private SlewRateLimiter[] driveRateLimiters = new SlewRateLimiter[4]; + // Voltage tracking private double currentVoltage = 12.0; + double[] lastAngleError = new double[4]; /** * Constructor for the Coaxial Swerve Drivetrain @@ -57,7 +64,6 @@ public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants consta this.nominalVoltage = 12.0; // Get voltage sensor from the Control Hub - // This is typically named "Control Hub" or "Expansion Hub 1" try { voltageSensor = hardwareMap.voltageSensor.iterator().next(); } catch (Exception e) { @@ -70,43 +76,57 @@ public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants consta steeringEncoders = new AbsoluteAnalogEncoder[4]; targetAngles = new double[4]; currentAngles = new double[4]; + for (int i = 0; i < 4; i++) { + lastAngleError[i] = 0; + } + + // Initialize slew rate limiters + for (int i = 0; i < 4; i++) { + driveRateLimiters[i] = new SlewRateLimiter(constants.driveMaxAcceleration); + } // Get hardware from the hardware map driveMotors[0] = hardwareMap.get(DcMotorEx.class, constants.frontLeftDriveMotorName); driveMotors[1] = hardwareMap.get(DcMotorEx.class, constants.frontRightDriveMotorName); - driveMotors[2] = hardwareMap.get(DcMotorEx.class, constants.rearLeftEncoderName); - driveMotors[3] = hardwareMap.get(DcMotorEx.class, constants.rearRightEncoderName); + driveMotors[2] = hardwareMap.get(DcMotorEx.class, constants.rearLeftDriveMotorName); + driveMotors[3] = hardwareMap.get(DcMotorEx.class, constants.rearRightDriveMotorName); steeringServos[0] = hardwareMap.get(CRServo.class, constants.frontLeftSteeringServoName); steeringServos[1] = hardwareMap.get(CRServo.class, constants.frontRightSteeringServoName); steeringServos[2] = hardwareMap.get(CRServo.class, constants.rearLeftSteeringServoName); - steeringServos[3] = hardwareMap.get(CRServo.class, constants.rearLeftDriveMotorName); + steeringServos[3] = hardwareMap.get(CRServo.class, constants.rearRightSteeringServoName); // Initialize absolute encoders for steering feedback + steeringEncoders[0] = new AbsoluteAnalogEncoder( hardwareMap.get(AnalogInput.class, constants.frontLeftEncoderName)) - .zero(constants.frontLeftEncoderOffset); + .zero(constants.frontLeftEncoderOffset) + .setInverted(constants.isFrontLeftEncoderInverted); + steeringEncoders[1] = new AbsoluteAnalogEncoder( hardwareMap.get(AnalogInput.class, constants.frontRightEncoderName)) - .zero(constants.frontRightEncoderOffset); + .zero(constants.frontRightEncoderOffset) + .setInverted(constants.isFrontRightEncoderInverted); steeringEncoders[2] = new AbsoluteAnalogEncoder( - hardwareMap.get(AnalogInput.class, constants.rearLeftSteeringServoName)) - .zero(constants.rearLeftEncoderOffset); + hardwareMap.get(AnalogInput.class, constants.rearLeftEncoderName)) + .zero(constants.rearLeftEncoderOffset) + .setInverted(constants.isRearLeftEncoderInverted); steeringEncoders[3] = new AbsoluteAnalogEncoder( - hardwareMap.get(AnalogInput.class, constants.rearLeftDriveMotorName)) - .zero(constants.rearRightEncoderOffset); + hardwareMap.get(AnalogInput.class, constants.rearRightEncoderName)) + .zero(constants.rearRightEncoderOffset) + .setInverted(constants.isRearRightEncoderInverted); // Set motor directions driveMotors[0].setDirection(constants.frontLeftDriveMotorDirection); driveMotors[1].setDirection(constants.frontRightDriveMotorDirection); - driveMotors[2].setDirection(constants.backLeftDriveMotorDirection); - driveMotors[3].setDirection(constants.backRightDriveMotorDirection); + driveMotors[2].setDirection(constants.rearLeftDriveMotorDirection); + driveMotors[3].setDirection(constants.rearRightDriveMotorDirection); // Set servo directions steeringServos[0].setDirection(constants.frontLeftSteeringServoDirection); steeringServos[1].setDirection(constants.frontRightSteeringServoDirection); - steeringServos[2].setDirection(constants.backLeftSteeringServoDirection); - steeringServos[3].setDirection(constants.backRightSteeringServoDirection); + steeringServos[2].setDirection(constants.rearLeftSteeringServoDirection); + steeringServos[3].setDirection(constants.rearRightSteeringServoDirection); // Initialize module positions (relative to robot center) // These represent the physical location of each module on the robot @@ -118,7 +138,7 @@ public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants consta // Set motor modes for (DcMotorEx motor : driveMotors) { - motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); } } @@ -139,10 +159,7 @@ public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants consta @Override public double[] calculateDrive(Vector correctivePower, Vector headingPower, Vector pathingPower, double robotHeading) { // Combine all the input vectors into a single desired movement vector - Vector desiredTranslation = new Vector( - correctivePower.getXComponent() + pathingPower.getXComponent(), - correctivePower.getYComponent() + pathingPower.getYComponent() - ); + Vector desiredTranslation = correctivePower.plus(pathingPower); // Get the desired rotation (heading power magnitude determines rotation speed) double desiredRotation = headingPower.getMagnitude() * Math.signum(headingPower.getXComponent()); @@ -157,9 +174,17 @@ public double[] calculateDrive(Vector correctivePower, Vector headingPower, Vect // Array to store module speeds and angles double[] moduleSpeeds = new double[4]; double[] moduleAngles = new double[4]; - + if (desiredTranslation.getMagnitude() < 0.02 && + Math.abs(desiredRotation) < 0.02) { + double[] zeroPowers = new double[8]; + for (int i = 0; i < 8; i++) { + zeroPowers[i] = 0; + } + return zeroPowers; + } // Calculate the desired state for each swerve module for (int i = 0; i < 4; i++) { + // Calculate the rotational contribution for this module // Rotation creates a tangent vector perpendicular to the module's position double rotationX = -modulePositions[i].getYComponent() * desiredRotation; @@ -175,20 +200,15 @@ public double[] calculateDrive(Vector correctivePower, Vector headingPower, Vect // Optimize the module angle to avoid rotating more than 90 degrees // If the module needs to rotate >90°, we can reverse the drive direction instead - double angleDifference = moduleAngles[i] - currentAngles[i]; - - // Normalize angle difference to [-PI, PI] - while (angleDifference > Math.PI) angleDifference -= 2 * Math.PI; - while (angleDifference < -Math.PI) angleDifference += 2 * Math.PI; + double angleDifference = Angle.normDelta(moduleAngles[i] - currentAngles[i]); // If we need to turn more than 90 degrees, flip the angle and reverse speed if (Math.abs(angleDifference) > Math.PI / 2) { moduleAngles[i] += Math.PI; moduleSpeeds[i] *= -1; - // Normalize the angle - while (moduleAngles[i] > Math.PI) moduleAngles[i] -= 2 * Math.PI; - while (moduleAngles[i] < -Math.PI) moduleAngles[i] += 2 * Math.PI; + // Normalize the angle [-pi, pi] + moduleAngles[i] = Angle.normDelta(moduleAngles[i]); } // Store the target angle for this module @@ -220,11 +240,25 @@ public double[] calculateDrive(Vector correctivePower, Vector headingPower, Vect // Get actual current angle from absolute encoder currentAngles[i] = steeringEncoders[i].getCurrentPosition(); - // Calculate angle error using the Angle utility for proper wrapping + // Calculate angle error double angleError = Angle.normDelta(targetAngles[i] - currentAngles[i]); - // PID control for steering (currently just P) - steeringPowers[i] = MathFunctions.clamp(angleError * constants.steeringKp, -1, 1); + double derivative = angleError - lastAngleError[i]; + + double steeringPower = + constants.steeringKp * angleError + - constants.steeringKd * derivative; + + // Small minimum power to overcome static friction + if (Math.abs(steeringPower) < constants.steeringMinPower && + Math.abs(angleError) > constants.steeringDeadband) { + steeringPower = Math.signum(steeringPower) * constants.steeringMinPower; + } + + steeringPowers[i] = MathFunctions.clamp(steeringPower, -1, 1); + + lastAngleError[i] = angleError; + } // Apply voltage compensation if enabled @@ -257,17 +291,31 @@ public void runDrive(double[] drivePowers) { throw new IllegalArgumentException("Drive powers array must have length 8 for coaxial swerve"); } + double[] limitedPowers = new double[8]; + for (int i = 0; i < 4; i++) { + // Drive motor slew rate limiting + if (constants.useDriveSlewRateLimiting) { + limitedPowers[i * 2] = driveRateLimiters[i].calculate(drivePowers[i * 2]); + } else { + limitedPowers[i * 2] = drivePowers[i * 2]; + } + + limitedPowers[i * 2 + 1] = drivePowers[i * 2 + 1]; + + + } // Set drive motor powers - driveMotors[0].setPower(drivePowers[0]); - driveMotors[1].setPower(drivePowers[2]); - driveMotors[2].setPower(drivePowers[4]); - driveMotors[3].setPower(drivePowers[6]); + driveMotors[0].setPower(limitedPowers[0]); + driveMotors[1].setPower(limitedPowers[2]); + driveMotors[2].setPower(limitedPowers[4]); + driveMotors[3].setPower(limitedPowers[6]); // Set steering servo powers - steeringServos[0].setPower(drivePowers[1]); - steeringServos[1].setPower(drivePowers[3]); - steeringServos[2].setPower(drivePowers[5]); - steeringServos[3].setPower(drivePowers[7]); + steeringServos[0].setPower(limitedPowers[1]); + steeringServos[1].setPower(limitedPowers[3]); + steeringServos[2].setPower(limitedPowers[5]); + steeringServos[3].setPower(limitedPowers[7]); + } @Override @@ -288,6 +336,11 @@ public void breakFollowing() { for (CRServo servo : steeringServos) { servo.setPower(0); } + + // Reset slew rate limiters to zero + for (int i = 0; i < 4; i++) { + driveRateLimiters[i].reset(0); + } } @Override @@ -349,7 +402,6 @@ public double getVoltage() { // Return nominal voltage if sensor unavailable return nominalVoltage; } - @Override public String debugString() { StringBuilder sb = new StringBuilder(); @@ -363,4 +415,7 @@ public String debugString() { sb.append(String.format("Voltage: %.2fV\n", currentVoltage)); return sb.toString(); } -} \ No newline at end of file +} + + + diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/SlewRateLimiter.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/SlewRateLimiter.java new file mode 100644 index 00000000..c329e0d5 --- /dev/null +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/SlewRateLimiter.java @@ -0,0 +1,107 @@ +package org.firstinspires.ftc.sixteen750.swerveutil; + +/** + * Slew Rate Limiter - Limits the rate of change of a value + * + * This prevents sudden jumps in motor power or servo position, + * providing smoother control and reducing mechanical stress. + * + * Example uses: + * - Limiting drive motor acceleration + * - Smoothing steering servo movements + * - Preventing sudden direction changes + */ +public class SlewRateLimiter { + + private double maxRateOfChange; // Maximum change per update + private double previousValue; // Last output value + private long lastUpdateTime; // Timestamp of last update (nanoseconds) + + /** + * Creates a slew rate limiter with a maximum rate of change + * + * @param maxRateOfChange maximum change allowed per second + * For example: 2.0 means value can change by 2.0 per second + * (from 0 to 1.0 would take 0.5 seconds) + */ + public SlewRateLimiter(double maxRateOfChange) { + this(maxRateOfChange, 0.0); + } + + /** + * Creates a slew rate limiter with initial value + * + * @param maxRateOfChange maximum change allowed per second + * @param initialValue starting value for the limiter + */ + public SlewRateLimiter(double maxRateOfChange, double initialValue) { + this.maxRateOfChange = maxRateOfChange; + this.previousValue = initialValue; + this.lastUpdateTime = System.nanoTime(); + } + + /** + * Calculate the next output value with slew rate limiting applied + * + * @param input desired target value + * @return limited output value that doesn't exceed max rate of change + */ + public double calculate(double input) { + // Calculate time since last update (in seconds) + long currentTime = System.nanoTime(); + double deltaTime = (currentTime - lastUpdateTime) / 1_000_000_000.0; + lastUpdateTime = currentTime; + + // Calculate maximum allowed change for this time step + double maxChange = maxRateOfChange * deltaTime; + + // Calculate the difference between desired and current + double difference = input - previousValue; + + // Limit the change to maxChange + double limitedChange = Math.clamp(difference, -maxChange, maxChange); + + // Calculate new value + previousValue = previousValue + limitedChange; + + return previousValue; + } + + /** + * Reset the limiter to a new value + * Useful when you want to jump to a new value without limiting + * + * @param value new value to reset to + */ + public void reset(double value) { + previousValue = value; + lastUpdateTime = System.nanoTime(); + } + + /** + * Get the current output value without updating + * + * @return current limited value + */ + public double getCurrentValue() { + return previousValue; + } + + /** + * Set a new maximum rate of change + * + * @param maxRateOfChange new maximum rate (units per second) + */ + public void setMaxRateOfChange(double maxRateOfChange) { + this.maxRateOfChange = maxRateOfChange; + } + + /** + * Get the current maximum rate of change + * + * @return maximum rate of change (units per second) + */ + public double getMaxRateOfChange() { + return maxRateOfChange; + } +} \ No newline at end of file From e4535e7acfd3eae8505eeb76dcedf2b2fa4f88bf Mon Sep 17 00:00:00 2001 From: koolpoolo <181432562+koolpoolo@users.noreply.github.com> Date: Thu, 18 Dec 2025 23:24:08 -0800 Subject: [PATCH 3/8] got some smoothing going to prevent jittering --- .../opmodes/SwerveCalibrationOpMode.java | 8 +- .../swerveutil/AbsoluteAnalogEncoder.java | 92 ++++++++++++++++--- .../swerveutil/CoaxialSwerveConstants.java | 12 +++ .../swerveutil/CoaxialSwerveDrive.java | 26 ++++-- 4 files changed, 117 insertions(+), 21 deletions(-) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/SwerveCalibrationOpMode.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/SwerveCalibrationOpMode.java index 5aeec3a8..1bb528c5 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/SwerveCalibrationOpMode.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/SwerveCalibrationOpMode.java @@ -60,10 +60,10 @@ public void runOpMode() { while (opModeIsActive()) { // Read current positions - double flPos = flEncoder.getCurrentPosition(); - double frPos = frEncoder.getCurrentPosition(); - double blPos = blEncoder.getCurrentPosition(); - double brPos = brEncoder.getCurrentPosition(); + 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(); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/AbsoluteAnalogEncoder.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/AbsoluteAnalogEncoder.java index 3623d910..7a629fe4 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/AbsoluteAnalogEncoder.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/AbsoluteAnalogEncoder.java @@ -1,47 +1,117 @@ package org.firstinspires.ftc.sixteen750.swerveutil; - import com.bylazar.configurables.annotations.Configurable; import com.qualcomm.robotcore.hardware.AnalogInput; -import com.technototes.library.hardware.sensor.encoder.Encoder; @Configurable public class AbsoluteAnalogEncoder { public static double DEFAULT_RANGE = 3.3; - public static boolean VALUE_REJECTION = false; + private final AnalogInput encoder; - private double offset, analogRange;; + 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;; + 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; + } - - private double pastPosition = 1; + /** + * Get the current position of the encoder in radians [0, 2π] + * with offset applied and filtering + * @return position in radians + */ public double getCurrentPosition() { - double pos = Angle.norm((!inverted ? 1 - getVoltage() / analogRange : getVoltage() / analogRange) * Math.PI*2 - offset); - //checks for crazy values when the encoder is close to zero - if(!VALUE_REJECTION || Math.abs(Angle.normDelta(pastPosition)) > 0.1 || Math.abs(Angle.normDelta(pos)) < 1) pastPosition = pos; - return pastPosition; + 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; + } } \ No newline at end of file diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java index d6eabebe..342a4e2e 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java @@ -38,6 +38,10 @@ public class CoaxialSwerveConstants { public double frontRightEncoderOffset = 0.0; public double rearLeftEncoderOffset = 0.0; public double rearRightEncoderOffset = 0.0; + public double frontLeftSmoothing = 0.3; + public double frontRightSmoothing = 0.3; + public double rearLeftSmoothing = 0.3; + public double rearRightSmoothing = 0.3; // encoder inverted status public boolean isFrontLeftEncoderInverted = false; @@ -204,6 +208,14 @@ public CoaxialSwerveConstants withDimensions(double trackWidth, double wheelBase return this; } + public CoaxialSwerveConstants withEncoderSmoothing(double fl, double fr, double rl, double rr) { + this.frontLeftSmoothing = fl; + this.frontRightSmoothing = fr; + this.rearLeftSmoothing = rl; + this.rearRightSmoothing = rr; + return this; + } + /** * Builder-style method to set steering parameters */ diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java index bd608e22..5439622e 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java @@ -49,6 +49,7 @@ public class CoaxialSwerveDrive extends Drivetrain { // Voltage tracking private double currentVoltage = 12.0; + double lastUpdateTime = 0; double[] lastAngleError = new double[4]; /** @@ -101,20 +102,24 @@ public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants consta steeringEncoders[0] = new AbsoluteAnalogEncoder( hardwareMap.get(AnalogInput.class, constants.frontLeftEncoderName)) .zero(constants.frontLeftEncoderOffset) - .setInverted(constants.isFrontLeftEncoderInverted); + .setInverted(constants.isFrontLeftEncoderInverted) + .setSmoothing(constants.frontLeftSmoothing); steeringEncoders[1] = new AbsoluteAnalogEncoder( hardwareMap.get(AnalogInput.class, constants.frontRightEncoderName)) .zero(constants.frontRightEncoderOffset) - .setInverted(constants.isFrontRightEncoderInverted); + .setInverted(constants.isFrontRightEncoderInverted) + .setSmoothing(constants.frontRightSmoothing); steeringEncoders[2] = new AbsoluteAnalogEncoder( hardwareMap.get(AnalogInput.class, constants.rearLeftEncoderName)) .zero(constants.rearLeftEncoderOffset) - .setInverted(constants.isRearLeftEncoderInverted); + .setInverted(constants.isRearLeftEncoderInverted) + .setSmoothing(constants.rearLeftSmoothing); steeringEncoders[3] = new AbsoluteAnalogEncoder( hardwareMap.get(AnalogInput.class, constants.rearRightEncoderName)) .zero(constants.rearRightEncoderOffset) - .setInverted(constants.isRearRightEncoderInverted); + .setInverted(constants.isRearRightEncoderInverted) + .setSmoothing(constants.rearRightSmoothing); // Set motor directions driveMotors[0].setDirection(constants.frontLeftDriveMotorDirection); @@ -242,8 +247,17 @@ public double[] calculateDrive(Vector correctivePower, Vector headingPower, Vect // Calculate angle error double angleError = Angle.normDelta(targetAngles[i] - currentAngles[i]); + if (Math.abs(angleError) < constants.steeringDeadband) { + steeringPowers[i] = 0.0; + lastAngleError[i] = angleError; // Still update for derivative + continue; // Skip PD calculation + } + + long currentTime = System.currentTimeMillis(); + double dt = (currentTime - lastUpdateTime) / 1000.0; + double derivative = (angleError - lastAngleError[i]) / dt; + - double derivative = angleError - lastAngleError[i]; double steeringPower = constants.steeringKp * angleError @@ -270,7 +284,7 @@ public double[] calculateDrive(Vector correctivePower, Vector headingPower, Vect steeringPowers[i] *= voltageScale; } } - + lastUpdateTime = System.currentTimeMillis(); // Return interleaved array: [drive0, steer0, drive1, steer1, ...] return new double[] { moduleSpeeds[0], steeringPowers[0], From a494b92c7d354a54b87d4d9a6e2c23a28f2793fc Mon Sep 17 00:00:00 2001 From: koolpoolo <181432562+koolpoolo@users.noreply.github.com> Date: Thu, 18 Dec 2025 23:25:13 -0800 Subject: [PATCH 4/8] got rid of stuff i dont need --- .../sixteen750/swerveutil/CoaxialSwerveConstants.java | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java index 342a4e2e..da2f3fdc 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java @@ -112,17 +112,6 @@ public class CoaxialSwerveConstants { */ public double driveMaxAcceleration = 4.0; // 0 to 1.0 in 0.25 seconds - /** - * Enable slew rate limiting for steering servos - * Prevents sudden steering movements - */ - public boolean useSteeringSlewRateLimiting = true; - - /** - * Maximum steering power change per second - * This smooths out steering servo movements - */ - public double steeringMaxAcceleration = 8.0; // 0 to 1.0 in 0.125 seconds public double steeringDeadband = Math.toRadians(2); public double steeringMinPower = 0.05; From aaa91325df6ff599aba793cfba367208539fe6a3 Mon Sep 17 00:00:00 2001 From: koolpoolo <181432562+koolpoolo@users.noreply.github.com> Date: Fri, 19 Dec 2025 13:08:31 -0800 Subject: [PATCH 5/8] made it a little more consise --- .../swerveutil/CoaxialSwerveDrive.java | 22 +++++++------------ 1 file changed, 8 insertions(+), 14 deletions(-) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java index 5439622e..5c4a6549 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java @@ -52,12 +52,6 @@ public class CoaxialSwerveDrive extends Drivetrain { double lastUpdateTime = 0; double[] lastAngleError = new double[4]; - /** - * Constructor for the Coaxial Swerve Drivetrain - * - * @param hardwareMap the FTC hardware map to get motors and servos - * @param constants the constants object containing configuration parameters - */ public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants constants) { this.constants = constants; this.maxPowerScaling = 1.0; @@ -143,8 +137,7 @@ public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants consta // Set motor modes for (DcMotorEx motor : driveMotors) { - motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } } @@ -155,10 +148,12 @@ public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants consta * 1. What direction each module should point * 2. How fast each module should drive * - * @param correctivePower translational correction vector (for path following) - * @param headingPower rotational power for turning - * @param pathingPower forward movement vector along the path - * @param robotHeading current robot heading in radians + * @param correctivePower this Vector includes the centrifugal force scaling Vector as well as a + * translational power Vector to correct onto the Bezier curve the Follower is following. + * @param headingPower this Vector points in the direction of the robot's current heading, and + * the magnitude tells the robot how much it should turn and in which direction. + * @param pathingPower this Vector points in the direction the robot needs to go to continue along the Path. + * @param robotHeading this is the current heading of the robot, which is used to calculate how much power to allocate to each wheel. * @return array of 8 values: [drive0, steer0, drive1, steer1, drive2, steer2, drive3, steer3] */ @Override @@ -167,7 +162,7 @@ public double[] calculateDrive(Vector correctivePower, Vector headingPower, Vect Vector desiredTranslation = correctivePower.plus(pathingPower); // Get the desired rotation (heading power magnitude determines rotation speed) - double desiredRotation = headingPower.getMagnitude() * Math.signum(headingPower.getXComponent()); + double desiredRotation = headingPower.getXComponent(); // Convert field-relative movement to robot-relative // This rotates the desired movement vector by the robot's current heading @@ -368,7 +363,6 @@ public void startTeleopDrive(boolean brakeMode) { DcMotor.ZeroPowerBehavior.BRAKE : DcMotor.ZeroPowerBehavior.FLOAT; for (DcMotorEx motor : driveMotors) { - motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); motor.setZeroPowerBehavior(behavior); } } From dd15beaad41fc5b2d37bd57b13172a29712fb79d Mon Sep 17 00:00:00 2001 From: koolpoolo <181432562+koolpoolo@users.noreply.github.com> Date: Fri, 19 Dec 2025 13:10:26 -0800 Subject: [PATCH 6/8] got rid of unused imports --- .../ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java index 5c4a6549..4a3a52c2 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java @@ -1,6 +1,4 @@ package org.firstinspires.ftc.sixteen750.swerveutil; -import android.annotation.SuppressLint; - import com.pedropathing.Drivetrain; import com.pedropathing.math.Vector; import com.pedropathing.math.MathFunctions; @@ -10,8 +8,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.VoltageSensor; -import org.firstinspires.ftc.sixteen750.swerveutil.AbsoluteAnalogEncoder; -import org.firstinspires.ftc.sixteen750.swerveutil.Angle; + /** * Coaxial Swerve Drivetrain Implementation From 0628a61249fd8e51b13ee0db8ce2b53eb10ffe9a Mon Sep 17 00:00:00 2001 From: koolpoolo <181432562+koolpoolo@users.noreply.github.com> Date: Thu, 22 Jan 2026 19:17:28 -0800 Subject: [PATCH 7/8] switched to PIDF steering controller with actual class --- .../swerveutil/CoaxialSwerveConstants.java | 9 +- .../swerveutil/CoaxialSwerveDrive.java | 82 ++++++++++--------- 2 files changed, 47 insertions(+), 44 deletions(-) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java index da2f3fdc..d077c738 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java @@ -2,6 +2,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; /** * Constants class for the Coaxial Swerve Drivetrain @@ -87,8 +88,7 @@ public class CoaxialSwerveConstants { * Higher values make steering more aggressive but can cause oscillation * Start with a low value (0.5-1.0) and increase if steering is too slow */ - public double steeringKp = 1.5; - public double steeringKd = 0; + PIDFCoefficients steeringPIDF = new PIDFCoefficients(0, 0, 0,0); /** * Maximum steering speed (radians per second) @@ -208,9 +208,8 @@ public CoaxialSwerveConstants withEncoderSmoothing(double fl, double fr, double /** * Builder-style method to set steering parameters */ - public CoaxialSwerveConstants withSteeringParams(double kp, double kd, double maxSpeed, double db, double minPower) { - this.steeringKp = kp; - this.steeringKd = kd; + public CoaxialSwerveConstants withSteeringParams(PIDFCoefficients steeringPIDF, double maxSpeed, double db, double minPower) { + this.steeringPIDF = steeringPIDF; this.steeringSpeed = maxSpeed; this.steeringDeadband = db; this.steeringMinPower = minPower; diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java index 4a3a52c2..42ce5f34 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java @@ -8,6 +8,8 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.technototes.library.util.PIDFController; /** @@ -44,10 +46,11 @@ public class CoaxialSwerveDrive extends Drivetrain { // Slew rate limiters for smooth acceleration private SlewRateLimiter[] driveRateLimiters = new SlewRateLimiter[4]; + // PIDF Controllers for steering + private PIDFController[] steeringControllers = new PIDFController[4]; + // Voltage tracking private double currentVoltage = 12.0; - double lastUpdateTime = 0; - double[] lastAngleError = new double[4]; public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants constants) { this.constants = constants; @@ -68,15 +71,17 @@ public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants consta steeringEncoders = new AbsoluteAnalogEncoder[4]; targetAngles = new double[4]; currentAngles = new double[4]; - for (int i = 0; i < 4; i++) { - lastAngleError[i] = 0; - } // Initialize slew rate limiters for (int i = 0; i < 4; i++) { driveRateLimiters[i] = new SlewRateLimiter(constants.driveMaxAcceleration); } + for (int i = 0; i < 4; i++) { + steeringControllers[i] = new PIDFController(constants.steeringPIDF); + steeringControllers[i].setInputBounds(-Math.PI, Math.PI); + } + // Get hardware from the hardware map driveMotors[0] = hardwareMap.get(DcMotorEx.class, constants.frontLeftDriveMotorName); driveMotors[1] = hardwareMap.get(DcMotorEx.class, constants.frontRightDriveMotorName); @@ -89,7 +94,6 @@ public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants consta steeringServos[3] = hardwareMap.get(CRServo.class, constants.rearRightSteeringServoName); // Initialize absolute encoders for steering feedback - steeringEncoders[0] = new AbsoluteAnalogEncoder( hardwareMap.get(AnalogInput.class, constants.frontLeftEncoderName)) .zero(constants.frontLeftEncoderOffset) @@ -101,11 +105,13 @@ public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants consta .zero(constants.frontRightEncoderOffset) .setInverted(constants.isFrontRightEncoderInverted) .setSmoothing(constants.frontRightSmoothing); + steeringEncoders[2] = new AbsoluteAnalogEncoder( hardwareMap.get(AnalogInput.class, constants.rearLeftEncoderName)) .zero(constants.rearLeftEncoderOffset) .setInverted(constants.isRearLeftEncoderInverted) .setSmoothing(constants.rearLeftSmoothing); + steeringEncoders[3] = new AbsoluteAnalogEncoder( hardwareMap.get(AnalogInput.class, constants.rearRightEncoderName)) .zero(constants.rearRightEncoderOffset) @@ -171,6 +177,8 @@ public double[] calculateDrive(Vector correctivePower, Vector headingPower, Vect // Array to store module speeds and angles double[] moduleSpeeds = new double[4]; double[] moduleAngles = new double[4]; + + // If essentially no movement commanded, return zero powers if (desiredTranslation.getMagnitude() < 0.02 && Math.abs(desiredRotation) < 0.02) { double[] zeroPowers = new double[8]; @@ -179,9 +187,9 @@ public double[] calculateDrive(Vector correctivePower, Vector headingPower, Vect } return zeroPowers; } + // Calculate the desired state for each swerve module for (int i = 0; i < 4; i++) { - // Calculate the rotational contribution for this module // Rotation creates a tangent vector perpendicular to the module's position double rotationX = -modulePositions[i].getYComponent() * desiredRotation; @@ -231,40 +239,34 @@ public double[] calculateDrive(Vector correctivePower, Vector headingPower, Vect moduleSpeeds[i] *= maxPowerScaling; } - // Calculate steering servo powers based on angle error + // Calculate steering servo powers using PIDF controllers double[] steeringPowers = new double[4]; for (int i = 0; i < 4; i++) { - // Get actual current angle from absolute encoder - currentAngles[i] = steeringEncoders[i].getCurrentPosition(); - - // Calculate angle error - double angleError = Angle.normDelta(targetAngles[i] - currentAngles[i]); - if (Math.abs(angleError) < constants.steeringDeadband) { - steeringPowers[i] = 0.0; - lastAngleError[i] = angleError; // Still update for derivative - continue; // Skip PD calculation - } + // Get actual current angle from absolute encoder and normalize + currentAngles[i] = Angle.normDelta(steeringEncoders[i].getCurrentPosition()); - long currentTime = System.currentTimeMillis(); - double dt = (currentTime - lastUpdateTime) / 1000.0; - double derivative = (angleError - lastAngleError[i]) / dt; + // Set the target for the controller (already normalized in loop above) + steeringControllers[i].setTarget(targetAngles[i]); + // Use PIDF controller to calculate steering power + // Always call update() to keep controller state consistent + double steeringPower = steeringControllers[i].update(currentAngles[i]); + // Get the error for deadband and minimum power checks + double angleError = steeringControllers[i].getLastError(); - double steeringPower = - constants.steeringKp * angleError - - constants.steeringKd * derivative; + // Apply deadband - set power to zero if error is small + if (Math.abs(angleError) < constants.steeringDeadband) { + steeringPowers[i] = 0.0; + continue; + } // Small minimum power to overcome static friction - if (Math.abs(steeringPower) < constants.steeringMinPower && - Math.abs(angleError) > constants.steeringDeadband) { + if (Math.abs(steeringPower) < constants.steeringMinPower) { steeringPower = Math.signum(steeringPower) * constants.steeringMinPower; } steeringPowers[i] = MathFunctions.clamp(steeringPower, -1, 1); - - lastAngleError[i] = angleError; - } // Apply voltage compensation if enabled @@ -276,7 +278,7 @@ public double[] calculateDrive(Vector correctivePower, Vector headingPower, Vect steeringPowers[i] *= voltageScale; } } - lastUpdateTime = System.currentTimeMillis(); + // Return interleaved array: [drive0, steer0, drive1, steer1, ...] return new double[] { moduleSpeeds[0], steeringPowers[0], @@ -307,9 +309,8 @@ public void runDrive(double[] drivePowers) { } limitedPowers[i * 2 + 1] = drivePowers[i * 2 + 1]; - - } + // Set drive motor powers driveMotors[0].setPower(limitedPowers[0]); driveMotors[1].setPower(limitedPowers[2]); @@ -321,7 +322,6 @@ public void runDrive(double[] drivePowers) { steeringServos[1].setPower(limitedPowers[3]); steeringServos[2].setPower(limitedPowers[5]); steeringServos[3].setPower(limitedPowers[7]); - } @Override @@ -347,6 +347,11 @@ public void breakFollowing() { for (int i = 0; i < 4; i++) { driveRateLimiters[i].reset(0); } + + // Reset PIDF controllers + for (int i = 0; i < 4; i++) { + steeringControllers[i].reset(); + } } @Override @@ -407,20 +412,19 @@ public double getVoltage() { // Return nominal voltage if sensor unavailable return nominalVoltage; } + @Override public String debugString() { StringBuilder sb = new StringBuilder(); sb.append("Coaxial Swerve Drivetrain Debug:\n"); for (int i = 0; i < 4; i++) { - sb.append(String.format("Module %d: Speed=%.2f, Angle=%.2f°, Target=%.2f°\n", + sb.append(String.format("Module %d: Speed=%.2f, Angle=%.2f°, Target=%.2f°, Error=%.2f°\n", i, driveMotors[i].getPower(), Math.toDegrees(currentAngles[i]), - Math.toDegrees(targetAngles[i]))); + Math.toDegrees(targetAngles[i]), + Math.toDegrees(steeringControllers[i].getLastError()))); } sb.append(String.format("Voltage: %.2fV\n", currentVoltage)); return sb.toString(); } -} - - - +} \ No newline at end of file From a8ccf56914032d52c065760de660e29a3c9b4ccd Mon Sep 17 00:00:00 2001 From: koolpoolo <181432562+koolpoolo@users.noreply.github.com> Date: Fri, 23 Jan 2026 19:32:03 -0800 Subject: [PATCH 8/8] updated vectors --- .../swerveutil/CoaxialSwerveDrive.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java index 42ce5f34..00bc4468 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java @@ -133,10 +133,10 @@ public CoaxialSwerveDrive(HardwareMap hardwareMap, CoaxialSwerveConstants consta // Initialize module positions (relative to robot center) // These represent the physical location of each module on the robot modulePositions = new Vector[4]; - modulePositions[0] = new Vector(constants.trackWidth / 2, constants.wheelBase / 2); // Front Left - modulePositions[1] = new Vector(constants.trackWidth / 2, -constants.wheelBase / 2); // Front Right - modulePositions[2] = new Vector(-constants.trackWidth / 2, constants.wheelBase / 2); // Back Left - modulePositions[3] = new Vector(-constants.trackWidth / 2, -constants.wheelBase / 2); // Back Right + modulePositions[0] = new Vector(-constants.trackWidth / 2, constants.wheelBase / 2); // Front Left + modulePositions[1] = new Vector(constants.trackWidth / 2, constants.wheelBase / 2); // Front Right + modulePositions[2] = new Vector(-constants.trackWidth / 2, -constants.wheelBase / 2); // Back Left + modulePositions[3] = new Vector(constants.trackWidth / 2, -constants.wheelBase / 2); // Back Right // Set motor modes for (DcMotorEx motor : driveMotors) { @@ -327,10 +327,10 @@ public void runDrive(double[] drivePowers) { @Override public void updateConstants() { // Update module positions if constants changed - modulePositions[0] = new Vector(constants.trackWidth / 2, constants.wheelBase / 2); - modulePositions[1] = new Vector(constants.trackWidth / 2, -constants.wheelBase / 2); - modulePositions[2] = new Vector(-constants.trackWidth / 2, constants.wheelBase / 2); - modulePositions[3] = new Vector(-constants.trackWidth / 2, -constants.wheelBase / 2); + modulePositions[0] = new Vector(-constants.trackWidth / 2, constants.wheelBase / 2); // Front Left + modulePositions[1] = new Vector(constants.trackWidth / 2, constants.wheelBase / 2); // Front Right + modulePositions[2] = new Vector(-constants.trackWidth / 2, -constants.wheelBase / 2); // Back Left + modulePositions[3] = new Vector(constants.trackWidth / 2, -constants.wheelBase / 2); // Back Right } @Override