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

Working but offsets are busted

This commit is contained in:
Tyler Wilcox 2026-01-29 20:53:53 -05:00
parent bb59c528b7
commit f9c7df64ba
3 changed files with 73 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -67,6 +67,7 @@ public class SwerveBuilder {
private double driveKI;
private double driveKD;
private double driveKV;
private boolean driveInverted = false;
private int steerMotorId;
private double steerKP;
private double steerKI;
@ -110,6 +111,11 @@ public class SwerveBuilder {
return this;
}
public SwerveModuleBuilder withDriveInverted(){
this.driveInverted = true;
return this;
}
public SwerveModuleBuilder withSteerMotorId(int steerMotorId) {
this.steerMotorId = steerMotorId;
return this;
@ -150,6 +156,11 @@ public class SwerveBuilder {
return this;
}
public SwerveModuleBuilder withSteerOffset(double offsetRotations) {
this.steerOffset = offsetRotations;
return this;
}
public SwerveModuleBuilder withModuleLocation(Translation2d moduleLocation) {
this.moduleLocation = moduleLocation;
return this;
@ -171,7 +182,7 @@ public class SwerveBuilder {
}
public SwerveModule build() {
return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, steerMotorId, steerKP, steerKI,
return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, driveInverted, steerMotorId, steerKP, steerKI,
steerKD, steerKS, steerKV, steerKA,
steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters,
moduleName);

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

@ -54,16 +54,17 @@ public class SwerveDriveSubsystem extends SubsystemBase{
@Override
public void periodic() {
super.periodic();
SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation);
SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds);
// for(int i=0; i<modules.length;i++){
// moduleStates[i].optimize(modules[i].getSteerRotations());
// }
for(int i=0; i<modules.length;i++){
moduleStates[i].optimize(modules[i].getSteerRotations());
}
poseEstimator.updateWithTime(Utils.getCurrentTimeSeconds(), new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions());
for(int i=0;i<modules.length;i++){
modules[i].setModuleState(moduleStates[i]);
modules[i].setRequestedModuleState(moduleStates[i]);
modules[i].periodic();
}
}
@ -85,8 +86,12 @@ public class SwerveDriveSubsystem extends SubsystemBase{
return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleLocation()).toArray(Translation2d[]::new);
}
public SwerveModuleState[] getModuleStates(){
return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleState()).toArray(SwerveModuleState[]::new);
public SwerveModuleState[] getRequestedModuleStates(){
return Arrays.stream(modules).map((SwerveModule m) -> m.getRequestedModuleState()).toArray(SwerveModuleState[]::new);
}
public SwerveModuleState[] getActualModuleStates(){
return Arrays.stream(modules).map((SwerveModule m) -> m.getActualModuleState()).toArray(SwerveModuleState[]::new);
}
public SwerveModule[] getModules(){
@ -98,11 +103,11 @@ public class SwerveDriveSubsystem extends SubsystemBase{
}
public Angle getCurrentYaw(){
return gyro.getYaw().refresh().getValue();
return gyro.getRoll().getValue();
}
public void setChassisSpeed(ChassisSpeeds chassisSpeeds){
this.setChassisSpeed(chassisSpeeds, Translation2d.kZero);
this.setChassisSpeed(chassisSpeeds, new Translation2d());
}
public void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){

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

@ -1,10 +1,13 @@
package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive;
import java.lang.reflect.Modifier;
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.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
@ -13,6 +16,8 @@ import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
@ -20,23 +25,22 @@ 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.wpilibj.smartdashboard.SmartDashboard;
public class SwerveModule {
private final int CAN_UPDATE_FREQUENCY = 50;
private final int TALON_BUILT_IN_ENCODER_UNITS_PER_ROTATION = 2048;
private final int CANCODER_UNITS_PER_ROTATION = 2048;
private final double gearRatio;
private final double wheelDiameterInMeters;
private SwerveModuleState moduleState;
private SwerveModuleState requestedModuleState;
private TalonFX driveMotor;
private VelocityVoltage driveRequest = new VelocityVoltage(0);
private TalonFX steerMotor;
private MotionMagicVoltage steerRequest = new MotionMagicVoltage(0);
private PositionVoltage steerPosRequ = new PositionVoltage(0);
private CANcoder steerEncoder;
private String moduleName;
@ -49,6 +53,7 @@ public class SwerveModule {
double driveKI,
double driveKD,
double driveKV,
boolean driveInverted,
int steerMotorId,
double steerKP,
double steerKI,
@ -66,18 +71,19 @@ public class SwerveModule {
){
steerEncoder = new CANcoder(steerEncoderId, canBusName);
steerEncoder.getPosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
steerEncoder.optimizeBusUtilization();
// steerEncoder.getAbsolutePosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
// steerEncoder.optimizeBusUtilization();
steerEncoder.getConfigurator().apply(generateCanCoderConfiguration(steerOffset));
driveMotor = new TalonFX(driveMotorId, canBusName);
driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
driveMotor.optimizeBusUtilization();
driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV));
// driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
// driveMotor.optimizeBusUtilization();
driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV, driveInverted));
steerMotor = new TalonFX(steerMotorId, canBusName);
steerMotor.optimizeBusUtilization();
// steerMotor.optimizeBusUtilization();
steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKS, steerKV, steerKA));
// steerMotor.setNeutralMode(NeutralModeValue.Brake);
this.moduleLocation = moduleLocation;
@ -88,7 +94,9 @@ public class SwerveModule {
}
public void setSteerRotations(Rotation2d rotations){
steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations.getMeasure()));
// steerMotor.setVoltage(steerPid.calculate(getSteerRotations().getRadians(), rotations.getRadians()));
// steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations.getRotations()));
steerMotor.setControl(steerRequest.withPosition(rotations.getRotations()));
}
public Rotation2d getSteerRotations(){
@ -124,20 +132,33 @@ public class SwerveModule {
return moduleLocation;
}
public void setModuleState(SwerveModuleState state){
public void setRequestedModuleState(SwerveModuleState state){
this.setSteerRotations(state.angle);
this.setDriveSpeed(state.speedMetersPerSecond);
moduleState = state;
requestedModuleState = state;
}
public SwerveModuleState getModuleState(){
return moduleState;
public SwerveModuleState getRequestedModuleState(){
return requestedModuleState;
}
public SwerveModuleState getActualModuleState() {
return new SwerveModuleState(getDriveWheelSpeed(), getSteerRotations());
}
public SwerveModulePosition getModulePosition(){
return new SwerveModulePosition(this.getModuleDistance(), getSteerRotations());
}
public void periodic(){
SmartDashboard.putNumber(moduleName + " steerReq", steerPosRequ.Position);
SmartDashboard.putNumber(moduleName + " steerPow", steerMotor.getClosedLoopOutput().getValueAsDouble());
SmartDashboard.putNumber(moduleName + " steer error", steerMotor.getClosedLoopError().getValueAsDouble());
SmartDashboard.putNumber(moduleName + " steer setpoint", steerMotor.getClosedLoopReference().getValueAsDouble());
SmartDashboard.putNumber(moduleName + " steerPos", getSteerRotations().getRotations());
}
public String getModuleName(){
return moduleName;
}
@ -147,14 +168,18 @@ public class SwerveModule {
return distance;
}
private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD, double kV){
private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD, double kV, boolean driveInverted){
return new TalonFXConfiguration().withSlot0(
new Slot0Configs()
.withKP(kP)
.withKI(kI)
.withKD(kD)
.withKV(kV)
);
)
.withMotorOutput(
new MotorOutputConfigs()
.withInverted(driveInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive)
);
}
private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD, double kS, double kV, double kA){
@ -169,13 +194,13 @@ public class SwerveModule {
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign)
).withMotionMagic(
new MotionMagicConfigs()
.withMotionMagicCruiseVelocity(80)
.withMotionMagicCruiseVelocity(160)
.withMotionMagicAcceleration(160)
.withMotionMagicJerk(1600)
).withFeedback(
new FeedbackConfigs()
.withFeedbackRemoteSensorID(steerEncoder.getDeviceID())
.withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder)
.withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
.withSensorToMechanismRatio(1.0)
.withRotorToSensorRatio(12.8)
).withClosedLoopGeneral(
@ -187,8 +212,9 @@ public class SwerveModule {
private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){
return new CANcoderConfiguration().withMagnetSensor(
new MagnetSensorConfigs()
.withMagnetOffset(steerOffset)
.withSensorDirection(SensorDirectionValue.CounterClockwise_Positive)
.withMagnetOffset(-steerOffset)
.withSensorDirection(SensorDirectionValue.Clockwise_Positive)
.withAbsoluteSensorDiscontinuityPoint(0.5)
);
}