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; private int pigeonId;
public SwerveBuilder(){ public SwerveBuilder() {
} }
public SwerveBuilder withPigeonId(int pigeonId){ public SwerveBuilder withPigeonId(int pigeonId) {
this.pigeonId = pigeonId; this.pigeonId = pigeonId;
return this; return this;
} }
public SwerveBuilder withCanbusName(String canBusname){ public SwerveBuilder withCanbusName(String canBusname) {
this.canBusName = canBusname; this.canBusName = canBusname;
return this; return this;
} }
public SwerveBuilder withSwerveModule(SwerveModule module){ public SwerveBuilder withSwerveModule(SwerveModule module) {
this.modules.add(module); this.modules.add(module);
return this; return this;
} }
/** /**
* 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
*/ */
public SwerveBuilder withMaxLinearVelocity(double maxVel){ public SwerveBuilder withMaxLinearVelocity(double maxVel) {
this.maxLinearVelMetersSecond = maxVel; this.maxLinearVelMetersSecond = maxVel;
return this; return this;
} }
/** /**
* 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
*/ */
public SwerveBuilder withMaxAngularVelocity(double maxVel){ public SwerveBuilder withMaxAngularVelocity(double maxVel) {
this.maxAngularVelRadiansSecond = maxVel; this.maxAngularVelRadiansSecond = maxVel;
return this; return this;
} }
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;
@ -80,84 +85,96 @@ public class SwerveBuilder {
} }
public SwerveModuleBuilder withDriveMotorId(int driveMotorId){ public SwerveModuleBuilder withDriveMotorId(int driveMotorId) {
this.driveMotorId = driveMotorId; this.driveMotorId = driveMotorId;
return this; return this;
} }
public SwerveModuleBuilder withDriveKP(double driveKP){ public SwerveModuleBuilder withDriveKP(double driveKP) {
this.driveKP = driveKP; this.driveKP = driveKP;
return this; return this;
} }
public SwerveModuleBuilder withDriveKI(double driveKI){ public SwerveModuleBuilder withDriveKI(double driveKI) {
this.driveKI = driveKI; this.driveKI = driveKI;
return this; return this;
} }
public SwerveModuleBuilder withDriveKD(double driveKD){ public SwerveModuleBuilder withDriveKD(double driveKD) {
this.driveKD = driveKD; this.driveKD = driveKD;
return this; return this;
} }
public SwerveModuleBuilder withDriveKV(double driveKV){ public SwerveModuleBuilder withDriveKV(double driveKV) {
this.driveKV = driveKV; this.driveKV = driveKV;
return this; return this;
} }
public SwerveModuleBuilder withSteerMotorId(int steerMotorId){ public SwerveModuleBuilder withSteerMotorId(int steerMotorId) {
this.steerMotorId = steerMotorId; this.steerMotorId = steerMotorId;
return this; return this;
} }
public SwerveModuleBuilder withSteerKP(double steerKP){ public SwerveModuleBuilder withSteerKP(double steerKP) {
this.steerKP = steerKP; this.steerKP = steerKP;
return this; return this;
} }
public SwerveModuleBuilder withSteerKI(double steerKI){ public SwerveModuleBuilder withSteerKI(double steerKI) {
this.steerKI = steerKI; this.steerKI = steerKI;
return this; return this;
} }
public SwerveModuleBuilder withSteerKD(double steerKD){ public SwerveModuleBuilder withSteerKD(double steerKD) {
this.steerKD = steerKD; this.steerKD = steerKD;
return this; 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; this.steerKV = steerKV;
return this; 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; this.steerEncoderId = steerEncoderId;
return this; return this;
} }
public SwerveModuleBuilder withModuleLocation(Translation2d moduleLocation){ public SwerveModuleBuilder withModuleLocation(Translation2d moduleLocation) {
this.moduleLocation = moduleLocation; this.moduleLocation = moduleLocation;
return this; return this;
} }
public SwerveModuleBuilder withGearRatio(double gearRatio){ public SwerveModuleBuilder withGearRatio(double gearRatio) {
this.gearRatio = gearRatio; this.gearRatio = gearRatio;
return this; return this;
} }
public SwerveModuleBuilder withWheelDiameter(double wheelDiameterInMeters){ public SwerveModuleBuilder withWheelDiameter(double wheelDiameterInMeters) {
this.wheelDiameterInMeters = wheelDiameterInMeters; this.wheelDiameterInMeters = wheelDiameterInMeters;
return this; return this;
} }
public SwerveModuleBuilder withModuleName(String moduleName){ public SwerveModuleBuilder withModuleName(String moduleName) {
this.moduleName = moduleName; this.moduleName = moduleName;
return this; return this;
} }
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())