From 41959a1f88f5b78016c3f18fbce7bd2b8e7d52da Mon Sep 17 00:00:00 2001 From: Alex Resnick Date: Sat, 20 Jan 2024 15:56:35 -0600 Subject: [PATCH] IT WORKS!!!!!!!!!!! Next time just steal from the creators originally --- src/main/java/frc/lib/math/Conversions.java | 97 ++--- .../java/frc/lib/util/ctre/CTREConfigs.java | 114 +++--- .../frc/lib/util/ctre/CTREModuleState.java | 61 ---- .../swerve/COTSTalonFXSwerveConstants.java | 332 ++++++++++++++++++ .../frc/lib/util/swerve/SwerveModule.java | 203 +++-------- .../util/swerve/SwerveModuleConstants.java | 19 +- src/main/java/frc/robot/Constants.java | 222 ++++++------ src/main/java/frc/robot/RobotContainer.java | 6 - .../frc/robot/commands/TestTransform.java | 65 ---- .../robot/commands/ZeroMotorsWaitCommand.java | 3 +- .../java/frc/robot/subsystems/Swerve.java | 263 ++++---------- 11 files changed, 648 insertions(+), 737 deletions(-) delete mode 100644 src/main/java/frc/lib/util/ctre/CTREModuleState.java create mode 100644 src/main/java/frc/lib/util/swerve/COTSTalonFXSwerveConstants.java delete mode 100644 src/main/java/frc/robot/commands/TestTransform.java diff --git a/src/main/java/frc/lib/math/Conversions.java b/src/main/java/frc/lib/math/Conversions.java index e48082e..84781f3 100644 --- a/src/main/java/frc/lib/math/Conversions.java +++ b/src/main/java/frc/lib/math/Conversions.java @@ -1,93 +1,44 @@ package frc.lib.math; -/** - * Mathematical conversions for swerve calculations - */ public class Conversions { /** - * @param counts Falcon Counts - * @param gearRatio Gear Ratio between Falcon and Mechanism - * @return Degrees of Rotation of Mechanism falconToDegrees + * @param wheelRPS Wheel Velocity: (in Rotations per Second) + * @param circumference Wheel Circumference: (in Meters) + * @return Wheel Velocity: (in Meters per Second) */ - public static double falconToDegrees(double counts, double gearRatio) { - return counts * (360.0 / (gearRatio * 2048.0)); - } - - /** - * @param degrees Degrees of rotation of Mechanism - * @param gearRatio Gear Ratio between Falcon and Mechanism - * @return Falcon Counts degreesToFalcon - */ - public static double degreesToFalcon(double degrees, double gearRatio) { - double ticks = degrees / (360.0 / (gearRatio * 2048.0)); - return ticks; - } - - /** - * @param velocityCounts Falcon Velocity Counts - * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) - * @return RPM of Mechanism - */ - public static double falconToRPM(double velocityCounts, double gearRatio) { - double motorRPM = velocityCounts * (600.0 / 2048.0); - double mechRPM = motorRPM / gearRatio; - return mechRPM; - } - - /** - * @param rpm RPM of mechanism - * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) - * @return RPM of Mechanism - */ - public static double rpmToFalcon(double rpm, double gearRatio) { - double motorRPM = rpm * gearRatio; - double sensorCounts = motorRPM * (2048.0 / 600.0); - return sensorCounts; - } - - /** - * @param counts Falcon Counts - * @param gearRatio Gear Ratio between Falcon and Mechanism - * @return Degrees of Rotation of Mechanism falconToDegrees - */ - public static double falconToMeters(double counts, double gearRatio, double circumference) { - return counts * circumference / (gearRatio * 2048.0); + public static double RPSToMPS(double wheelRPS, double circumference) { + double wheelMPS = wheelRPS * circumference; + return wheelMPS; } /** - * @param velocitycounts Falcon Velocity Counts - * @param circumference Circumference of Wheel - * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) - * @return Falcon Velocity Counts + * @param wheelMPS Wheel Velocity: (in Meters per Second) + * @param circumference Wheel Circumference: (in Meters) + * @return Wheel Velocity: (in Rotations per Second) */ - public static double falconToMPS(double velocitycounts, double circumference, - double gearRatio) { - double wheelRPM = falconToRPM(velocitycounts, gearRatio); - double wheelMPS = (wheelRPM * circumference) / 60; - return wheelMPS; + public static double MPSToRPS(double wheelMPS, double circumference) { + double wheelRPS = wheelMPS / circumference; + return wheelRPS; } /** - * @param velocity Velocity MPS - * @param circumference Circumference of Wheel - * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) - * @return Falcon Velocity Counts + * @param wheelRotations Wheel Position: (in Rotations) + * @param circumference Wheel Circumference: (in Meters) + * @return Wheel Distance: (in Meters) */ - public static double mpsToFalcon(double velocity, double circumference, double gearRatio) { - double wheelRPM = ((velocity * 60) / circumference); - double wheelVelocity = rpmToFalcon(wheelRPM, gearRatio); - return wheelVelocity; + public static double rotationsToMeters(double wheelRotations, double circumference) { + double wheelMeters = wheelRotations * circumference; + return wheelMeters; } /** - * Normalize angle to between 0 to 360 - * - * @param goal initial angle - * @return normalized angle + * @param wheelMeters Wheel Distance: (in Meters) + * @param circumference Wheel Circumference: (in Meters) + * @return Wheel Position: (in Rotations) */ - public static double reduceTo0_360(double goal) { - return goal % 360; + public static double metersToRotations(double wheelMeters, double circumference) { + double wheelRotations = wheelMeters / circumference; + return wheelRotations; } - } diff --git a/src/main/java/frc/lib/util/ctre/CTREConfigs.java b/src/main/java/frc/lib/util/ctre/CTREConfigs.java index 2bea54f..c42ccbd 100644 --- a/src/main/java/frc/lib/util/ctre/CTREConfigs.java +++ b/src/main/java/frc/lib/util/ctre/CTREConfigs.java @@ -1,61 +1,71 @@ package frc.lib.util.ctre; -import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; -import com.ctre.phoenix.sensors.AbsoluteSensorRange; -import com.ctre.phoenix.sensors.CANCoderConfiguration; -import com.ctre.phoenix.sensors.SensorInitializationStrategy; -import com.ctre.phoenix.sensors.SensorTimeBase; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import frc.robot.Constants; -/** - * CTRE config constants - */ public final class CTREConfigs { - public TalonFXConfiguration swerveAngleFXConfig; - public TalonFXConfiguration swerveDriveFXConfig; - public CANCoderConfiguration swerveCanCoderConfig; + public TalonFXConfiguration swerveAngleFXConfig = new TalonFXConfiguration(); + public TalonFXConfiguration swerveDriveFXConfig = new TalonFXConfiguration(); + public CANcoderConfiguration swerveCANcoderConfig = new CANcoderConfiguration(); - /** - * CTRE config constants - */ public CTREConfigs() { - swerveAngleFXConfig = new TalonFXConfiguration(); - swerveDriveFXConfig = new TalonFXConfiguration(); - swerveCanCoderConfig = new CANCoderConfiguration(); - - /* Swerve Angle Motor Configurations */ - SupplyCurrentLimitConfiguration angleSupplyLimit = new SupplyCurrentLimitConfiguration( - Constants.Swerve.angleEnableCurrentLimit, Constants.Swerve.angleContinuousCurrentLimit, - Constants.Swerve.anglePeakCurrentLimit, Constants.Swerve.anglePeakCurrentDuration); - - swerveAngleFXConfig.slot0.kP = Constants.Swerve.angleKP; - swerveAngleFXConfig.slot0.kI = Constants.Swerve.angleKI; - swerveAngleFXConfig.slot0.kD = Constants.Swerve.angleKD; - swerveAngleFXConfig.slot0.kF = Constants.Swerve.angleKF; - swerveAngleFXConfig.supplyCurrLimit = angleSupplyLimit; - swerveAngleFXConfig.initializationStrategy = SensorInitializationStrategy.BootToZero; - - /* Swerve Drive Motor Configuration */ - SupplyCurrentLimitConfiguration driveSupplyLimit = new SupplyCurrentLimitConfiguration( - Constants.Swerve.driveEnableCurrentLimit, Constants.Swerve.driveContinuousCurrentLimit, - Constants.Swerve.drivePeakCurrentLimit, Constants.Swerve.drivePeakCurrentDuration); - - swerveDriveFXConfig.slot0.kP = Constants.Swerve.driveKP; - swerveDriveFXConfig.slot0.kI = Constants.Swerve.driveKI; - swerveDriveFXConfig.slot0.kD = Constants.Swerve.driveKD; - swerveDriveFXConfig.slot0.kF = Constants.Swerve.driveKF; - swerveDriveFXConfig.supplyCurrLimit = driveSupplyLimit; - swerveDriveFXConfig.initializationStrategy = SensorInitializationStrategy.BootToZero; - swerveDriveFXConfig.openloopRamp = Constants.Swerve.openLoopRamp; - swerveDriveFXConfig.closedloopRamp = Constants.Swerve.closedLoopRamp; - - /* Swerve CANCoder Configuration */ - swerveCanCoderConfig.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360; - swerveCanCoderConfig.sensorDirection = Constants.Swerve.canCoderInvert; - swerveCanCoderConfig.initializationStrategy = - SensorInitializationStrategy.BootToAbsolutePosition; - swerveCanCoderConfig.sensorTimeBase = SensorTimeBase.PerSecond; - } + /** Swerve CANCoder Configuration */ + swerveCANcoderConfig.MagnetSensor.SensorDirection = Constants.Swerve.cancoderInvert; + + /** Swerve Angle Motor Configurations */ + /* Motor Inverts and Neutral Mode */ + swerveAngleFXConfig.MotorOutput.Inverted = Constants.Swerve.angleMotorInvert; + swerveAngleFXConfig.MotorOutput.NeutralMode = Constants.Swerve.angleNeutralMode; + + /* Gear Ratio and Wrapping Config */ + swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.angleGearRatio; + swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true; + + /* Current Limiting */ + swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimitEnable = + Constants.Swerve.angleEnableCurrentLimit; + swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.angleCurrentLimit; + swerveAngleFXConfig.CurrentLimits.SupplyCurrentThreshold = + Constants.Swerve.angleCurrentThreshold; + swerveAngleFXConfig.CurrentLimits.SupplyTimeThreshold = + Constants.Swerve.angleCurrentThresholdTime; + + /* PID Config */ + swerveAngleFXConfig.Slot0.kP = Constants.Swerve.angleKP; + swerveAngleFXConfig.Slot0.kI = Constants.Swerve.angleKI; + swerveAngleFXConfig.Slot0.kD = Constants.Swerve.angleKD; + + /** Swerve Drive Motor Configuration */ + /* Motor Inverts and Neutral Mode */ + swerveDriveFXConfig.MotorOutput.Inverted = Constants.Swerve.driveMotorInvert; + swerveDriveFXConfig.MotorOutput.NeutralMode = Constants.Swerve.driveNeutralMode; + /* Gear Ratio Config */ + swerveDriveFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.driveGearRatio; + + /* Current Limiting */ + swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimitEnable = + Constants.Swerve.driveEnableCurrentLimit; + swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.driveCurrentLimit; + swerveDriveFXConfig.CurrentLimits.SupplyCurrentThreshold = + Constants.Swerve.driveCurrentThreshold; + swerveDriveFXConfig.CurrentLimits.SupplyTimeThreshold = + Constants.Swerve.driveCurrentThresholdTime; + + /* PID Config */ + swerveDriveFXConfig.Slot0.kP = Constants.Swerve.driveKP; + swerveDriveFXConfig.Slot0.kI = Constants.Swerve.driveKI; + swerveDriveFXConfig.Slot0.kD = Constants.Swerve.driveKD; + + /* Open and Closed Loop Ramping */ + swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = + Constants.Swerve.openLoopRamp; + swerveDriveFXConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Constants.Swerve.openLoopRamp; + + swerveDriveFXConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = + Constants.Swerve.closedLoopRamp; + swerveDriveFXConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = + Constants.Swerve.closedLoopRamp; + } } diff --git a/src/main/java/frc/lib/util/ctre/CTREModuleState.java b/src/main/java/frc/lib/util/ctre/CTREModuleState.java deleted file mode 100644 index 9c844d9..0000000 --- a/src/main/java/frc/lib/util/ctre/CTREModuleState.java +++ /dev/null @@ -1,61 +0,0 @@ -package frc.lib.util.ctre; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; - -/** - * Retrieve and set module states on CTRE devices - */ -public class CTREModuleState { - - /** - * Minimize the change in heading the desired swerve module state would require by potentially - * reversing the direction the wheel spins. Customized from WPILib's version to include placing - * in appropriate scope for CTRE onboard control. - * - * @param desiredState The desired state. - * @param currentAngle The current module angle. - */ - public static SwerveModuleState optimize(SwerveModuleState desiredState, - Rotation2d currentAngle) { - double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), - desiredState.angle.getDegrees()); - double targetSpeed = desiredState.speedMetersPerSecond; - double delta = targetAngle - currentAngle.getDegrees(); - if (Math.abs(delta) > 90) { - targetSpeed = -targetSpeed; - targetAngle = delta > 90 ? (targetAngle -= 180) : (targetAngle += 180); - } - return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle)); - } - - /** - * @param scopeReference Current Angle - * @param newAngle Target Angle - * @return Closest angle within scope - */ - private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) { - double lowerBound; - double upperBound; - double lowerOffset = scopeReference % 360; - if (lowerOffset >= 0) { - lowerBound = scopeReference - lowerOffset; - upperBound = scopeReference + (360 - lowerOffset); - } else { - upperBound = scopeReference - lowerOffset; - lowerBound = scopeReference - (360 + lowerOffset); - } - while (newAngle < lowerBound) { - newAngle += 360; - } - while (newAngle > upperBound) { - newAngle -= 360; - } - if (newAngle - scopeReference > 180) { - newAngle -= 360; - } else if (newAngle - scopeReference < -180) { - newAngle += 360; - } - return newAngle; - } -} diff --git a/src/main/java/frc/lib/util/swerve/COTSTalonFXSwerveConstants.java b/src/main/java/frc/lib/util/swerve/COTSTalonFXSwerveConstants.java new file mode 100644 index 0000000..73deb32 --- /dev/null +++ b/src/main/java/frc/lib/util/swerve/COTSTalonFXSwerveConstants.java @@ -0,0 +1,332 @@ +package frc.lib.util.swerve; + +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.math.util.Units; + +/* Contains values and required settings for common COTS swerve modules. */ +public class COTSTalonFXSwerveConstants { + public final double wheelDiameter; + public final double wheelCircumference; + public final double angleGearRatio; + public final double driveGearRatio; + public final double angleKP; + public final double angleKI; + public final double angleKD; + public final InvertedValue driveMotorInvert; + public final InvertedValue angleMotorInvert; + public final SensorDirectionValue cancoderInvert; + + public COTSTalonFXSwerveConstants(double wheelDiameter, double angleGearRatio, + double driveGearRatio, double angleKP, double angleKI, double angleKD, + InvertedValue driveMotorInvert, InvertedValue angleMotorInvert, + SensorDirectionValue cancoderInvert) { + this.wheelDiameter = wheelDiameter; + this.wheelCircumference = wheelDiameter * Math.PI; + this.angleGearRatio = angleGearRatio; + this.driveGearRatio = driveGearRatio; + this.angleKP = angleKP; + this.angleKI = angleKI; + this.angleKD = angleKD; + this.driveMotorInvert = driveMotorInvert; + this.angleMotorInvert = angleMotorInvert; + this.cancoderInvert = cancoderInvert; + } + + /** West Coast Products */ + public static final class WCP { + /** West Coast Products - SwerveX Standard */ + public static final class SwerveXStandard { + /** West Coast Products - SwerveX Standard (Falcon 500) */ + public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** (396 / 35) : 1 */ + double angleGearRatio = ((396.0 / 35.0) / 1.0); + + double angleKP = 1.0; + double angleKI = 0.0; + double angleKD = 0.0; + + InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; + InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; + SensorDirectionValue cancoderInvert = + SensorDirectionValue.CounterClockwise_Positive; + return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, + angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); + } + + /** West Coast Products - SwerveX Standard (Kraken X60) */ + public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** (396 / 35) : 1 */ + double angleGearRatio = ((396.0 / 35.0) / 1.0); + + double angleKP = 1.0; + double angleKI = 0.0; + double angleKD = 0.0; + + InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; + InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; + SensorDirectionValue cancoderInvert = + SensorDirectionValue.CounterClockwise_Positive; + return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, + angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); + } + + public static final class driveRatios { + /** WCP SwerveX Standard X1 - 10 Tooth - (7.85 : 1) */ + public static final double X1_10 = (7.85 / 1.0); + + /** WCP SwerveX Standard X1 - 11 Tooth - (7.13 : 1) */ + public static final double X1_11 = (7.13 / 1.0); + + /** WCP SwerveX Standard X1 - 12 Tooth - (6.54 : 1) */ + public static final double X1_12 = (6.54 / 1.0); + + /** WCP SwerveX Standard X2 - 10 Tooth - (6.56 : 1) */ + public static final double X2_10 = (6.56 / 1.0); + + /** WCP SwerveX Standard X2 - 11 Tooth - (5.96 : 1) */ + public static final double X2_11 = (5.96 / 1.0); + + /** WCP SwerveX Standard X2 - 12 Tooth - (5.46 : 1) */ + public static final double X2_12 = (5.46 / 1.0); + + /** WCP SwerveX Standard X3 - 12 Tooth - (5.14 : 1) */ + public static final double X3_12 = (5.14 / 1.0); + + /** WCP SwerveX Standard X3 - 13 Tooth - (4.75 : 1) */ + public static final double X3_13 = (4.75 / 1.0); + + /** WCP SwerveX Standard X3 - 14 Tooth - (4.41 : 1) */ + public static final double X3_14 = (4.41 / 1.0); + } + } + + /** West Coast Products - SwerveX Flipped */ + public static final class SwerveXFlipped { + /** West Coast Products - SwerveX Flipped (Falcon 500) */ + public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** (468 / 35) : 1 */ + double angleGearRatio = ((468.0 / 35.0) / 1.0); + + double angleKP = 1.0; + double angleKI = 0.0; + double angleKD = 0.0; + + InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; + InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; + SensorDirectionValue cancoderInvert = + SensorDirectionValue.CounterClockwise_Positive; + return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, + angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); + } + + /** West Coast Products - SwerveX Flipped (Kraken X60) */ + public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** (468 / 35) : 1 */ + double angleGearRatio = ((468.0 / 35.0) / 1.0); + + double angleKP = 1.0; + double angleKI = 0.0; + double angleKD = 0.0; + + InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; + InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; + SensorDirectionValue cancoderInvert = + SensorDirectionValue.CounterClockwise_Positive; + return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, + angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); + } + + public static final class driveRatios { + /** WCP SwerveX Flipped X1 - 10 Tooth - (8.10 : 1) */ + public static final double X1_10 = (8.10 / 1.0); + + /** WCP SwerveX Flipped X1 - 11 Tooth - (7.36 : 1) */ + public static final double X1_11 = (7.36 / 1.0); + + /** WCP SwerveX Flipped X1 - 12 Tooth - (6.75 : 1) */ + public static final double X1_12 = (6.75 / 1.0); + + /** WCP SwerveX Flipped X2 - 10 Tooth - (6.72 : 1) */ + public static final double X2_10 = (6.72 / 1.0); + + /** WCP SwerveX Flipped X2 - 11 Tooth - (6.11 : 1) */ + public static final double X2_11 = (6.11 / 1.0); + + /** WCP SwerveX Flipped X2 - 12 Tooth - (5.60 : 1) */ + public static final double X2_12 = (5.60 / 1.0); + + /** WCP SwerveX Flipped X3 - 10 Tooth - (5.51 : 1) */ + public static final double X3_10 = (5.51 / 1.0); + + /** WCP SwerveX Flipped X3 - 11 Tooth - (5.01 : 1) */ + public static final double X3_11 = (5.01 / 1.0); + + /** WCP SwerveX Flipped X3 - 12 Tooth - (4.59 : 1) */ + public static final double X3_12 = (4.59 / 1.0); + } + } + } + + /** Swerve Drive Specialities */ + public static final class SDS { + /** Swerve Drive Specialties - MK3 Module */ + public static final class MK3 { + /** Swerve Drive Specialties - MK3 Module (Falcon 500) */ + public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** 12.8 : 1 */ + double angleGearRatio = (12.8 / 1.0); + + double angleKP = 1.0; + double angleKI = 0.0; + double angleKD = 0.0; + + InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; + InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; + SensorDirectionValue cancoderInvert = + SensorDirectionValue.CounterClockwise_Positive; + return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, + angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); + } + + /** Swerve Drive Specialties - MK3 Module (Kraken X60) */ + public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** 12.8 : 1 */ + double angleGearRatio = (12.8 / 1.0); + + double angleKP = 1.0; + double angleKI = 0.0; + double angleKD = 0.0; + + InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; + InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; + SensorDirectionValue cancoderInvert = + SensorDirectionValue.CounterClockwise_Positive; + return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, + angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); + } + + public static final class driveRatios { + /** SDS MK3 - (8.16 : 1) */ + public static final double Standard = (8.16 / 1.0); + /** SDS MK3 - (6.86 : 1) */ + public static final double Fast = (6.86 / 1.0); + } + } + + /** Swerve Drive Specialties - MK4 Module */ + public static final class MK4 { + /** Swerve Drive Specialties - MK4 Module (Falcon 500) */ + public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** 12.8 : 1 */ + double angleGearRatio = (12.8 / 1.0); + + double angleKP = 1.0; + double angleKI = 0.0; + double angleKD = 0.0; + + InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; + InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; + SensorDirectionValue cancoderInvert = + SensorDirectionValue.CounterClockwise_Positive; + return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, + angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); + } + + /** Swerve Drive Specialties - MK4 Module (Kraken X60) */ + public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** 12.8 : 1 */ + double angleGearRatio = (12.8 / 1.0); + + double angleKP = 1.0; + double angleKI = 0.0; + double angleKD = 0.0; + + InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; + InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; + SensorDirectionValue cancoderInvert = + SensorDirectionValue.CounterClockwise_Positive; + return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, + angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); + } + + public static final class driveRatios { + /** SDS MK4 - (8.14 : 1) */ + public static final double L1 = (8.14 / 1.0); + /** SDS MK4 - (6.75 : 1) */ + public static final double L2 = (6.75 / 1.0); + /** SDS MK4 - (6.12 : 1) */ + public static final double L3 = (6.12 / 1.0); + /** SDS MK4 - (5.14 : 1) */ + public static final double L4 = (5.14 / 1.0); + } + } + + /** Swerve Drive Specialties - MK4i Module */ + public static final class MK4i { + /** Swerve Drive Specialties - MK4i Module (Falcon 500) */ + public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** (150 / 7) : 1 */ + double angleGearRatio = ((150.0 / 7.0) / 1.0); + + double angleKP = 100.0; + double angleKI = 0.0; + double angleKD = 0.0; + + InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; + InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; + SensorDirectionValue cancoderInvert = + SensorDirectionValue.CounterClockwise_Positive; + return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, + angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); + } + + /** Swerve Drive Specialties - MK4i Module (Kraken X60) */ + public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** (150 / 7) : 1 */ + double angleGearRatio = ((150.0 / 7.0) / 1.0); + + double angleKP = 1.0; + double angleKI = 0.0; + double angleKD = 0.0; + + InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; + InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; + SensorDirectionValue cancoderInvert = + SensorDirectionValue.CounterClockwise_Positive; + return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, + angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); + } + + public static final class driveRatios { + /** SDS MK4i - (8.14 : 1) */ + public static final double L1 = (8.14 / 1.0); + /** SDS MK4i - (6.75 : 1) */ + public static final double L2 = (6.75 / 1.0); + /** SDS MK4i - (6.12 : 1) */ + public static final double L3 = (6.12 / 1.0); + } + } + } +} + diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index 199f645..310bebb 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -1,181 +1,94 @@ package frc.lib.util.swerve; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.DemandType; -import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.lib.math.Conversions; -import frc.lib.util.ctre.CTREModuleState; import frc.robot.Constants; import frc.robot.Robot; -/** - * Base Swerve Module Class. Creates an instance of the swerve module - */ public class SwerveModule { public int moduleNumber; - private double angleOffset; - private TalonFX angleMotor; - private TalonFX driveMotor; - private CANCoder angleEncoder; - private double lastAngle; - SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.Swerve.driveKS, - Constants.Swerve.driveKV, Constants.Swerve.driveKA); - - /** - * Creates an instance of a Swerve Module - * - * @param moduleNumber Swerve Module ID. Must be unique - * @param constants Constants specific to the swerve module - */ - public SwerveModule(int moduleNumber, frc.lib.util.swerve.SwerveModuleConstants constants) { + private Rotation2d angleOffset; + + private TalonFX mAngleMotor; + private TalonFX mDriveMotor; + private CANcoder angleEncoder; + + private final SimpleMotorFeedforward driveFeedForward = new SimpleMotorFeedforward( + Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA); + + /* drive motor control requests */ + private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0); + private final VelocityVoltage driveVelocity = new VelocityVoltage(0); + + /* angle motor control requests */ + private final PositionVoltage anglePosition = new PositionVoltage(0); + + public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants) { this.moduleNumber = moduleNumber; - angleOffset = constants.angleOffset; + this.angleOffset = moduleConstants.angleOffset; /* Angle Encoder Config */ - angleEncoder = new CANCoder(constants.cancoderID); - configAngleEncoder(); + angleEncoder = new CANcoder(moduleConstants.cancoderID); + angleEncoder.getConfigurator().apply(Robot.ctreConfigs.swerveCANcoderConfig); /* Angle Motor Config */ - angleMotor = new TalonFX(constants.angleMotorID); - configAngleMotor(); + mAngleMotor = new TalonFX(moduleConstants.angleMotorID); + mAngleMotor.getConfigurator().apply(Robot.ctreConfigs.swerveAngleFXConfig); + resetToAbsolute(); /* Drive Motor Config */ - driveMotor = new TalonFX(constants.driveMotorID); - configDriveMotor(); - - - - lastAngle = getState().angle.getDegrees(); + mDriveMotor = new TalonFX(moduleConstants.driveMotorID); + mDriveMotor.getConfigurator().apply(Robot.ctreConfigs.swerveDriveFXConfig); + mDriveMotor.getConfigurator().setPosition(0.0); } - /** - * Sets the desired state for the swerve module for speed and angle - * - * @param desiredState The desired state (speed and angle) - * @param isOpenLoop Whether to use open or closed loop formula - */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { - desiredState = CTREModuleState.optimize(desiredState, getState().angle); - // Custom optimize - // command, since - // default WPILib - // optimize assumes - // continuous - // controller which - // CTRE is - // not + desiredState = SwerveModuleState.optimize(desiredState, getState().angle); + mAngleMotor.setControl(anglePosition.withPosition(desiredState.angle.getRotations())); + setSpeed(desiredState, isOpenLoop); + } + private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { if (isOpenLoop) { - double percentOutput = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed; - driveMotor.set(ControlMode.PercentOutput, percentOutput); + driveDutyCycle.Output = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed; + mDriveMotor.setControl(driveDutyCycle); } else { - double velocity = Conversions.mpsToFalcon(desiredState.speedMetersPerSecond, - Constants.Swerve.wheelCircumference, Constants.Swerve.driveGearRatio); - driveMotor.set(ControlMode.Velocity, velocity, DemandType.ArbitraryFeedForward, - feedforward.calculate(desiredState.speedMetersPerSecond)); + driveVelocity.Velocity = Conversions.MPSToRPS(desiredState.speedMetersPerSecond, + Constants.Swerve.wheelCircumference); + driveVelocity.FeedForward = + driveFeedForward.calculate(desiredState.speedMetersPerSecond); + mDriveMotor.setControl(driveVelocity); } - - double angle = - (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.Swerve.maxSpeed * 0.01)) - ? lastAngle - : desiredState.angle.getDegrees(); // Prevent rotating module if speed is - // less then 1%. Prevents - // Jittering. - angleMotor.set(ControlMode.Position, - Conversions.degreesToFalcon(angle, Constants.Swerve.angleGearRatio)); - lastAngle = angle; - } - - /** - * - */ - private void resetToAbsolute() { - double absolutePosition = Conversions.degreesToFalcon( - getCanCoder().getDegrees() - angleOffset, Constants.Swerve.angleGearRatio); - angleMotor.setSelectedSensorPosition(absolutePosition); } - /** - * - * - * @param rotationSpeed Drive motor speed (-1 <= value <= 1) - */ - public void setTurnAngle(double rotationSpeed) { - double absolutePosition = Conversions.degreesToFalcon( - getCanCoder().getDegrees() - angleOffset, Constants.Swerve.angleGearRatio); - angleMotor.setSelectedSensorPosition(absolutePosition); - driveMotor.set(ControlMode.PercentOutput, rotationSpeed); + public Rotation2d getCANcoder() { + return Rotation2d.fromRotations(angleEncoder.getAbsolutePosition().getValue()); } - /** - * Gets the Swerve module position - * - * @return Swerve module position - */ - public SwerveModulePosition getPosition() { - double position = Conversions.falconToMeters(driveMotor.getSelectedSensorPosition(), - Constants.Swerve.driveGearRatio, Constants.Swerve.wheelCircumference); - - Rotation2d angle = Rotation2d.fromDegrees(Conversions.falconToDegrees( - angleMotor.getSelectedSensorPosition(), Constants.Swerve.angleGearRatio)); - return new SwerveModulePosition(position, angle); - } - - /** - * Configure the Angle motor CANCoder - */ - private void configAngleEncoder() { - angleEncoder.configFactoryDefault(); - angleEncoder.configAllSettings(Robot.ctreConfigs.swerveCanCoderConfig); - } - - /** - * Configure the Angle motor - */ - private void configAngleMotor() { - angleMotor.configFactoryDefault(); - angleMotor.configAllSettings(Robot.ctreConfigs.swerveAngleFXConfig); - angleMotor.setInverted(Constants.Swerve.angleMotorInvert); - angleMotor.setNeutralMode(Constants.Swerve.angleNeutralMode); - resetToAbsolute(); + public void resetToAbsolute() { + double absolutePosition = getCANcoder().getRotations() - angleOffset.getRotations(); + mAngleMotor.setPosition(absolutePosition); } - /** - * Configure the Drive motor - */ - private void configDriveMotor() { - driveMotor.configFactoryDefault(); - driveMotor.configAllSettings(Robot.ctreConfigs.swerveDriveFXConfig); - driveMotor.setInverted(Constants.Swerve.driveMotorInvert); - driveMotor.setNeutralMode(Constants.Swerve.driveNeutralMode); - driveMotor.setSelectedSensorPosition(0); - } - - /** - * Get the 2d rotation of the module - * - * @return 2d rotation of the module - */ - public Rotation2d getCanCoder() { - return Rotation2d.fromDegrees(angleEncoder.getAbsolutePosition()); - } - - /** - * Gets the Swerve module state - * - * @return Swerve module state - */ public SwerveModuleState getState() { - double velocity = Conversions.falconToMPS(driveMotor.getSelectedSensorVelocity(), - Constants.Swerve.wheelCircumference, Constants.Swerve.driveGearRatio); - Rotation2d angle = Rotation2d.fromDegrees(Conversions.falconToDegrees( - angleMotor.getSelectedSensorPosition(), Constants.Swerve.angleGearRatio)); - return new SwerveModuleState(velocity, angle); + return new SwerveModuleState( + Conversions.RPSToMPS(mDriveMotor.getVelocity().getValue(), + Constants.Swerve.wheelCircumference), + Rotation2d.fromRotations(mAngleMotor.getPosition().getValue())); } + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + Conversions.rotationsToMeters(mDriveMotor.getPosition().getValue(), + Constants.Swerve.wheelCircumference), + Rotation2d.fromRotations(mAngleMotor.getPosition().getValue())); + } } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleConstants.java b/src/main/java/frc/lib/util/swerve/SwerveModuleConstants.java index d6402e4..77ac8b4 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleConstants.java @@ -1,24 +1,23 @@ package frc.lib.util.swerve; -/** - * Constants file used when creating swerve modules - */ +import edu.wpi.first.math.geometry.Rotation2d; + public class SwerveModuleConstants { public final int driveMotorID; public final int angleMotorID; public final int cancoderID; - public final double angleOffset; + public final Rotation2d angleOffset; /** * Swerve Module Constants to be used when creating swerve modules. - * - * @param driveMotorID ID of the drive motor - * @param angleMotorID ID of the angle motor - * @param canCoderID ID of the canCoder - * @param angleOffset offset of the canCoder angle + * + * @param driveMotorID + * @param angleMotorID + * @param canCoderID + * @param angleOffset */ public SwerveModuleConstants(int driveMotorID, int angleMotorID, int canCoderID, - double angleOffset) { + Rotation2d angleOffset) { this.driveMotorID = driveMotorID; this.angleMotorID = angleMotorID; this.cancoderID = canCoderID; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7db5350..9f53381 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,185 +1,161 @@ package frc.robot; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import frc.lib.util.swerve.COTSTalonFXSwerveConstants; import frc.lib.util.swerve.SwerveModuleConstants; -/** - * Constants file. - */ - public final class Constants { public static final double stickDeadband = 0.1; - /** - * PID constants for Swerve Auto Holonomic Drive Controller - */ - public static class SwerveTransformPID { - public static final double pidXkP = 1.5; - public static final double pidXkI = 0.0; - public static final double pidXkD = 0.0; - public static final double pidYkP = 1.5; - public static final double pidYkI = 0.0; - public static final double pidYkD = 0.0; - public static final double pidTkP = 3.0; - public static final double pidTkI = 0.0; - public static final double pidTkD = 0.0; - - public static final double maxAngularVelocity = 3.0; - public static final double maxAngularAcceleration = 3.0; - public static final double stdDevMod = 2.0; - } - - /** - * Camera offset constants - */ - public static class CameraConstants { - - public static final double pitch = 0 * Math.PI / 180; - public static final double roll = Math.PI; - public static final double yaw = 0.0; - public static final Transform3d kCameraToRobot = - new Transform3d(new Translation3d(Units.inchesToMeters(-10), Units.inchesToMeters(2), - Units.inchesToMeters(7.5)), new Rotation3d(roll, pitch, yaw)); - public static final String cameraName = "pv2"; - public static final double largestDistance = 0.1; - } - - /** - * Swerve ID's - */ public static final class Swerve { public static final edu.wpi.first.wpilibj.SPI.Port navXID = edu.wpi.first.wpilibj.SPI.Port.kMXP; - public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW- - - /* Drivetrain Constants */ - public static final double trackWidth = Units.inchesToMeters(14); - public static final double wheelBase = Units.inchesToMeters(14); - public static final double wheelDiameter = Units.inchesToMeters(4); - public static final double wheelCircumference = wheelDiameter * Math.PI; - public static final boolean isFieldRelative = true; public static final boolean isOpenLoop = false; - public static final double openLoopRamp = 0.25; - public static final double closedLoopRamp = 0.4; - - public static final double driveGearRatio = (8.14 / 1.0); // 8.14:1 - public static final double angleGearRatio = (12.8 / 1.0); // 12.8:1 + public static final COTSTalonFXSwerveConstants chosenModule = // TODO: This must be tuned to + // specific robot + COTSTalonFXSwerveConstants.SDS.MK4i + .Falcon500(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L2); + /* Drivetrain Constants */ + public static final double trackWidth = Units.inchesToMeters(21.73); // TODO: This must be + // tuned to specific + // robot + public static final double wheelBase = Units.inchesToMeters(21.73); // TODO: This must be + // tuned to specific + // robot + public static final double wheelCircumference = chosenModule.wheelCircumference; + + /* + * Swerve Kinematics No need to ever change this unless you are not doing a traditional + * rectangular/square 4 module swerve + */ public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics(new Translation2d(wheelBase / 2.0, trackWidth / 2.0), new Translation2d(wheelBase / 2.0, -trackWidth / 2.0), new Translation2d(-wheelBase / 2.0, trackWidth / 2.0), new Translation2d(-wheelBase / 2.0, -trackWidth / 2.0)); + /* Module Gear Ratios */ + public static final double driveGearRatio = chosenModule.driveGearRatio; + public static final double angleGearRatio = chosenModule.angleGearRatio; + + /* Motor Inverts */ + public static final InvertedValue angleMotorInvert = chosenModule.angleMotorInvert; + public static final InvertedValue driveMotorInvert = chosenModule.driveMotorInvert; + + /* Angle Encoder Invert */ + public static final SensorDirectionValue cancoderInvert = chosenModule.cancoderInvert; + /* Swerve Current Limiting */ - public static final int angleContinuousCurrentLimit = 25; - public static final int anglePeakCurrentLimit = 40; - public static final double anglePeakCurrentDuration = 0.1; + public static final int angleCurrentLimit = 25; + public static final int angleCurrentThreshold = 40; + public static final double angleCurrentThresholdTime = 0.1; public static final boolean angleEnableCurrentLimit = true; - public static final int driveContinuousCurrentLimit = 35; - public static final int drivePeakCurrentLimit = 60; - public static final double drivePeakCurrentDuration = 0.1; + public static final int driveCurrentLimit = 35; + public static final int driveCurrentThreshold = 60; + public static final double driveCurrentThresholdTime = 0.1; public static final boolean driveEnableCurrentLimit = true; + /* + * These values are used by the drive falcon to ramp in open loop and closed loop driving. + * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc + */ + public static final double openLoopRamp = 0.25; + public static final double closedLoopRamp = 0.0; + /* Angle Motor PID Values */ - public static final double angleKP = 0.6; - public static final double angleKI = 0.0; - public static final double angleKD = 12.0; - public static final double angleKF = 0.0; + public static final double angleKP = chosenModule.angleKP; + public static final double angleKI = chosenModule.angleKI; + public static final double angleKD = chosenModule.angleKD; /* Drive Motor PID Values */ - public static final double driveKP = 0.10; + public static final double driveKP = 0.12; // TODO: This must be tuned to specific robot public static final double driveKI = 0.0; public static final double driveKD = 0.0; public static final double driveKF = 0.0; - /* Drive Motor Characterization Values */ - public static final double driveKS = (0.667 / 12); - // divide by 12 to convert from volts to percent output for CTRE - public static final double driveKV = (2.44 / 12); - public static final double driveKA = (0.27 / 12); + /* Drive Motor Characterization Values From SYSID */ + public static final double driveKS = 0.32; // TODO: This must be tuned to specific robot + public static final double driveKV = 1.51; + public static final double driveKA = 0.27; /* Swerve Profiling Values */ - public static final double maxSpeed = 4; // meters per second - public static final double maxAngularVelocity = 2; + /** Meters per Second */ + public static final double maxSpeed = 4.5; // TODO: This must be tuned to specific robot + /** Radians per Second */ + public static final double maxAngularVelocity = 10.0; // TODO: This must be tuned to + // specific robot /* Neutral Modes */ - public static final NeutralMode angleNeutralMode = NeutralMode.Coast; - public static final NeutralMode driveNeutralMode = NeutralMode.Brake; - - /* Motor Inverts */ - public static final boolean driveMotorInvert = false; - public static final boolean angleMotorInvert = false; - - /* Angle Encoder Invert */ - public static final boolean canCoderInvert = false; + public static final NeutralModeValue angleNeutralMode = NeutralModeValue.Coast; + public static final NeutralModeValue driveNeutralMode = NeutralModeValue.Brake; /* Module Specific Constants */ - /** - * Front Left Module - Module 0. - */ - public static final class Mod0 { - public static final int driveMotorID = 50; + /* Front Left Module - Module 0 */ + public static final class Mod0 { // TODO: This must be tuned to specific robot + public static final int driveMotorID = 10; public static final int angleMotorID = 8; - public static final int canCoderID = 3; - public static final double angleOffset = 139.043; + public static final int canCoderID = 10; + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(115.400390625); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } - /** - * Front Right Module - Module 1. PROBLEM CHILD - */ - public static final class Mod1 { - public static final int driveMotorID = 51; - public static final int angleMotorID = 52; - public static final int canCoderID = 2; - public static final double angleOffset = 118.564; + /* Front Right Module - Module 1 */ + public static final class Mod1 { // TODO: This must be tuned to specific robot + public static final int driveMotorID = 3; + public static final int angleMotorID = 9; + public static final int canCoderID = 1; + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-55.37109375); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } - /** - * Back Left Module - Module 2. - */ - public static final class Mod2 { - public static final int driveMotorID = 4; - public static final int angleMotorID = 2; - public static final int canCoderID = 1; - public static final double angleOffset = 259.805; + /* Back Left Module - Module 2 */ + public static final class Mod2 { // TODO: This must be tuned to specific robot + public static final int driveMotorID = 2; + public static final int angleMotorID = 40; + public static final int canCoderID = 2; + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(145.01953125); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } - /** - * Back Right Module - Module 3. - */ - public static final class Mod3 { - public static final int driveMotorID = 3; - public static final int angleMotorID = 53; + /* Back Right Module - Module 3 */ + public static final class Mod3 { // TODO: This must be tuned to specific robot + public static final int driveMotorID = 6; + public static final int angleMotorID = 51; public static final int canCoderID = 4; - public static final double angleOffset = 31.816; + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(3.955078125); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } - } - - - /** - * Autonomous constants for swerve bot. - */ - public static final class AutoConstants { - + public static final class AutoConstants { // TODO: The below constants are used in the example + // auto, and must be tuned to specific robot + public static final double kMaxSpeedMetersPerSecond = 3; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + + public static final double kPXController = 1; + public static final double kPYController = 1; + public static final double kPThetaController = 1; + + /* Constraint for the motion profilied robot angle controller */ + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = + new TrapezoidProfile.Constraints(kMaxAngularSpeedRadiansPerSecond, + kMaxAngularSpeedRadiansPerSecondSquared); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5dadbcd..6b942b1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,9 +4,6 @@ package frc.robot; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -15,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.TeleopSwerve; -import frc.robot.commands.TestTransform; import frc.robot.subsystems.Swerve; /** @@ -56,8 +52,6 @@ public void periodic() { private void configureButtonBindings() { /* Driver Buttons */ driver.y().whileTrue(new InstantCommand(() -> s_Swerve.resetFieldRelativeOffset())); - driver.x().whileTrue(new TestTransform(s_Swerve, - new Transform2d(new Translation2d(1, 0), Rotation2d.fromDegrees(180)), 7)); driver.a().whileTrue(new InstantCommand(() -> s_Swerve.resetInitialized())); } diff --git a/src/main/java/frc/robot/commands/TestTransform.java b/src/main/java/frc/robot/commands/TestTransform.java deleted file mode 100644 index 13552d4..0000000 --- a/src/main/java/frc/robot/commands/TestTransform.java +++ /dev/null @@ -1,65 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.math.controller.HolonomicDriveController; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.Command; -import frc.lib.util.FieldConstants; -import frc.robot.Constants; -import frc.robot.subsystems.Swerve; - -/** - * Test April tag transform - */ -public class TestTransform extends Command { - - private Swerve swerve; - private Transform2d transform2d; - - HolonomicDriveController holonomicDriveController = new HolonomicDriveController( - new PIDController(Constants.SwerveTransformPID.pidXkP, Constants.SwerveTransformPID.pidXkI, - Constants.SwerveTransformPID.pidXkD), - new PIDController(Constants.SwerveTransformPID.pidYkP, Constants.SwerveTransformPID.pidYkI, - Constants.SwerveTransformPID.pidYkD), - new ProfiledPIDController(Constants.SwerveTransformPID.pidTkP, - Constants.SwerveTransformPID.pidTkI, Constants.SwerveTransformPID.pidTkD, - new TrapezoidProfile.Constraints(Constants.SwerveTransformPID.maxAngularVelocity, - Constants.SwerveTransformPID.maxAngularAcceleration))); - Pose2d pose2d = new Pose2d(); - private int aprilTagId; - - /** - * Test April tag transform - */ - public TestTransform(Swerve swerve, Transform2d transform2d, int aprilTagId) { - - super(); - this.swerve = swerve; - this.transform2d = transform2d; - this.addRequirements(swerve); - this.aprilTagId = aprilTagId; - holonomicDriveController.setTolerance(new Pose2d(.01, .01, Rotation2d.fromDegrees(1))); - } - - @Override - public void initialize() { - pose2d = FieldConstants.aprilTags.get(aprilTagId).toPose2d().plus(transform2d); - } - - @Override - public void execute() { - ChassisSpeeds ctrlEffort = - holonomicDriveController.calculate(swerve.getPose(), pose2d, 0, pose2d.getRotation()); - swerve.setModuleStates(ctrlEffort); - } - - @Override - public boolean isFinished() { - return holonomicDriveController.atReference(); - } -} diff --git a/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java b/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java index 1082587..7eb4b52 100644 --- a/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java +++ b/src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java @@ -3,7 +3,6 @@ import edu.wpi.first.util.sendable.SendableRegistry; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; import frc.robot.subsystems.Swerve; /** @@ -27,7 +26,7 @@ public ZeroMotorsWaitCommand(double seconds) { @Override public void initialize() { - s_Swerve.setMotorsZero(Constants.Swerve.isOpenLoop, Constants.Swerve.isFieldRelative); + // s_Swerve.setMotorsZero(Constants.Swerve.isOpenLoop, Constants.Swerve.isFieldRelative); m_timer.reset(); m_timer.start(); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 4a31219..18b8955 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,265 +1,128 @@ package frc.robot.subsystems; -import java.util.ArrayList; -import org.photonvision.PhotonCamera; import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.lib.util.FieldConstants; import frc.lib.util.swerve.SwerveModule; import frc.robot.Constants; -/** - * Creates swerve drive and commands for drive. - */ public class Swerve extends SubsystemBase { + public SwerveDriveOdometry swerveOdometry; + public SwerveModule[] mSwerveMods; public AHRS gyro = new AHRS(Constants.Swerve.navXID); - public SwerveDrivePoseEstimator swerveOdometry; - private PhotonCamera cam = new PhotonCamera(Constants.CameraConstants.cameraName); - public SwerveModule[] swerveMods; private double fieldOffset = gyro.getYaw(); - ChassisSpeeds chassisSpeeds; - private final Field2d field = new Field2d(); private boolean hasInitialized = false; - /** - * Initializes swerve modules. - */ public Swerve() { - SmartDashboard.putData("Field Pos", field); - - - swerveMods = new SwerveModule[] {new SwerveModule(0, Constants.Swerve.Mod0.constants), + mSwerveMods = new SwerveModule[] {new SwerveModule(0, Constants.Swerve.Mod0.constants), new SwerveModule(1, Constants.Swerve.Mod1.constants), new SwerveModule(2, Constants.Swerve.Mod2.constants), new SwerveModule(3, Constants.Swerve.Mod3.constants)}; - swerveOdometry = new SwerveDrivePoseEstimator(Constants.Swerve.swerveKinematics, getYaw(), - getPositions(), new Pose2d()); + swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getGyroYaw(), + getModulePositions()); } - /** - * New command to set wheels inward. - */ - public void wheelsIn() { - swerveMods[0].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(45)), false); - swerveMods[1].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(135)), false); - swerveMods[2].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(-45)), false); - swerveMods[3].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(-135)), - false); - this.setMotorsZero(Constants.Swerve.isOpenLoop, Constants.Swerve.isFieldRelative); - } - - /** - * Moves the swerve drive train - * - * @param translation The 2d translation in the X-Y plane - * @param rotation The amount of rotation in the Z axis - * @param fieldRelative Whether the movement is relative to the field or absolute - * @param isOpenLoop Open or closed loop system - */ public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { - ChassisSpeeds chassisSpeeds = fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), - rotation, Rotation2d.fromDegrees(getYaw().getDegrees() - fieldOffset)) - : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); - setModuleStates(chassisSpeeds); - } - - /** - * Sets motors to 0 or inactive. - * - * @param isOpenLoop Open or closed loop system - * @param fieldRelative Whether the movement is relative to the field or absolute - */ - public void setMotorsZero(boolean isOpenLoop, boolean fieldRelative) { - System.out.println("Setting Zero!!!!!!"); - setModuleStates(new ChassisSpeeds(0, 0, 0)); + SwerveModuleState[] swerveModuleStates = + Constants.Swerve.swerveKinematics.toSwerveModuleStates(fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), + rotation, Rotation2d.fromDegrees(gyro.getYaw() - fieldOffset)) + : new ChassisSpeeds(translation.getX(), translation.getY(), rotation)); + SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Constants.Swerve.maxSpeed); + + for (SwerveModule mod : mSwerveMods) { + mod.setDesiredState(swerveModuleStates[mod.moduleNumber], isOpenLoop); + } } - /** - * Used by SwerveControllerCommand in Auto - * - * @param desiredStates The desired states of the swerve modules - */ + /* Used by SwerveControllerCommand in Auto */ public void setModuleStates(SwerveModuleState[] desiredStates) { SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.Swerve.maxSpeed); - for (SwerveModule mod : swerveMods) { + for (SwerveModule mod : mSwerveMods) { mod.setDesiredState(desiredStates[mod.moduleNumber], false); } } - /** - * Sets swerve module states using Chassis Speeds. - * - * @param chassisSpeeds The desired Chassis Speeds - */ - public void setModuleStates(ChassisSpeeds chassisSpeeds) { - SwerveModuleState[] swerveModuleStates = - Constants.Swerve.swerveKinematics.toSwerveModuleStates(chassisSpeeds); - setModuleStates(swerveModuleStates); + public SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (SwerveModule mod : mSwerveMods) { + states[mod.moduleNumber] = mod.getState(); + } + return states; + } + + public SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (SwerveModule mod : mSwerveMods) { + positions[mod.moduleNumber] = mod.getPosition(); + } + return positions; } - /** - * Returns the position of the robot on the field. - * - * @return The pose of the robot (x and y are in meters). - */ public Pose2d getPose() { - return swerveOdometry.getEstimatedPosition(); + return swerveOdometry.getPoseMeters(); } - /** - * Resets the robot's position on the field. - * - * @param pose The position on the field that your robot is at. - */ - public void resetOdometry(Pose2d pose) { - swerveOdometry.resetPosition(getYaw(), getPositions(), pose); + public void setPose(Pose2d pose) { + swerveOdometry.resetPosition(getGyroYaw(), getModulePositions(), pose); } - /** - * Gets the states of each swerve module. - * - * @return Swerve module state - */ - public SwerveModuleState[] getStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; - for (SwerveModule mod : swerveMods) { - states[mod.moduleNumber] = mod.getState(); - } - return states; + public Rotation2d getHeading() { + return getPose().getRotation(); + } + + public void setHeading(Rotation2d heading) { + swerveOdometry.resetPosition(getGyroYaw(), getModulePositions(), + new Pose2d(getPose().getTranslation(), heading)); + } + + public void zeroHeading() { + swerveOdometry.resetPosition(getGyroYaw(), getModulePositions(), + new Pose2d(getPose().getTranslation(), new Rotation2d())); } + public Rotation2d getGyroYaw() { + return Rotation2d.fromDegrees(gyro.getYaw()); + } + + /** * Resets the gyro field relative driving offset */ public void resetFieldRelativeOffset() { // gyro.zeroYaw(); - fieldOffset = getYaw().getDegrees(); + fieldOffset = getGyroYaw().getDegrees(); } - /** - * Gets the rotation degree from swerve modules. - */ - public Rotation2d getYaw() { - float yaw = gyro.getYaw(); - return (Constants.Swerve.invertGyro) ? Rotation2d.fromDegrees(-yaw) - : Rotation2d.fromDegrees(yaw); - } - - public String getStringYaw() { - float yaw = gyro.getYaw(); - return (Constants.Swerve.invertGyro) ? "Yaw: " + (360 - yaw) : "Yaw: " + yaw; + public void resetModulesToAbsolute() { + for (SwerveModule mod : mSwerveMods) { + mod.resetToAbsolute(); + } } @Override public void periodic() { - Rotation2d yaw = getYaw(); - swerveOdometry.update(yaw, getPositions()); - var res = cam.getLatestResult(); - if (res.hasTargets()) { - var imageCaptureTime = res.getTimestampSeconds(); - if (!hasInitialized) { - var target = res.getBestTarget(); - var camToTargetTrans = target.getBestCameraToTarget(); - var aprilTagPose = FieldConstants.aprilTags.get(target.getFiducialId()); - if (aprilTagPose != null) { - var camPose = aprilTagPose.transformBy(camToTargetTrans.inverse()); - var robotPose = - camPose.transformBy(Constants.CameraConstants.kCameraToRobot).toPose2d(); - swerveOdometry.resetPosition(getYaw(), getPositions(), robotPose); - SmartDashboard.putNumberArray("Initial Position", - new double[] {robotPose.getX(), robotPose.getY()}); - hasInitialized = true; - } - - } - var pose2dList = new ArrayList(); - var target = res.getBestTarget(); - // for (var Target : res.targets) { + swerveOdometry.update(getGyroYaw(), getModulePositions()); - var camToTargetTrans = target.getBestCameraToTarget(); - var aprilTagPose = FieldConstants.aprilTags.get(target.getFiducialId()); - if (aprilTagPose != null) { - var camPose = aprilTagPose.transformBy(camToTargetTrans.inverse()); - var robotPose = - camPose.transformBy(Constants.CameraConstants.kCameraToRobot).toPose2d(); - pose2dList.add(robotPose); - // swerveOdometry.resetPosition(getYaw(), getPositions(), robotPose); - if (robotPose.minus(getPose()).getTranslation() - .getNorm() < Constants.CameraConstants.largestDistance) { - swerveOdometry.addVisionMeasurement(robotPose, imageCaptureTime, - VecBuilder.fill(Constants.SwerveTransformPID.stdDevMod / target.getArea(), - Constants.SwerveTransformPID.stdDevMod / target.getArea(), - Constants.SwerveTransformPID.stdDevMod / target.getArea())); - } - } - // } - - - outer: for (int i = 0; i < pose2dList.size(); i++) { - for (int j = i + 1; j < pose2dList.size(); j++) { - var diff = pose2dList.get(i).minus(pose2dList.get(j)); - if (diff.getTranslation() - .getNorm() < Constants.CameraConstants.largestDistance) { - swerveOdometry.resetPosition(getYaw(), getPositions(), pose2dList.get(i)); - break outer; - } - } - } - } - - - field.setRobotPose(swerveOdometry.getEstimatedPosition()); - - SmartDashboard.putBoolean("Has Initialized", hasInitialized); - SmartDashboard.putNumber("Robot X", swerveOdometry.getEstimatedPosition().getX()); - SmartDashboard.putNumber("Robot Y", swerveOdometry.getEstimatedPosition().getY()); - SmartDashboard.putNumber("Robot Rotation", - swerveOdometry.getEstimatedPosition().getRotation().getDegrees()); - SmartDashboard.putNumber("Gyro Yaw", yaw.getDegrees()); - SmartDashboard.putNumber("Field Offset", fieldOffset); - SmartDashboard.putNumber("Gyro Yaw - Offset", yaw.getDegrees() - fieldOffset); - - - for (SwerveModule mod : swerveMods) { - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Cancoder", - mod.getCanCoder().getDegrees()); - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Integrated", - mod.getState().angle.getDegrees()); + for (SwerveModule mod : mSwerveMods) { + SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", + mod.getCANcoder().getDegrees()); + SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", + mod.getPosition().angle.getDegrees()); SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Position", - mod.getPosition().distanceMeters); - } - } - - /** - * Get position of all swerve modules - * - * @return Array of Swerve Module Positions - */ - public SwerveModulePosition[] getPositions() { - SwerveModulePosition[] positions = new SwerveModulePosition[4]; - - for (SwerveModule mod : swerveMods) { - positions[mod.moduleNumber] = mod.getPosition(); - } - return positions; } public void resetInitialized() {