From 4c20746a544bb6cb19ce457a1aa048900cbd07b8 Mon Sep 17 00:00:00 2001
From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com>
Date: Mon, 11 Mar 2024 13:07:42 -0400
Subject: [PATCH] changed current limiting setup

---
 .../subsystems/drive/module/ModuleIOTalonFX.java   | 14 ++++++++++----
 .../robot/subsystems/shooter/ShooterIOKraken.java  |  2 +-
 2 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java
index 1ec2ad14..9e64bf48 100644
--- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java
@@ -99,8 +99,11 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
 
     // run configs on drive motor
     var driveConfig = new TalonFXConfiguration();
+    driveConfig.CurrentLimits.StatorCurrentLimit = 120.0;
+    driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
     driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0;
     driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+    driveConfig.Voltage.SupplyVoltageTimeConstant = 0.02;
     driveConfig.MotorOutput.Inverted =
             moduleConstants.DRIVE_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive
                     : InvertedValue.CounterClockwise_Positive;
@@ -121,8 +124,10 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
 
     // run configs on turning motor
     var turnConfig = new TalonFXConfiguration();
-    turnConfig.CurrentLimits.StatorCurrentLimit = 30.0;
+    turnConfig.CurrentLimits.StatorCurrentLimit = 80.0;
     turnConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+    turnConfig.CurrentLimits.SupplyCurrentLimit = 40.0;
+    turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
     turnConfig.MotorOutput.Inverted =
             moduleConstants.TURN_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive
                     : InvertedValue.CounterClockwise_Positive;
@@ -142,8 +147,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
             .addI(m_moduleConstants.TURN_KI())
             .addD(m_moduleConstants.TURN_KD());
 
-    m_posRequest = new PositionVoltage(0, 0, false, 0, 0, false, false, false);
-
+    m_posRequest = new PositionVoltage(0, 0, true, 0, 0, false, false, false);
 
     // Fancy multithreaded odometry update stuff
     // setup drive values
@@ -225,6 +229,8 @@ public void updateInputs(ModuleIOInputsAutoLogged inputs) {
     inputs.setTurnAppliedVolts(m_turnAppliedVolts.getValueAsDouble());
     inputs.setTurnCurrentAmps(new double[] {m_turnCurrent.getValueAsDouble()});
 
+    inputs.odometryTimestamps =
+            timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
     inputs.setOdometryDrivePositionsRad(m_drivePositionQueue.stream()
         .mapToDouble(Units::rotationsToRadians)
         .toArray());
@@ -239,7 +245,7 @@ public void updateInputs(ModuleIOInputsAutoLogged inputs) {
   @Override
   public void setDriveVelocityMPS(double mps) {
     double rps = (mps / m_moduleConstants.WHEEL_CURCUMFERENCE_METERS()) * m_moduleConstants.DRIVE_GEAR_RATIO();
-    VelocityVoltage velRequest = new VelocityVoltage(rps).withSlot(0);
+    VelocityVoltage velRequest = new VelocityVoltage(rps).withSlot(0).withEnableFOC(true);
     m_driveTalon.setControl(velRequest.withVelocity(rps));
   }
 
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java
index 4e0c21e2..becc0a86 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java
@@ -194,7 +194,7 @@ public void updateInputs(ShooterIOInputsAutoLogged inputs) {
     inputs.intakeTemperature = m_intakeTemperatureSignal.getValueAsDouble();
     inputs.indexerTemperature = m_indexerTemperatureSignal.getValueAsDouble();
 
-   inputs.tofDistanceIn = m_tof.getRange();
+    inputs.tofDistanceIn = m_tof.getRange();
 
     m_leftProperty.updateIfChanged();
     m_rightProperty.updateIfChanged();