diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogAccelerometer.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogAccelerometer.java new file mode 100644 index 0000000000..5ae76b058b --- /dev/null +++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogAccelerometer.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.pidwrappers; + +import edu.wpi.first.wpilibj.AnalogAccelerometer; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; + +/** Wrapper so that PIDSource is implemented for AnalogAccelerometer for old PIDController. */ +public class PIDAnalogAccelerometer extends AnalogAccelerometer implements PIDSource { + protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement; + + public PIDAnalogAccelerometer(int channel) { + super(channel); + } + + public PIDAnalogAccelerometer(AnalogInput channel) { + super(channel); + } + + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + m_pidSource = pidSource; + } + + @Override + public PIDSourceType getPIDSourceType() { + return m_pidSource; + } + + /** + * Get the Acceleration for the PID Source parent. + * + * @return The current acceleration in Gs. + */ + @Override + public double pidGet() { + return getAcceleration(); + } +} diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogGyro.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogGyro.java new file mode 100644 index 0000000000..1d58edf86c --- /dev/null +++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogGyro.java @@ -0,0 +1,65 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.pidwrappers; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; + +/** Wrapper so that PIDSource is implemented for AnalogGyro for old PIDController. */ +public class PIDAnalogGyro extends AnalogGyro implements PIDSource { + private PIDSourceType m_pidSource = PIDSourceType.kDisplacement; + + public PIDAnalogGyro(int channel) { + super(channel); + } + + public PIDAnalogGyro(AnalogInput channel) { + super(channel); + } + + public PIDAnalogGyro(int channel, int center, double offset) { + super(channel, center, offset); + } + + public PIDAnalogGyro(AnalogInput channel, int center, double offset) { + super(channel, center, offset); + } + + /** + * Set which parameter of the gyro you are using as a process control variable. The Gyro class + * supports the rate and displacement parameters + * + * @param pidSource An enum to select the parameter. + */ + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + m_pidSource = pidSource; + } + + @Override + public PIDSourceType getPIDSourceType() { + return m_pidSource; + } + + /** + * Get the output of the gyro for use with PIDControllers. May be the angle or rate depending on + * the set PIDSourceType + * + * @return the output according to the gyro + */ + @Override + public double pidGet() { + switch (m_pidSource) { + case kRate: + return getRate(); + case kDisplacement: + return getAngle(); + default: + return 0.0; + } + } +} diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogInput.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogInput.java new file mode 100644 index 0000000000..fd1d9b216b --- /dev/null +++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogInput.java @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.pidwrappers; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; + +/** Wrapper so that PIDSource is implemented for AnalogInput for old PIDController. */ +public class PIDAnalogInput extends AnalogInput implements PIDSource { + protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement; + + public PIDAnalogInput(int channel) { + super(channel); + } + + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + m_pidSource = pidSource; + } + + @Override + public PIDSourceType getPIDSourceType() { + return m_pidSource; + } + + /** + * Get the average voltage for use with PIDController. + * + * @return the average voltage + */ + @Override + public double pidGet() { + return getAverageVoltage(); + } +} diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogPotentiometer.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogPotentiometer.java new file mode 100644 index 0000000000..e6fc8adf77 --- /dev/null +++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogPotentiometer.java @@ -0,0 +1,62 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.pidwrappers; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; + +/** Wrapper so that PIDSource is implemented for AnalogPotentiometer for old PIDController. */ +public class PIDAnalogPotentiometer extends AnalogPotentiometer implements PIDSource { + protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement; + + public PIDAnalogPotentiometer(int channel, double fullRange, double offset) { + super(channel, fullRange, offset); + } + + public PIDAnalogPotentiometer(AnalogInput input, double fullRange, double offset) { + super(input, fullRange, offset); + } + + public PIDAnalogPotentiometer(int channel, double scale) { + super(channel, scale); + } + + public PIDAnalogPotentiometer(AnalogInput input, double scale) { + super(input, scale); + } + + public PIDAnalogPotentiometer(int channel) { + super(channel); + } + + public PIDAnalogPotentiometer(AnalogInput input) { + super(input); + } + + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + if (!pidSource.equals(PIDSourceType.kDisplacement)) { + throw new IllegalArgumentException("Only displacement PID is allowed for potentiometers."); + } + m_pidSource = pidSource; + } + + @Override + public PIDSourceType getPIDSourceType() { + return m_pidSource; + } + + /** + * Implement the PIDSource interface. + * + * @return The current reading. + */ + @Override + public double pidGet() { + return get(); + } +} diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDEncoder.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDEncoder.java new file mode 100644 index 0000000000..76ed04fe33 --- /dev/null +++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDEncoder.java @@ -0,0 +1,101 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.pidwrappers; + +import edu.wpi.first.wpilibj.DigitalSource; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; + +/** Wrapper so that PIDSource is implemented for Encoder for old PIDController. */ +public class PIDEncoder extends Encoder implements PIDSource { + private PIDSourceType m_pidSource = PIDSourceType.kDisplacement; + + public PIDEncoder(final int channelA, final int channelB, boolean reverseDirection) { + super(channelA, channelB, reverseDirection, EncodingType.k4X); + } + + public PIDEncoder(final int channelA, final int channelB) { + super(channelA, channelB, false); + } + + public PIDEncoder( + final int channelA, + final int channelB, + boolean reverseDirection, + final EncodingType encodingType) { + super(channelA, channelB, reverseDirection, encodingType); + } + + public PIDEncoder( + final int channelA, final int channelB, final int indexChannel, boolean reverseDirection) { + super(channelA, channelB, indexChannel, reverseDirection); + } + + public PIDEncoder(final int channelA, final int channelB, final int indexChannel) { + super(channelA, channelB, indexChannel, false); + } + + public PIDEncoder(DigitalSource sourceA, DigitalSource sourceB, boolean reverseDirection) { + super(sourceA, sourceB, reverseDirection, EncodingType.k4X); + } + + public PIDEncoder(DigitalSource sourceA, DigitalSource sourceB) { + super(sourceA, sourceB, false); + } + + public PIDEncoder( + DigitalSource sourceA, + DigitalSource sourceB, + boolean reverseDirection, + final EncodingType encodingType) { + super(sourceA, sourceB, reverseDirection, encodingType); + } + + public PIDEncoder( + DigitalSource sourceA, + DigitalSource sourceB, + DigitalSource indexSource, + boolean reverseDirection) { + super(sourceA, sourceB, indexSource, reverseDirection); + } + + public PIDEncoder(DigitalSource sourceA, DigitalSource sourceB, DigitalSource indexSource) { + super(sourceA, sourceB, indexSource, false); + } + + /** + * Set which parameter of the encoder you are using as a process control variable. The encoder + * class supports the rate and distance parameters. + * + * @param pidSource An enum to select the parameter. + */ + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + m_pidSource = pidSource; + } + + @Override + public PIDSourceType getPIDSourceType() { + return m_pidSource; + } + + /** + * Implement the PIDSource interface. + * + * @return The current value of the selected source parameter. + */ + @Override + public double pidGet() { + switch (m_pidSource) { + case kDisplacement: + return getDistance(); + case kRate: + return getRate(); + default: + return 0.0; + } + } +} diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDMotorController.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDMotorController.java new file mode 100644 index 0000000000..85df7cd850 --- /dev/null +++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDMotorController.java @@ -0,0 +1,108 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.pidwrappers; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; + +/** Wrapper so that PIDOutput is implemented for MotorController for old PIDController. */ +public class PIDMotorController implements PIDOutput, MotorController, Sendable { + private final MotorController m_motorController; + + public PIDMotorController(MotorController motorController) { + m_motorController = motorController; + } + + /** + * Write out the PID value as seen in the PIDOutput base object. + * + * @param output Write out the PWM value as was found in the PIDController + */ + @Override + public void pidWrite(double output) { + m_motorController.set(output); + } + + /** + * Common interface for setting the speed of a motor controller. + * + * @param speed The speed to set. Value should be between -1.0 and 1.0. + */ + @Override + public void set(double speed) { + m_motorController.set(speed); + } + + /** + * Sets the voltage output of the MotorController. Compensates for the current bus voltage to + * ensure that the desired voltage is output even if the battery voltage is below 12V - highly + * useful when the voltage outputs are "meaningful" (e.g. they come from a feedforward + * calculation). + * + *
NOTE: This function *must* be called regularly in order for voltage compensation to work
+ * properly - unlike the ordinary set function, it is not "set it and forget it."
+ *
+ * @param outputVolts The voltage to output.
+ */
+ @Override
+ public void setVoltage(double outputVolts) {
+ m_motorController.setVoltage(outputVolts);
+ }
+
+ /**
+ * Common interface for getting the current set speed of a motor controller.
+ *
+ * @return The current set speed. Value is between -1.0 and 1.0.
+ */
+ @Override
+ public double get() {
+ return m_motorController.get();
+ }
+
+ /**
+ * Common interface for inverting direction of a motor controller.
+ *
+ * @param isInverted The state of inversion true is inverted.
+ */
+ @Override
+ public void setInverted(boolean isInverted) {
+ m_motorController.setInverted(isInverted);
+ }
+
+ /**
+ * Common interface for returning if a motor controller is in the inverted state or not.
+ *
+ * @return isInverted The state of the inversion true is inverted.
+ */
+ @Override
+ public boolean getInverted() {
+ return m_motorController.getInverted();
+ }
+
+ /** Disable the motor controller. */
+ @Override
+ public void disable() {
+ m_motorController.disable();
+ }
+
+ /**
+ * Stops motor movement. Motor can be moved again by calling set without having to re-enable the
+ * motor.
+ */
+ @Override
+ public void stopMotor() {
+ m_motorController.stopMotor();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Motor Controller");
+ builder.setActuator(true);
+ builder.setSafeState(this::disable);
+ builder.addDoubleProperty("Value", this::get, this::set);
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDUltrasonic.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDUltrasonic.java
new file mode 100644
index 0000000000..48cb1e6f40
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDUltrasonic.java
@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.pidwrappers;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.Ultrasonic;
+
+/** Wrapper so that PIDSource is implemented for Ultrasonic for old PIDController. */
+public class PIDUltrasonic extends Ultrasonic implements PIDSource {
+ protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+ public PIDUltrasonic(int pingChannel, int echoChannel) {
+ super(pingChannel, echoChannel);
+ }
+
+ public PIDUltrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
+ super(pingChannel, echoChannel);
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ if (!pidSource.equals(PIDSourceType.kDisplacement)) {
+ throw new IllegalArgumentException("Only displacement PID is allowed for ultrasonics.");
+ }
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ /**
+ * Get the range in the inches for the PIDSource base object.
+ *
+ * @return The range in inches
+ */
+ @Override
+ public double pidGet() {
+ return getRangeInches();
+ }
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogAccelerometer.cpp b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogAccelerometer.cpp
new file mode 100644
index 0000000000..62801e75d6
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogAccelerometer.cpp
@@ -0,0 +1,11 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/pidwrappers/PIDAnalogAccelerometer.h"
+
+using namespace frc;
+
+double PIDAnalogAccelerometer::PIDGet() {
+ return GetAcceleration();
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogGyro.cpp b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogGyro.cpp
new file mode 100644
index 0000000000..d6a9ace5d1
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogGyro.cpp
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/pidwrappers/PIDAnalogGyro.h"
+
+using namespace frc;
+
+double PIDAnalogGyro::PIDGet() {
+ switch (GetPIDSourceType()) {
+ case PIDSourceType::kRate:
+ return GetRate();
+ case PIDSourceType::kDisplacement:
+ return GetAngle();
+ default:
+ return 0;
+ }
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogInput.cpp b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogInput.cpp
new file mode 100644
index 0000000000..d8dc7165aa
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogInput.cpp
@@ -0,0 +1,11 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/pidwrappers/PIDAnalogInput.h"
+
+using namespace frc;
+
+double PIDAnalogInput::PIDGet() {
+ return GetAverageVoltage();
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogPotentiometer.cpp b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogPotentiometer.cpp
new file mode 100644
index 0000000000..7b44f6b177
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogPotentiometer.cpp
@@ -0,0 +1,11 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/pidwrappers/PIDAnalogPotentiometer.h"
+
+using namespace frc;
+
+double PIDAnalogPotentiometer::PIDGet() {
+ return Get();
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDEncoder.cpp b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDEncoder.cpp
new file mode 100644
index 0000000000..b98d1c9753
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDEncoder.cpp
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/pidwrappers/PIDEncoder.h"
+
+using namespace frc;
+
+double PIDEncoder::PIDGet() {
+ switch (GetPIDSourceType()) {
+ case PIDSourceType::kDisplacement:
+ return GetDistance();
+ case PIDSourceType::kRate:
+ return GetRate();
+ default:
+ return 0.0;
+ }
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDMotorController.cpp b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDMotorController.cpp
new file mode 100644
index 0000000000..68d0be301f
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDMotorController.cpp
@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/pidwrappers/PIDMotorController.h"
+
+#include