onboard turnPID added

Team-4795 · Feb 6, 2025 · 0796b75 · 0796b75
1 parent ad7a167
commit 0796b75
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java
@@ -16,7 +16,7 @@ public class Module {
     private final int index;
     private final SimpleMotorFeedforward drivFeedforward;
     private final PIDController driveFeddbackPidController;
-    private final PIDController turnFeePidController;
+    private final PIDController turnFeedPidController;
     private Rotation2d angleSetpoint = null;
     private Double speedSetPoint = null;
     private Rotation2d turnRelativeOffset = null;
@@ -29,26 +29,26 @@ public Module(ModuleIO io, int index) {
             case REAL:
                 drivFeedforward = new SimpleMotorFeedforward(DriveConstants.TranslationKS, DriveConstants.TranslationKV);
                 driveFeddbackPidController = new PIDController(DriveConstants.TranslationKP, DriveConstants.TranslationKI, DriveConstants.TranslationKD);
-                turnFeePidController = new PIDController(DriveConstants.RotationKP, DriveConstants.RotationKI, DriveConstants.RotationKD);
+               turnFeedPidController = new PIDController(DriveConstants.RotationKP, DriveConstants.RotationKI, DriveConstants.RotationKD);
                 break;
             case REPLAY:
                 drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
                 driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0);
-                turnFeePidController = new PIDController(7, 0.0, 0.0);
+                turnFeedPidController = new PIDController(7, 0.0, 0.0);
                 break;
             case SIM:
                 drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
                 driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0);
-                turnFeePidController = new PIDController(10, 0.0, 0.0);
+                turnFeedPidController = new PIDController(10, 0.0, 0.0);
                 break;
             default:
                 drivFeedforward = new SimpleMotorFeedforward(0.0, 0.0);
                 driveFeddbackPidController = new PIDController(0.0, 0.0, 0.0);
-                turnFeePidController = new PIDController(0.0, 0.0, 0.0);
+                turnFeedPidController = new PIDController(0.0, 0.0, 0.0);
                 break;
         }
 
-        turnFeePidController.enableContinuousInput(-Math.PI, Math.PI);
+        turnFeedPidController.enableContinuousInput(-Math.PI, Math.PI);
         setBrakeMode(true);
     }
 
@@ -61,11 +61,11 @@ public void periodic() {
         }
 
         if (angleSetpoint != null) {
-            io.setTurnVoltage(turnFeePidController.calculate(getAngle().getRadians(), angleSetpoint.getRadians()));
+            io.setTurnVoltage(turnFeedPidController.calculate(getAngle().getRadians(), angleSetpoint.getRadians()));
         }
 
         if (speedSetPoint != null) {
-            double adjustSpeedSetpoint = speedSetPoint * Math.cos(turnFeePidController.getPositionError());
+            double adjustSpeedSetpoint = speedSetPoint * Math.cos(turnFeedPidController.getPositionError());
 
             double velocityRadPerSec = adjustSpeedSetpoint / DriveConstants.WHEEL_RADIUS;
             io.setDriveVoltage(

diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
@@ -127,9 +127,6 @@ public ModuleIOTalonFX(int index) {
     driveTalonFX.setPosition(0);
     driveTalonFX.setInverted(true);
 
-    encoderconfig.positionConversionFactor(2 * Math.PI);
-    encoderconfig.velocityConversionFactor(2 * Math.PI / 60);
-
     turnSparkFlex.setCANTimeout(0);