fixed getstate units

team3316 · Jan 7, 2022 · 0418ac4 · 0418ac4
1 parent 29776c1
commit 0418ac4
Showing 1 changed file with 12 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java
@@ -31,7 +31,7 @@ public class SwerveModule {
     public SwerveModule(SwerveModuleConstants constants) {
         _driveSparkMax = configSparkMax(constants.idDrive, _drivePID, _driveEncoder, constants.driveGains);
         _steeringSparkMax = configSparkMax(constants.idSteering, _steeringPID, _steeringEncoder, constants.steeringGains);
-        
+
         _drivePID = _driveSparkMax.getPIDController();
         _driveEncoder = _driveSparkMax.getEncoder();
 
@@ -40,14 +40,14 @@ public SwerveModule(SwerveModuleConstants constants) {
 
         setPIDGains(_drivePID, constants.driveGains);
         setPIDGains(_steeringPID, constants.steeringGains);
-                
+
         _steerSetpoint = 0;
     }
 
     private static CANSparkMax configSparkMax(int id, CANPIDController pidController, CANEncoder encoder, PIDFGains gains) {
         CANSparkMax sparkMax = new CANSparkMax(id, MotorType.kBrushless);
         sparkMax.setInverted(false);
-        
+
         return sparkMax;
     }
 
@@ -59,7 +59,7 @@ private static void setPIDGains(CANPIDController pidController, PIDFGains gains)
         pidController.setIZone(gains.getIZone());
         pidController.setOutputRange(-1.0,1.0);
     }
-    
+
     public void setDriveSteering(double percent) {
         this._steeringSparkMax.set(percent);
     }
@@ -71,27 +71,28 @@ public void stop() {
         setDriveDrive(0);
         // setDriveSteering(0);
     }
-    
+
     public SwerveModuleState getState() {
-        // TODO: Do we need here getAbsSteeringPos or just _steeringEncoder.getPosition() ?
-        return new SwerveModuleState(_driveEncoder.getVelocity(), new Rotation2d(getAbsSteeringPos()));
+        return new SwerveModuleState(
+                _driveEncoder.getVelocity() / 60 * Constants.Drivetrain.SwerveModuleConstants.driveDPRMeters,
+                new Rotation2d(getAngle()));
     }
-    
+
     public void setDesiredState(SwerveModuleState desiredState) {
         // Optimize the reference state to avoid spinning further than 90 degrees
         SwerveModuleState state = optimizeAngle(desiredState, Rotation2d.fromDegrees(getAngle()));
 
         _steerSetpoint = addDeltaFromZeroToEncoder(state.angle.getDegrees());
         _driveSetpoint = driveVelocityToRPM(state.speedMetersPerSecond);
 
-        if (state.speedMetersPerSecond != 0) { 
+        if (state.speedMetersPerSecond != 0) {
             _steeringPID.setReference(_steerSetpoint, ControlType.kPosition);
         }
         // _driveSparkMax.set(1 * Math.signum(state.speedMetersPerSecond));
         if (state.speedMetersPerSecond == 0)
             _driveSparkMax.set(0);
         else
-        _drivePID.setReference(_driveSetpoint, ControlType.kVelocity);
+            _drivePID.setReference(_driveSetpoint, ControlType.kVelocity);
     }
 
     public static SwerveModuleState optimizeAngle(SwerveModuleState desiredState, Rotation2d currentRadian) {
@@ -117,7 +118,7 @@ public double addDeltaFromZeroToEncoder(double angle) {
 
     private double driveVelocityToRPM(double velocity) {
         // divide by distance per revolution, multiply by a minute to get RPM
-        return velocity / (Constants.Drivetrain.SwerveModuleConstants.driveDPRMeters) * 60; 
+        return velocity / (Constants.Drivetrain.SwerveModuleConstants.driveDPRMeters) * 60;
     }
 
     public double getAbsSteeringPos() {