From bb59c528b79864a79737b192cd058ad892d43307 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Tue, 27 Jan 2026 22:17:09 -0500 Subject: [PATCH] Hopeful fixes from the second round of testing --- .../SwerveDrive/SwerveBuilder.java | 67 ++++++++++++------- .../SwerveDrive/SwerveModule.java | 32 ++++++--- 2 files changed, 64 insertions(+), 35 deletions(-) diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java index 66ba603..0eed21a 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java @@ -14,47 +14,50 @@ public class SwerveBuilder { private int pigeonId; - public SwerveBuilder(){ + public SwerveBuilder() { } - public SwerveBuilder withPigeonId(int pigeonId){ + public SwerveBuilder withPigeonId(int pigeonId) { this.pigeonId = pigeonId; return this; } - public SwerveBuilder withCanbusName(String canBusname){ + public SwerveBuilder withCanbusName(String canBusname) { this.canBusName = canBusname; return this; } - public SwerveBuilder withSwerveModule(SwerveModule module){ + public SwerveBuilder withSwerveModule(SwerveModule module) { this.modules.add(module); return this; } /** * Set the max linear velocity in meters per second + * * @param maxVel Max linear velocity in meters per second * @return The swerve builder */ - public SwerveBuilder withMaxLinearVelocity(double maxVel){ + public SwerveBuilder withMaxLinearVelocity(double maxVel) { this.maxLinearVelMetersSecond = maxVel; return this; } /** * Set the max angular velocity in radians per second + * * @param maxVel Max angular velocity in radians per second * @return The swerve builder */ - public SwerveBuilder withMaxAngularVelocity(double maxVel){ + public SwerveBuilder withMaxAngularVelocity(double maxVel) { this.maxAngularVelRadiansSecond = maxVel; return this; } - public SwerveDriveSubsystem build(){ - return new SwerveDriveSubsystem(pigeonId, canBusName, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size])); + public SwerveDriveSubsystem build() { + return new SwerveDriveSubsystem(pigeonId, canBusName, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, + modules.toArray(size -> new SwerveModule[size])); } public class SwerveModuleBuilder { @@ -68,7 +71,9 @@ public class SwerveBuilder { private double steerKP; private double steerKI; private double steerKD; + private double steerKS; private double steerKV; + private double steerKA; private int steerEncoderId; private double steerOffset; private Translation2d moduleLocation; @@ -80,84 +85,96 @@ public class SwerveBuilder { } - public SwerveModuleBuilder withDriveMotorId(int driveMotorId){ + public SwerveModuleBuilder withDriveMotorId(int driveMotorId) { this.driveMotorId = driveMotorId; return this; } - public SwerveModuleBuilder withDriveKP(double driveKP){ + public SwerveModuleBuilder withDriveKP(double driveKP) { this.driveKP = driveKP; return this; } - public SwerveModuleBuilder withDriveKI(double driveKI){ + public SwerveModuleBuilder withDriveKI(double driveKI) { this.driveKI = driveKI; return this; } - public SwerveModuleBuilder withDriveKD(double driveKD){ + public SwerveModuleBuilder withDriveKD(double driveKD) { this.driveKD = driveKD; return this; } - public SwerveModuleBuilder withDriveKV(double driveKV){ + public SwerveModuleBuilder withDriveKV(double driveKV) { this.driveKV = driveKV; return this; } - public SwerveModuleBuilder withSteerMotorId(int steerMotorId){ + public SwerveModuleBuilder withSteerMotorId(int steerMotorId) { this.steerMotorId = steerMotorId; return this; } - public SwerveModuleBuilder withSteerKP(double steerKP){ + public SwerveModuleBuilder withSteerKP(double steerKP) { this.steerKP = steerKP; return this; } - public SwerveModuleBuilder withSteerKI(double steerKI){ + public SwerveModuleBuilder withSteerKI(double steerKI) { this.steerKI = steerKI; return this; } - public SwerveModuleBuilder withSteerKD(double steerKD){ + public SwerveModuleBuilder withSteerKD(double steerKD) { this.steerKD = steerKD; return this; } - public SwerveModuleBuilder withSteerKV(double steerKV){ + public SwerveModuleBuilder withSteerKS(double steerKS) { + this.steerKS = steerKS; + return this; + } + + public SwerveModuleBuilder withSteerKV(double steerKV) { this.steerKV = steerKV; return this; } - public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId){ + public SwerveModuleBuilder withSteerKA(double steerKA) { + this.steerKA = steerKA; + return this; + } + + public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId) { this.steerEncoderId = steerEncoderId; return this; } - public SwerveModuleBuilder withModuleLocation(Translation2d moduleLocation){ + public SwerveModuleBuilder withModuleLocation(Translation2d moduleLocation) { this.moduleLocation = moduleLocation; return this; } - public SwerveModuleBuilder withGearRatio(double gearRatio){ + public SwerveModuleBuilder withGearRatio(double gearRatio) { this.gearRatio = gearRatio; return this; } - public SwerveModuleBuilder withWheelDiameter(double wheelDiameterInMeters){ + public SwerveModuleBuilder withWheelDiameter(double wheelDiameterInMeters) { this.wheelDiameterInMeters = wheelDiameterInMeters; return this; } - public SwerveModuleBuilder withModuleName(String moduleName){ + public SwerveModuleBuilder withModuleName(String moduleName) { this.moduleName = moduleName; return this; } public SwerveModule build() { - return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, steerMotorId, steerKP, steerKI, steerKD, steerKV, - steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters, moduleName); + return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, steerMotorId, steerKP, steerKI, + steerKD, steerKS, steerKV, steerKA, + steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters, + moduleName); } } diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java index 54a54de..f812215 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java @@ -4,8 +4,10 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; @@ -18,7 +20,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.units.measure.Angle; public class SwerveModule { @@ -35,7 +36,7 @@ public class SwerveModule { private TalonFX driveMotor; private VelocityVoltage driveRequest = new VelocityVoltage(0); private TalonFX steerMotor; - private PositionVoltage steerRequest = new PositionVoltage(0); + private MotionMagicVoltage steerRequest = new MotionMagicVoltage(0); private CANcoder steerEncoder; private String moduleName; @@ -52,7 +53,9 @@ public class SwerveModule { double steerKP, double steerKI, double steerKD, + double steerKS, double steerKV, + double steerKA, int steerEncoderId, double steerOffset, String canBusName, @@ -74,7 +77,7 @@ public class SwerveModule { steerMotor = new TalonFX(steerMotorId, canBusName); steerMotor.optimizeBusUtilization(); - steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKV)); + steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKS, steerKV, steerKA)); this.moduleLocation = moduleLocation; @@ -84,17 +87,19 @@ public class SwerveModule { this.moduleName = moduleName; } - public void setSteerRotations(Angle rotations){ - steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations)); + public void setSteerRotations(Rotation2d rotations){ + steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations.getMeasure())); } public Rotation2d getSteerRotations(){ - return new Rotation2d(steerEncoder.getPosition().refresh().getValueAsDouble()); + // Maybe go back to non-Absolute Pos + // Or try reading from the steer motot for the pos + return new Rotation2d(steerEncoder.getAbsolutePosition().getValue()); } public void setDriveSpeed(double speed){ // Convert wheel speed to rotor speed - // Circumfrence of wheel / speed * gear ratio + // (speed / Circumfrence of wheel) * gear ratio double rotorVelocity = (speed / (Math.PI * wheelDiameterInMeters)) * gearRatio; driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity)); } @@ -120,7 +125,7 @@ public class SwerveModule { } public void setModuleState(SwerveModuleState state){ - this.setSteerRotations(state.angle.getMeasure()); + this.setSteerRotations(state.angle); this.setDriveSpeed(state.speedMetersPerSecond); moduleState = state; } @@ -152,14 +157,21 @@ public class SwerveModule { ); } - private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD, double kV){ + private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD, double kS, double kV, double kA){ return new TalonFXConfiguration().withSlot0( new Slot0Configs() .withKP(kP) .withKI(kI) .withKD(kD) - .withKS(kV) + .withKS(kS) + .withKV(kV) + .withKA(kA) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign) + ).withMotionMagic( + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(80) + .withMotionMagicAcceleration(160) + .withMotionMagicJerk(1600) ).withFeedback( new FeedbackConfigs() .withFeedbackRemoteSensorID(steerEncoder.getDeviceID())