make HAL work

Team766 · Dec 27, 2024 · 1fbd484 · 1fbd484
1 parent ac04ccc
commit 1fbd484
Show file tree

Hide file tree

Showing 6 changed files with 27 additions and 49 deletions.
diff --git a/src/main/java/com/team766/hal/LocalMotorController.java b/src/main/java/com/team766/hal/LocalMotorController.java
@@ -10,9 +10,10 @@
 import com.team766.logging.Severity;
 
 public class LocalMotorController implements MotorController {
-    private BasicMotorController motor;
-    private ControlInputReader sensor;
-    private PIDController pidController;
+    private final BasicMotorController motor;
+    private final ControlInputReader sensor;
+    private final PIDController pidController;
+    private final PIDConfig pidConfig = new PIDConfig();
 
     private boolean inverted = false;
     private boolean sensorInverted = false;
@@ -307,15 +308,8 @@ public void setClosedLoopRamp(final double secondsFromNeutralToFull) {
     }
 
     @Override
-    public double getOutputVoltage() {
-        if (LocalMotorController.this.motor instanceof MotorController) {
-            double voltage = ((MotorController) LocalMotorController.this.motor).getOutputVoltage();
-            if (inverted) {
-                voltage *= -1;
-            }
-            return voltage;
-        } else {
-            return get() * RobotProvider.instance.getBatteryVoltage();
-        }
+    public PIDConfig getPIDConfig() {
+        // TODO: Deduplicate(?) this functionality with pidController
+        return pidConfig;
     }
 }
diff --git a/src/main/java/com/team766/hal/MotorController.java b/src/main/java/com/team766/hal/MotorController.java
@@ -10,10 +10,6 @@
  * Interface for motor controlling devices.
  */
 public interface MotorController extends BasicMotorController {
-    // Almost all systems on our robot work on the scale of 0-12 V.
-    // 0.1 V seems like a reasonable tolerance for that scale.
-    static final double VOLTAGE_TOLERANCE = 0.1;
-
     enum Type {
         VictorSP,
         VictorSPX,
@@ -158,8 +154,6 @@ default void setOutputRange(ValueProvider<Double> minOutput, ValueProvider<Doubl
 
     void setClosedLoopRamp(double secondsFromNeutralToFull);
 
-    double getOutputVoltage();
-
     default Mechanism.Directive requestStop() {
         return requestPercentOutput(0.0);
     }
@@ -205,9 +199,4 @@ default Mechanism.Directive requestVelocity(
         set(MotorController.ControlMode.Velocity, targetVelocity);
         return () -> Math.abs(targetVelocity - getSensorVelocity()) <= velocityErrorThreshold;
     }
-
-    default Mechanism.Directive requestVoltage(double targetVoltage) {
-        set(MotorController.ControlMode.Voltage, targetVoltage);
-        return () -> Math.abs(targetVoltage - getOutputVoltage()) <= VOLTAGE_TOLERANCE;
-    }
 }
diff --git a/src/main/java/com/team766/hal/MotorControllerWithSensorScale.java b/src/main/java/com/team766/hal/MotorControllerWithSensorScale.java
@@ -148,9 +148,4 @@ public PIDConfig getPIDConfig() {
         // TODO: Deduplicate this functionality with the delegate
         return pidConfig;
     }
-
-    @Override
-    public double getOutputVoltage() {
-        return delegate.getOutputVoltage();
-    }
 }
diff --git a/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java b/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java
@@ -270,16 +270,4 @@ public void setNeutralMode(final NeutralMode neutralMode) {
         }
         super.setNeutralMode(neutralModeValue);
     }
-
-    @Override
-    public double getOutputVoltage() {
-        StatusSignal<Double> status = super.getMotorVoltage();
-        StatusCode code = status.getStatus();
-        if (code.isOK()) {
-            return status.getValueAsDouble();
-        } else {
-            statusCodeToException(ExceptionTarget.LOG, code);
-            return 0.0;
-        }
-    }
 }
diff --git a/src/main/java/com/team766/hal/wpilib/CANTalonMotorController.java b/src/main/java/com/team766/hal/wpilib/CANTalonMotorController.java
@@ -6,22 +6,23 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.team766.hal.MotorController;
+import com.team766.hal.PIDConfig;
 import com.team766.logging.Category;
 import com.team766.logging.Logger;
 import com.team766.logging.LoggerExceptionUtils;
 import com.team766.logging.Severity;
 
 public class CANTalonMotorController extends BaseCTREMotorController implements MotorController {
 
-    private WPI_TalonSRX m_device;
-    private double m_feedForward = 0.0;
+    private final WPI_TalonSRX m_device;
+    private final PIDConfig pidConfig = new PIDConfig();
 
     public CANTalonMotorController(final int deviceNumber) {
         m_device = new WPI_TalonSRX(deviceNumber);
     }
 
     @Override
-    public void set(final ControlMode mode, double value) {
+    public void set(final ControlMode mode, double value, double arbitraryFeedForward) {
         com.ctre.phoenix.motorcontrol.ControlMode ctre_mode = null;
         boolean useFourTermSet = true;
         switch (mode) {
@@ -56,7 +57,7 @@ public void set(final ControlMode mode, double value) {
             ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Disabled;
         }
         if (useFourTermSet) {
-            m_device.set(ctre_mode, value, DemandType.ArbitraryFeedForward, m_feedForward);
+            m_device.set(ctre_mode, value, DemandType.ArbitraryFeedForward, arbitraryFeedForward);
         } else {
             m_device.set(ctre_mode, value);
         }
@@ -184,4 +185,9 @@ public boolean getInverted() {
     public void setNeutralMode(final NeutralMode neutralMode) {
         m_device.setNeutralMode(neutralMode);
     }
+
+    @Override
+    public PIDConfig getPIDConfig() {
+        return pidConfig;
+    }
 }
diff --git a/src/main/java/com/team766/hal/wpilib/CANVictorMotorController.java b/src/main/java/com/team766/hal/wpilib/CANVictorMotorController.java
@@ -6,22 +6,23 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
 import com.team766.hal.MotorController;
+import com.team766.hal.PIDConfig;
 import com.team766.logging.Category;
 import com.team766.logging.Logger;
 import com.team766.logging.LoggerExceptionUtils;
 import com.team766.logging.Severity;
 
 public class CANVictorMotorController extends BaseCTREMotorController implements MotorController {
 
-    private WPI_VictorSPX m_device;
-    private double m_feedForward = 0.0;
+    private final WPI_VictorSPX m_device;
+    private final PIDConfig pidConfig = new PIDConfig();
 
     public CANVictorMotorController(final int deviceNumber) {
         m_device = new WPI_VictorSPX(deviceNumber);
     }
 
     @Override
-    public void set(final ControlMode mode, double value) {
+    public void set(final ControlMode mode, double value, double arbitraryFeedForward) {
         com.ctre.phoenix.motorcontrol.ControlMode ctre_mode = null;
         boolean useFourTermSet = true;
         switch (mode) {
@@ -56,7 +57,7 @@ public void set(final ControlMode mode, double value) {
             ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Disabled;
         }
         if (useFourTermSet) {
-            m_device.set(ctre_mode, value, DemandType.ArbitraryFeedForward, m_feedForward);
+            m_device.set(ctre_mode, value, DemandType.ArbitraryFeedForward, arbitraryFeedForward);
         } else {
             m_device.set(ctre_mode, value);
         }
@@ -181,4 +182,9 @@ public void setCurrentLimit(final double ampsLimit) {
     public void restoreFactoryDefault() {
         errorCodeToException(ExceptionTarget.LOG, m_device.configFactoryDefault());
     }
+
+    @Override
+    public PIDConfig getPIDConfig() {
+        return pidConfig;
+    }
 }