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..1bb528c5 --- /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.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(); + } + } +} 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..7a629fe4 --- /dev/null +++ b/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; + } +} \ 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..d077c738 --- /dev/null +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveConstants.java @@ -0,0 +1,218 @@ +package org.firstinspires.ftc.sixteen750.swerveutil; + +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 + * + * This class contains all the configuration parameters needed for the swerve drivetrain. + * Modify these values to match your robot's physical configuration. + */ +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; + 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; + 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 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 rearLeftSteeringServoDirection = CRServo.Direction.FORWARD; + public CRServo.Direction rearRightSteeringServoDirection = 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 + + // ==================== 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 + */ + PIDFCoefficients steeringPIDF = new PIDFCoefficients(0, 0, 0,0); + + /** + * Maximum steering speed (radians per second) + * This limits how fast the modules can rotate + */ + public double steeringSpeed = Math.PI; // 180 degrees per second + + // ==================== SLEW RATE LIMITING ==================== + + /** + * Enable slew rate limiting for drive motors + * Prevents sudden acceleration/deceleration + */ + public boolean useDriveSlewRateLimiting = true; + + /** + * 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 driveMaxAcceleration = 4.0; // 0 to 1.0 in 0.25 seconds + + public double steeringDeadband = Math.toRadians(2); + + public double steeringMinPower = 0.05; + /** + * 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 rl, String rr) { + this.frontLeftEncoderName = fl; + this.frontRightEncoderName = fr; + this.rearLeftEncoderName = rl; + this.rearRightEncoderName = rr; + return this; + } + + /** + * Builder-style method to set encoder offsets + * Use this after calibrating your modules + */ + public CoaxialSwerveConstants withEncoderOffsets(double fl, double fr, double rl, double rr) { + this.frontLeftEncoderOffset = fl; + this.frontRightEncoderOffset = fr; + 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; + } + + /** + * Builder-style method to set physical dimensions + */ + public CoaxialSwerveConstants withDimensions(double trackWidth, double wheelBase, double wheelDiameter) { + this.trackWidth = trackWidth; + this.wheelBase = 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 + */ + public CoaxialSwerveConstants withSteeringParams(PIDFCoefficients steeringPIDF, double maxSpeed, double db, double minPower) { + this.steeringPIDF = steeringPIDF; + 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 new file mode 100644 index 00000000..00bc4468 --- /dev/null +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/swerveutil/CoaxialSwerveDrive.java @@ -0,0 +1,430 @@ +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 com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.technototes.library.util.PIDFController; + + +/** + * 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; + + // 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; + + 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 + 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]; + + // 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); + 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.rearRightSteeringServoName); + + // Initialize absolute encoders for steering feedback + steeringEncoders[0] = new AbsoluteAnalogEncoder( + hardwareMap.get(AnalogInput.class, constants.frontLeftEncoderName)) + .zero(constants.frontLeftEncoderOffset) + .setInverted(constants.isFrontLeftEncoderInverted) + .setSmoothing(constants.frontLeftSmoothing); + + steeringEncoders[1] = new AbsoluteAnalogEncoder( + hardwareMap.get(AnalogInput.class, constants.frontRightEncoderName)) + .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) + .setInverted(constants.isRearRightEncoderInverted) + .setSmoothing(constants.rearRightSmoothing); + + // Set motor directions + driveMotors[0].setDirection(constants.frontLeftDriveMotorDirection); + driveMotors[1].setDirection(constants.frontRightDriveMotorDirection); + 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.rearLeftSteeringServoDirection); + steeringServos[3].setDirection(constants.rearRightSteeringServoDirection); + + // 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); + } + } + + /** + * 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 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 + public double[] calculateDrive(Vector correctivePower, Vector headingPower, Vector pathingPower, double robotHeading) { + // Combine all the input vectors into a single desired movement vector + Vector desiredTranslation = correctivePower.plus(pathingPower); + + // Get the desired rotation (heading power magnitude determines rotation speed) + double desiredRotation = 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]; + + // If essentially no movement commanded, return zero powers + 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; + 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 = 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 [-pi, pi] + moduleAngles[i] = Angle.normDelta(moduleAngles[i]); + } + + // 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 using PIDF controllers + double[] steeringPowers = new double[4]; + for (int i = 0; i < 4; i++) { + // Get actual current angle from absolute encoder and normalize + currentAngles[i] = Angle.normDelta(steeringEncoders[i].getCurrentPosition()); + + // 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(); + + // 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) { + steeringPower = Math.signum(steeringPower) * constants.steeringMinPower; + } + + steeringPowers[i] = MathFunctions.clamp(steeringPower, -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"); + } + + 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(limitedPowers[0]); + driveMotors[1].setPower(limitedPowers[2]); + driveMotors[2].setPower(limitedPowers[4]); + driveMotors[3].setPower(limitedPowers[6]); + + // Set steering servo powers + steeringServos[0].setPower(limitedPowers[1]); + steeringServos[1].setPower(limitedPowers[3]); + steeringServos[2].setPower(limitedPowers[5]); + steeringServos[3].setPower(limitedPowers[7]); + } + + @Override + public void updateConstants() { + // Update module positions if constants changed + 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 + public void breakFollowing() { + // Stop all motors and servos + for (DcMotorEx motor : driveMotors) { + motor.setPower(0); + } + for (CRServo servo : steeringServos) { + servo.setPower(0); + } + + // Reset slew rate limiters to zero + 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 + 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.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°, Error=%.2f°\n", + i, driveMotors[i].getPower(), + Math.toDegrees(currentAngles[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 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