Working but offsets are busted

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

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

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

@@ -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++){
+        for(int i=0; i<modules.length;i++){
-        //     moduleStates[i].optimize(modules[i].getSteerRotations());
+            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(){
+    public SwerveModuleState[] getRequestedModuleStates(){
-        return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleState()).toArray(SwerveModuleState[]::new);
+        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){

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

@@ -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.getAbsolutePosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
-        steerEncoder.optimizeBusUtilization();
+        // steerEncoder.optimizeBusUtilization();
         steerEncoder.getConfigurator().apply(generateCanCoderConfiguration(steerOffset));
 
         driveMotor = new TalonFX(driveMotorId, canBusName);
-        driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
+        // driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
-        driveMotor.optimizeBusUtilization();
+        // driveMotor.optimizeBusUtilization();
-        driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV));
+        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(){
+    public SwerveModuleState getRequestedModuleState(){
-        return moduleState;
+        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)
+                .withMagnetOffset(-steerOffset)
-                .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive)
+                .withSensorDirection(SensorDirectionValue.Clockwise_Positive)
+                .withAbsoluteSensorDiscontinuityPoint(0.5)
             );
     }