From 9aba2b75833a0dc9fe765e9aaeb25ccc339150a4 Mon Sep 17 00:00:00 2001 From: sciencewhiz Date: Tue, 23 Nov 2021 20:30:30 -0800 Subject: [PATCH] [oldCommands] Add wrappers for WPILib objects to work with old PID Controller (#3710) --- .../pidwrappers/PIDAnalogAccelerometer.java | 43 +++++++ .../wpilibj/pidwrappers/PIDAnalogGyro.java | 65 +++++++++++ .../wpilibj/pidwrappers/PIDAnalogInput.java | 38 ++++++ .../pidwrappers/PIDAnalogPotentiometer.java | 62 ++++++++++ .../first/wpilibj/pidwrappers/PIDEncoder.java | 101 ++++++++++++++++ .../pidwrappers/PIDMotorController.java | 108 ++++++++++++++++++ .../wpilibj/pidwrappers/PIDUltrasonic.java | 47 ++++++++ .../pidwrappers/PIDAnalogAccelerometer.cpp | 11 ++ .../native/cpp/pidwrappers/PIDAnalogGyro.cpp | 18 +++ .../native/cpp/pidwrappers/PIDAnalogInput.cpp | 11 ++ .../pidwrappers/PIDAnalogPotentiometer.cpp | 11 ++ .../native/cpp/pidwrappers/PIDEncoder.cpp | 18 +++ .../cpp/pidwrappers/PIDMotorController.cpp | 50 ++++++++ .../native/cpp/pidwrappers/PIDUltrasonic.cpp | 11 ++ .../frc/pidwrappers/PIDAnalogAccelerometer.h | 28 +++++ .../include/frc/pidwrappers/PIDAnalogGyro.h | 28 +++++ .../include/frc/pidwrappers/PIDAnalogInput.h | 28 +++++ .../frc/pidwrappers/PIDAnalogPotentiometer.h | 28 +++++ .../include/frc/pidwrappers/PIDEncoder.h | 28 +++++ .../frc/pidwrappers/PIDMotorController.h | 63 ++++++++++ .../include/frc/pidwrappers/PIDUltrasonic.h | 27 +++++ 21 files changed, 824 insertions(+) create mode 100644 wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogAccelerometer.java create mode 100644 wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogGyro.java create mode 100644 wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogInput.java create mode 100644 wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogPotentiometer.java create mode 100644 wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDEncoder.java create mode 100644 wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDMotorController.java create mode 100644 wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDUltrasonic.java create mode 100644 wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogAccelerometer.cpp create mode 100644 wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogGyro.cpp create mode 100644 wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogInput.cpp create mode 100644 wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDAnalogPotentiometer.cpp create mode 100644 wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDEncoder.cpp create mode 100644 wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDMotorController.cpp create mode 100644 wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDUltrasonic.cpp create mode 100644 wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogAccelerometer.h create mode 100644 wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogGyro.h create mode 100644 wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogInput.h create mode 100644 wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogPotentiometer.h create mode 100644 wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDEncoder.h create mode 100644 wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDMotorController.h create mode 100644 wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDUltrasonic.h 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 +#include +#include + +using namespace frc; + +PIDMotorController::PIDMotorController(MotorController& motorController) + : m_motorController(motorController) {} + +void PIDMotorController::Set(double speed) { + m_motorController.Set(m_isInverted ? -speed : speed); +} + +double PIDMotorController::Get() const { + return m_motorController.Get() * (m_isInverted ? -1.0 : 1.0); +} + +void PIDMotorController::SetInverted(bool isInverted) { + m_isInverted = isInverted; +} + +bool PIDMotorController::GetInverted() const { + return m_isInverted; +} + +void PIDMotorController::Disable() { + m_motorController.Disable(); +} + +void PIDMotorController::StopMotor() { + Disable(); +} + +void PIDMotorController::PIDWrite(double output) { + m_motorController.Set(output); +} + +void PIDMotorController::InitSendable(wpi::SendableBuilder& builder) { + builder.SetSmartDashboardType("Motor Controller"); + builder.SetActuator(true); + builder.SetSafeState([=] { Disable(); }); + builder.AddDoubleProperty( + "Value", [=] { return Get(); }, [=](double value) { Set(value); }); +} diff --git a/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDUltrasonic.cpp b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDUltrasonic.cpp new file mode 100644 index 0000000000..4c02f17c6a --- /dev/null +++ b/wpilibOldCommands/src/main/native/cpp/pidwrappers/PIDUltrasonic.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/PIDUltrasonic.h" + +using namespace frc; + +double PIDUltrasonic::PIDGet() { + return GetRange().value(); +} diff --git a/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogAccelerometer.h b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogAccelerometer.h new file mode 100644 index 0000000000..67a3bedb36 --- /dev/null +++ b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogAccelerometer.h @@ -0,0 +1,28 @@ +// 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. + +#pragma once + +#include "frc/AnalogAccelerometer.h" +#include "frc/PIDSource.h" + +namespace frc { + +/** + * Wrapper so that PIDSource is implemented for AnalogAccelerometer for old + * PIDController + */ +class PIDAnalogAccelerometer : public PIDSource, public AnalogAccelerometer { + using AnalogAccelerometer::AnalogAccelerometer; + + public: + /** + * Get the Acceleration for the PID Source parent. + * + * @return The current acceleration in Gs. + */ + double PIDGet() override; +}; + +} // namespace frc diff --git a/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogGyro.h b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogGyro.h new file mode 100644 index 0000000000..7c46156b38 --- /dev/null +++ b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogGyro.h @@ -0,0 +1,28 @@ +// 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. + +#pragma once + +#include "frc/AnalogGyro.h" +#include "frc/PIDSource.h" + +namespace frc { + +/** + * Wrapper so that PIDSource is implemented for AnalogGyro for old PIDController + */ +class PIDAnalogGyro : public PIDSource, public AnalogGyro { + using AnalogGyro::AnalogGyro; + + public: + /** + * Get the PIDOutput for the PIDSource base object. Can be set to return + * angle or rate using SetPIDSourceType(). Defaults to angle. + * + * @return The PIDOutput (angle or rate, defaults to angle) + */ + double PIDGet() override; +}; + +} // namespace frc diff --git a/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogInput.h b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogInput.h new file mode 100644 index 0000000000..b7a293fcc6 --- /dev/null +++ b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogInput.h @@ -0,0 +1,28 @@ +// 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. + +#pragma once + +#include "frc/AnalogInput.h" +#include "frc/PIDSource.h" + +namespace frc { + +/** + * Wrapper so that PIDSource is implemented for AnalogInput for old + * PIDController + */ +class PIDAnalogInput : public PIDSource, public AnalogInput { + using AnalogInput::AnalogInput; + + public: + /** + * Get the Average value for the PID Source base object. + * + * @return The average voltage. + */ + double PIDGet() override; +}; + +} // namespace frc diff --git a/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogPotentiometer.h b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogPotentiometer.h new file mode 100644 index 0000000000..c857ccb8b9 --- /dev/null +++ b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogPotentiometer.h @@ -0,0 +1,28 @@ +// 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. + +#pragma once + +#include "frc/AnalogPotentiometer.h" +#include "frc/PIDSource.h" + +namespace frc { + +/** + * Wrapper so that PIDSource is implemented for AnalogPotentiometer for old + * PIDController + */ +class PIDAnalogPotentiometer : public PIDSource, public AnalogPotentiometer { + using AnalogPotentiometer::AnalogPotentiometer; + + public: + /** + * Implement the PIDSource interface. + * + * @return The current reading. + */ + double PIDGet() override; +}; + +} // namespace frc diff --git a/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDEncoder.h b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDEncoder.h new file mode 100644 index 0000000000..2c21118d89 --- /dev/null +++ b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDEncoder.h @@ -0,0 +1,28 @@ +// 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. + +#pragma once + +#include "frc/Encoder.h" +#include "frc/PIDSource.h" + +namespace frc { + +/** + * Wrapper so that PIDSource is implemented for Encoder for old PIDController + */ +class PIDEncoder : public PIDSource, public Encoder { + using Encoder::Encoder; + + public: + /** + * Get the PIDOutput for the PIDSource base object. Can be set to return + * distance or rate using SetPIDSourceType(). Defaults to distance. + * + * @return The PIDOutput (distance or rate, defaults to distance) + */ + double PIDGet() override; +}; + +} // namespace frc diff --git a/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDMotorController.h b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDMotorController.h new file mode 100644 index 0000000000..c764c7fa69 --- /dev/null +++ b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDMotorController.h @@ -0,0 +1,63 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/PIDOutput.h" +#include "frc/motorcontrol/MotorController.h" + +namespace frc { + +/** + * Wrapper so that PIDOutput is implemented for MotorController for old + * PIDController + */ +class PIDMotorController : public PIDOutput, + public MotorController, + public wpi::Sendable { + public: + explicit PIDMotorController(MotorController& motorController); + + /** + * Set the PWM value. + * + * The PWM value is set using a range of -1.0 to 1.0, appropriately scaling + * the value for the FPGA. + * + * @param value The speed value between -1.0 and 1.0 to set. + */ + void Set(double value) override; + + /** + * Get the recently set value of the PWM. This value is affected by the + * inversion property. If you want the value that is sent directly to the + * MotorController, use PWM::GetSpeed() instead. + * + * @return The most recently set value for the PWM between -1.0 and 1.0. + */ + double Get() const override; + + void SetInverted(bool isInverted) override; + + bool GetInverted() const override; + + void Disable() override; + + void StopMotor() override; + + void PIDWrite(double output) override; + + protected: + void InitSendable(wpi::SendableBuilder& builder) override; + + private: + bool m_isInverted = false; + + MotorController& m_motorController; +}; + +} // namespace frc diff --git a/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDUltrasonic.h b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDUltrasonic.h new file mode 100644 index 0000000000..a26491e748 --- /dev/null +++ b/wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDUltrasonic.h @@ -0,0 +1,27 @@ +// 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. + +#pragma once + +#include "frc/PIDSource.h" +#include "frc/Ultrasonic.h" + +namespace frc { + +/** + * Wrapper so that PIDSource is implemented for Ultrasonic for old PIDController + */ +class PIDUltrasonic : public PIDSource, public Ultrasonic { + using Ultrasonic::Ultrasonic; + + public: + /** + * Get the range for the PIDSource base object. + * + * @return The range + */ + double PIDGet() override; +}; + +} // namespace frc