District Event #3

ExcaliburFRC · Mar 7, 2024 · fed3ece · fed3ece
1 parent 492f32d
commit fed3ece
Showing 1 changed file with 5 additions and 12 deletions.
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
@@ -34,7 +34,7 @@ public class SwerveModule implements Sendable, Logged {
   //a pid controller for the angle of the module
   private final PIDController _spinningPIDController;
 
-  public Trigger isReset = new Trigger(()-> Math.abs(getRelPos()) < TOLERANCE).debounce(0.1);
+  public Trigger isReset = new Trigger(()-> Math.abs(getResetRad()) < TOLERANCE).debounce(0.1);
 
   // construct the class
   public SwerveModule(
@@ -90,12 +90,10 @@ public double getAbsEncoderRad() {
 
   public void resetEncoders() {
 //    _driveEncoder.setPosition(0);
-
-//    _angleMotor.setPosition(getAbsEncoderRad());
-    _angleMotor.setPosition(0);
+    _angleMotor.setPosition(getAbsEncoderRad());
   }
 
-  @Log.NT (key = "ModuleState")
+  @Log.NT (key = "ModuleState", level = LogLevel.NOT_FILE_ONLY)
   public SwerveModuleState getState() {
     return new SwerveModuleState(_driveMotor.getVelocity(), new Rotation2d(_angleMotor.getPosition()));
   }
@@ -105,10 +103,6 @@ public SwerveModulePosition getPosition(){
     return new SwerveModulePosition(_driveMotor.getPosition(), Rotation2d.fromRadians(_angleMotor.getPosition()));
   }
 
-  public double getRelPos(){
-    return _angleMotor.getPosition();
-  }
-
   @Log.NT
   public double getAbsPosition(){
     return _absEncoder.getAbsolutePosition();
@@ -132,8 +126,8 @@ public void setDesiredState(SwerveModuleState state) {
   }
 
   public void spinTo(double setpoint){
-    if (Math.abs(getRelPos() - setpoint) > TOLERANCE) {
-      _angleMotor.set(-_spinningPIDController.calculate(setpoint, getRelPos()));
+    if (Math.abs(getResetRad() - setpoint) > TOLERANCE) {
+      _angleMotor.set(-_spinningPIDController.calculate(setpoint, getResetRad()));
     }
     else {
       _angleMotor.set(0);
@@ -166,6 +160,5 @@ public void initSendable(SendableBuilder builder) {
     builder.addDoubleProperty("absEncoderPos", this::getAbsPos, null);
     builder.addDoubleProperty("drive output current", _driveMotor::getOutputCurrent, null);
     builder.addDoubleProperty("drive dc output", _driveMotor::getAppliedOutput, null);
-    builder.addDoubleProperty("relativeAngle", this::getRelPos, null);
   }
 }