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

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

@ -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);
}
}

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.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())