Merge pull request #141 from Robocubs/brake-efficiency

Brake efficiency
Robocubs · Apr 13, 2024 · b1ab592 · b1ab592
2 parents 6969266 + 2c1d6da
commit b1ab592
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 2 deletions.
diff --git a/src/main/java/com/team1701/lib/drivers/motors/MotorIOTalonFX.java b/src/main/java/com/team1701/lib/drivers/motors/MotorIOTalonFX.java
@@ -5,6 +5,7 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.controls.PositionDutyCycle;
@@ -28,6 +29,7 @@ public class MotorIOTalonFX implements MotorIO {
     private final StatusSignal<Double> mTorqueCurrentSignal;
     private final Slot0Configs mSlot0Configs;
 
+    private NeutralModeValue mNeutralMode;
     private Optional<Queue<Double>> mPositionSamples = Optional.empty();
     private Optional<Queue<Double>> mVelocitySamples = Optional.empty();
 
@@ -44,6 +46,10 @@ public MotorIOTalonFX(TalonFX motor) {
         mSlot0Configs = new Slot0Configs();
         mMotor.getConfigurator().refresh(mSlot0Configs);
 
+        var motorOutputConfigs = new MotorOutputConfigs();
+        mMotor.getConfigurator().refresh(motorOutputConfigs);
+        mNeutralMode = motorOutputConfigs.NeutralMode;
+
         BaseStatusSignal.setUpdateFrequencyForAll(
                 100, mPositionSignal, mVelocitySignal, mMotorVoltageSignal, mTorqueCurrentSignal);
         mMotor.optimizeBusUtilization();
@@ -110,7 +116,11 @@ public void runCharacterization(double input) {
 
     @Override
     public void setBrakeMode(boolean enable) {
-        mMotor.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast);
+        var mode = (enable ? NeutralModeValue.Brake : NeutralModeValue.Coast);
+        if (mode != mNeutralMode) {
+            mMotor.setNeutralMode(mode);
+            mNeutralMode = mode;
+        }
     }
 
     @Override

diff --git a/src/main/java/com/team1701/lib/drivers/motors/MotorIOTalonFXFOC.java b/src/main/java/com/team1701/lib/drivers/motors/MotorIOTalonFXFOC.java
@@ -5,6 +5,7 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
@@ -30,6 +31,7 @@ public class MotorIOTalonFXFOC implements MotorIO {
     private final StatusSignal<Double> mTorqueCurrentSignal;
     private final Slot0Configs mSlot0Configs;
 
+    private NeutralModeValue mNeutralMode;
     private Optional<Queue<Double>> mPositionSamples = Optional.empty();
     private Optional<Queue<Double>> mVelocitySamples = Optional.empty();
 
@@ -47,6 +49,10 @@ public MotorIOTalonFXFOC(TalonFX motor) {
         mSlot0Configs = new Slot0Configs();
         mMotor.getConfigurator().refresh(mSlot0Configs);
 
+        var motorOutputConfigs = new MotorOutputConfigs();
+        mMotor.getConfigurator().refresh(motorOutputConfigs);
+        mNeutralMode = motorOutputConfigs.NeutralMode;
+
         BaseStatusSignal.setUpdateFrequencyForAll(
                 100, mPositionSignal, mVelocitySignal, mMotorVoltageSignal, mTorqueCurrentSignal);
         mMotor.optimizeBusUtilization();
@@ -113,7 +119,11 @@ public void runCharacterization(double input) {
 
     @Override
     public void setBrakeMode(boolean enable) {
-        mMotor.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast);
+        var mode = (enable ? NeutralModeValue.Brake : NeutralModeValue.Coast);
+        if (mode != mNeutralMode) {
+            mMotor.setNeutralMode(mode);
+            mNeutralMode = mode;
+        }
     }
 
     @Override