FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Hopeful fixes from the second round of testing

This commit is contained in:
Tyler Wilcox 2026-01-27 22:17:09 -05:00
parent ae742134fe
commit bb59c528b7
2 changed files with 64 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java
View File

@ -35,6 +35,7 @@ public class SwerveBuilder {
/** /**
* Set the max linear velocity in meters per second * Set the max linear velocity in meters per second
*
* @param maxVel Max linear velocity in meters per second * @param maxVel Max linear velocity in meters per second
* @return The swerve builder * @return The swerve builder
*/ */
@ -45,6 +46,7 @@ public class SwerveBuilder {
/** /**
* Set the max angular velocity in radians per second * Set the max angular velocity in radians per second
*
* @param maxVel Max angular velocity in radians per second * @param maxVel Max angular velocity in radians per second
* @return The swerve builder * @return The swerve builder
*/ */
@ -54,7 +56,8 @@ public class SwerveBuilder {
} }
public SwerveDriveSubsystem build() { public SwerveDriveSubsystem build() {
return new SwerveDriveSubsystem(pigeonId, canBusName, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size])); return new SwerveDriveSubsystem(pigeonId, canBusName, maxLinearVelMetersSecond, maxAngularVelRadiansSecond,
modules.toArray(size -> new SwerveModule[size]));
} }
public class SwerveModuleBuilder { public class SwerveModuleBuilder {
@ -68,7 +71,9 @@ public class SwerveBuilder {
private double steerKP; private double steerKP;
private double steerKI; private double steerKI;
private double steerKD; private double steerKD;
private double steerKS;
private double steerKV; private double steerKV;
private double steerKA;
private int steerEncoderId; private int steerEncoderId;
private double steerOffset; private double steerOffset;
private Translation2d moduleLocation; private Translation2d moduleLocation;
@ -125,11 +130,21 @@ public class SwerveBuilder {
return this; return this;
} }
public SwerveModuleBuilder withSteerKS(double steerKS) {
this.steerKS = steerKS;
return this;
}
public SwerveModuleBuilder withSteerKV(double steerKV) { public SwerveModuleBuilder withSteerKV(double steerKV) {
this.steerKV = steerKV; this.steerKV = steerKV;
return this; return this;
} }
public SwerveModuleBuilder withSteerKA(double steerKA) {
this.steerKA = steerKA;
return this;
}
public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId) { public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId) {
this.steerEncoderId = steerEncoderId; this.steerEncoderId = steerEncoderId;
return this; return this;
@ -156,8 +171,10 @@ public class SwerveBuilder {
} }
public SwerveModule build() { public SwerveModule build() {
return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, steerMotorId, steerKP, steerKI, steerKD, steerKV, return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, steerMotorId, steerKP, steerKI,
steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters, moduleName); steerKD, steerKS, steerKV, steerKA,
steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters,
moduleName);
} }
} }

32
lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java
View File

@ -4,8 +4,10 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs;
import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.MagnetSensorConfigs;
import com.ctre.phoenix6.configs.MotionMagicConfigs;
import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.CANcoder; 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.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.units.measure.Angle;
public class SwerveModule { public class SwerveModule {
@ -35,7 +36,7 @@ public class SwerveModule {
private TalonFX driveMotor; private TalonFX driveMotor;
private VelocityVoltage driveRequest = new VelocityVoltage(0); private VelocityVoltage driveRequest = new VelocityVoltage(0);
private TalonFX steerMotor; private TalonFX steerMotor;
private PositionVoltage steerRequest = new PositionVoltage(0); private MotionMagicVoltage steerRequest = new MotionMagicVoltage(0);
private CANcoder steerEncoder; private CANcoder steerEncoder;
private String moduleName; private String moduleName;
@ -52,7 +53,9 @@ public class SwerveModule {
double steerKP, double steerKP,
double steerKI, double steerKI,
double steerKD, double steerKD,
double steerKS,
double steerKV, double steerKV,
double steerKA,
int steerEncoderId, int steerEncoderId,
double steerOffset, double steerOffset,
String canBusName, String canBusName,
@ -74,7 +77,7 @@ public class SwerveModule {
steerMotor = new TalonFX(steerMotorId, canBusName); steerMotor = new TalonFX(steerMotorId, canBusName);
steerMotor.optimizeBusUtilization(); steerMotor.optimizeBusUtilization();
steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKV)); steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKS, steerKV, steerKA));
this.moduleLocation = moduleLocation; this.moduleLocation = moduleLocation;
@ -84,17 +87,19 @@ public class SwerveModule {
this.moduleName = moduleName; this.moduleName = moduleName;
} }
public void setSteerRotations(Angle rotations){ public void setSteerRotations(Rotation2d rotations){
steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations)); steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations.getMeasure()));
} }
public Rotation2d getSteerRotations(){ 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){ public void setDriveSpeed(double speed){
// Convert wheel speed to rotor 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; double rotorVelocity = (speed / (Math.PI * wheelDiameterInMeters)) * gearRatio;
driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity)); driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity));
} }
@ -120,7 +125,7 @@ public class SwerveModule {
} }
public void setModuleState(SwerveModuleState state){ public void setModuleState(SwerveModuleState state){
this.setSteerRotations(state.angle.getMeasure()); this.setSteerRotations(state.angle);
this.setDriveSpeed(state.speedMetersPerSecond); this.setDriveSpeed(state.speedMetersPerSecond);
moduleState = state; 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( return new TalonFXConfiguration().withSlot0(
new Slot0Configs() new Slot0Configs()
.withKP(kP) .withKP(kP)
.withKI(kI) .withKI(kI)
.withKD(kD) .withKD(kD)
.withKS(kV) .withKS(kS)
.withKV(kV)
.withKA(kA)
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign)
).withMotionMagic(
new MotionMagicConfigs()
.withMotionMagicCruiseVelocity(80)
.withMotionMagicAcceleration(160)
.withMotionMagicJerk(1600)
).withFeedback( ).withFeedback(
new FeedbackConfigs() new FeedbackConfigs()
.withFeedbackRemoteSensorID(steerEncoder.getDeviceID()) .withFeedbackRemoteSensorID(steerEncoder.getDeviceID())