diff --git a/src/main/java/com/team766/hal/LocalMotorController.java b/src/main/java/com/team766/hal/LocalMotorController.java index ad3f5a63..272dff69 100644 --- 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 index 274931f4..ff53b461 100755 --- 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 minOutput, ValueProvider 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 index c8fac1fc..b791a32f 100644 --- 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 index b2924d3e..68618c5b 100644 --- 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 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 index 5d6f884a..3caf7c5c 100644 --- a/src/main/java/com/team766/hal/wpilib/CANTalonMotorController.java +++ b/src/main/java/com/team766/hal/wpilib/CANTalonMotorController.java @@ -6,6 +6,7 @@ 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; @@ -13,15 +14,15 @@ 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 index 27cc3b3c..fa621f0d 100644 --- a/src/main/java/com/team766/hal/wpilib/CANVictorMotorController.java +++ b/src/main/java/com/team766/hal/wpilib/CANVictorMotorController.java @@ -6,6 +6,7 @@ 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; @@ -13,15 +14,15 @@ 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; + } }