From 80a06d78f03caabd88009646b74cbef44b58e8b6 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 2 Aug 2023 20:50:46 -0700 Subject: [PATCH] [wpilib] Add class to calculate robot resistance Co-authored-by: ysthakur <45539777+ysthakur@users.noreply.github.com> --- .../src/main/native/cpp/PowerDistribution.cpp | 42 ++++- .../main/native/cpp/ResistanceCalculator.cpp | 55 ++++++ .../native/include/frc/PowerDistribution.h | 28 ++- .../native/include/frc/ResistanceCalculator.h | 160 ++++++++++++++++ .../native/cpp/ResistanceCalculatorTest.cpp | 33 ++++ .../cpp/examples/Resistance/cpp/Robot.cpp | 63 +++++++ .../src/main/cpp/examples/examples.json | 9 + .../wpi/first/wpilibj/PowerDistribution.java | 48 ++++- .../first/wpilibj/ResistanceCalculator.java | 178 ++++++++++++++++++ .../wpilibj/ResistanceCalculatorTest.java | 37 ++++ .../wpi/first/wpilibj/examples/examples.json | 9 + .../wpilibj/examples/resistance/Main.java | 25 +++ .../wpilibj/examples/resistance/Robot.java | 62 ++++++ 13 files changed, 737 insertions(+), 12 deletions(-) create mode 100644 wpilibc/src/main/native/cpp/ResistanceCalculator.cpp create mode 100644 wpilibc/src/main/native/include/frc/ResistanceCalculator.h create mode 100644 wpilibc/src/test/native/cpp/ResistanceCalculatorTest.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/Resistance/cpp/Robot.cpp create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/ResistanceCalculator.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/ResistanceCalculatorTest.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/resistance/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/resistance/Robot.java diff --git a/wpilibc/src/main/native/cpp/PowerDistribution.cpp b/wpilibc/src/main/native/cpp/PowerDistribution.cpp index 14a4cec49d8..dc0a38a3a3d 100644 --- a/wpilibc/src/main/native/cpp/PowerDistribution.cpp +++ b/wpilibc/src/main/native/cpp/PowerDistribution.cpp @@ -10,11 +10,14 @@ #include #include #include +#include +#include #include #include #include #include "frc/Errors.h" +#include "frc/ResistanceCalculator.h" static_assert(static_cast( frc::PowerDistribution::ModuleType::kCTRE) == @@ -27,7 +30,8 @@ static_assert(frc::PowerDistribution::kDefaultModule == using namespace frc; -PowerDistribution::PowerDistribution() { +PowerDistribution::PowerDistribution() + : m_totalResistanceNotifier([this] { UpdateResistance(); }) { auto stack = wpi::GetStackTrace(1); int32_t status = 0; @@ -41,9 +45,12 @@ PowerDistribution::PowerDistribution() { HAL_Report(HALUsageReporting::kResourceType_PDP, m_module + 1); wpi::SendableRegistry::AddLW(this, "PowerDistribution", m_module); + + m_totalResistanceNotifier.StartPeriodic(PowerDistribution::kUpdatePeriod); } -PowerDistribution::PowerDistribution(int module, ModuleType moduleType) { +PowerDistribution::PowerDistribution(int module, ModuleType moduleType) + : m_totalResistanceNotifier([this] { UpdateResistance(); }) { auto stack = wpi::GetStackTrace(1); int32_t status = 0; @@ -56,6 +63,25 @@ PowerDistribution::PowerDistribution(int module, ModuleType moduleType) { HAL_Report(HALUsageReporting::kResourceType_PDP, m_module + 1); wpi::SendableRegistry::AddLW(this, "PowerDistribution", m_module); + + m_totalResistanceNotifier.StartPeriodic(PowerDistribution::kUpdatePeriod); +} + +PowerDistribution::PowerDistribution(PowerDistribution&& other) + : m_handle(std::move(other.m_handle)), + m_module(other.m_module), + m_totalResistanceCalculator(std::move(other.m_totalResistanceCalculator)), + m_totalResistanceNotifier(std::move(other.m_totalResistanceNotifier)) { + m_totalResistance.store(other.m_totalResistance.load()); +} + +PowerDistribution& PowerDistribution::operator=(PowerDistribution&& other) { + m_handle = std::move(other.m_handle); + m_module = other.m_module; + m_totalResistanceCalculator = std::move(other.m_totalResistanceCalculator); + m_totalResistanceNotifier = std::move(other.m_totalResistanceNotifier); + m_totalResistance.store(other.m_totalResistance.load()); + return *this; } int PowerDistribution::GetNumChannels() const { @@ -310,6 +336,10 @@ PowerDistribution::StickyFaults PowerDistribution::GetStickyFaults() const { return stickyFaults; } +units::ohm_t PowerDistribution::GetTotalResistance() const { + return m_totalResistance.load(); +} + void PowerDistribution::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("PowerDistribution"); int numChannels = GetNumChannels(); @@ -347,4 +377,12 @@ void PowerDistribution::InitSendable(wpi::SendableBuilder& builder) { int32_t lStatus = 0; HAL_SetPowerDistributionSwitchableChannel(m_handle, value, &lStatus); }); + builder.AddDoubleProperty( + "TotalResistance", [this] { return GetTotalResistance().value(); }, + nullptr); +} + +void PowerDistribution::UpdateResistance() { + m_totalResistance.store(m_totalResistanceCalculator.Calculate( + units::ampere_t(GetTotalCurrent()), units::volt_t(GetVoltage()))); } diff --git a/wpilibc/src/main/native/cpp/ResistanceCalculator.cpp b/wpilibc/src/main/native/cpp/ResistanceCalculator.cpp new file mode 100644 index 00000000000..fe8109ae187 --- /dev/null +++ b/wpilibc/src/main/native/cpp/ResistanceCalculator.cpp @@ -0,0 +1,55 @@ +// 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/ResistanceCalculator.h" + +#include + +using namespace frc; + +ResistanceCalculator::ResistanceCalculator(int bufferSize, + double rSquaredThreshold) + : m_currentBuffer(bufferSize), + m_voltageBuffer(bufferSize), + m_bufferSize{bufferSize}, + m_rSquaredThreshold{rSquaredThreshold} {} + +units::ohm_t ResistanceCalculator::Calculate(units::ampere_t current, + units::volt_t voltage) { + // Update buffers only if drawing current + if (current != 0_A) { + if (m_currentBuffer.size() >= static_cast(m_bufferSize)) { + // Pop the last point and remove it from the sums + units::ampere_t lastCurrent = m_currentBuffer.pop_back(); + units::volt_t lastVoltage = m_voltageBuffer.pop_back(); + m_currentVariance.Calculate(lastCurrent, lastCurrent, Operator::kRemove); + m_voltageVariance.Calculate(lastVoltage, lastVoltage, Operator::kRemove); + m_covariance.Calculate(lastCurrent, lastVoltage, Operator::kRemove); + } + + m_currentBuffer.push_front(current); + m_voltageBuffer.push_front(voltage); + m_currentVariance.Calculate(current, current, Operator::kAdd); + m_voltageVariance.Calculate(voltage, voltage, Operator::kAdd); + m_covariance.Calculate(current, voltage, Operator::kAdd); + } + + // Recalculate resistance + if (m_currentBuffer.size() < 2) { + return units::ohm_t{std::nan("")}; + } + + auto currentVariance = m_currentVariance.GetCovariance(); + auto voltageVariance = m_voltageVariance.GetCovariance(); + auto covariance = m_covariance.GetCovariance(); + double rSquared = + covariance * covariance / (currentVariance * voltageVariance); + + if (rSquared > m_rSquaredThreshold) { + // Resistance is slope of current vs voltage + return covariance / currentVariance; + } else { + return units::ohm_t{std::nan("")}; + } +} diff --git a/wpilibc/src/main/native/include/frc/PowerDistribution.h b/wpilibc/src/main/native/include/frc/PowerDistribution.h index 8d647af542d..463bb6b2ee4 100644 --- a/wpilibc/src/main/native/include/frc/PowerDistribution.h +++ b/wpilibc/src/main/native/include/frc/PowerDistribution.h @@ -4,13 +4,19 @@ #pragma once +#include #include #include #include +#include +#include #include #include +#include "frc/Notifier.h" +#include "frc/ResistanceCalculator.h" + namespace frc { /** @@ -23,6 +29,9 @@ class PowerDistribution : public wpi::Sendable, /// Default module number. static constexpr int kDefaultModule = -1; + /// Seconds to wait before updating resistance. + static constexpr units::second_t kUpdatePeriod = 25_ms; + /** * Power distribution module type. */ @@ -49,8 +58,8 @@ class PowerDistribution : public wpi::Sendable, */ PowerDistribution(int module, ModuleType moduleType); - PowerDistribution(PowerDistribution&&) = default; - PowerDistribution& operator=(PowerDistribution&&) = default; + PowerDistribution(PowerDistribution&&); + PowerDistribution& operator=(PowerDistribution&&); ~PowerDistribution() override = default; @@ -157,6 +166,13 @@ class PowerDistribution : public wpi::Sendable, */ void SetSwitchableChannel(bool enabled); + /** + * Get the robot's total resistance. + * + * @return The robot total resistance, in Ohms. + */ + units::ohm_t GetTotalResistance() const; + /** Version and device data received from a PowerDistribution device */ struct Version { /** Firmware major version number. */ @@ -345,6 +361,14 @@ class PowerDistribution : public wpi::Sendable, private: hal::Handle m_handle; int m_module; + ResistanceCalculator m_totalResistanceCalculator; + std::atomic m_totalResistance; + frc::Notifier m_totalResistanceNotifier; + + /** + * Calculate new total resistance. + */ + void UpdateResistance(); }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/ResistanceCalculator.h b/wpilibc/src/main/native/include/frc/ResistanceCalculator.h new file mode 100644 index 00000000000..c53e7f3bc56 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/ResistanceCalculator.h @@ -0,0 +1,160 @@ +// 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 +#include +#include + +namespace frc { + +/** + * Finds the resistance of a channel or the entire robot using a running linear + * regression over a window. + * + * Must be updated with current and voltage periodically using the Calculate() + * method. + * + * To use this for finding the resistance of a channel, use the calculate method + * with the battery voltage minus the voltage at the motor controller or + * whatever is plugged in to the PDP at that channel. + */ +class ResistanceCalculator { + public: + /// Default buffer size. + static constexpr int kDefaultBufferSize = 250; + + /// Default R² threshold. + static constexpr double kDefaultRSquaredThreshold = 0.75; + + /** + * Creates a ResistanceCalculator with a default buffer size of 250 and R² + * threshold of 0.5. + */ + ResistanceCalculator() = default; + + /** + * Creates a ResistanceCalculator. + * + * @param bufferSize The maximum number of points to take the linear + * regression over. + * @param rSquaredThreshold The minimum R² value (0 to 1) considered + * significant enough to return the regression slope instead of NaN. A + * lower threshold allows resistance to be returned even with noisier + * data. + */ + ResistanceCalculator(int bufferSize, double rSquaredThreshold); + + ResistanceCalculator(ResistanceCalculator&&) = default; + ResistanceCalculator& operator=(ResistanceCalculator&&) = default; + + /** + * Update the buffers with new (current, voltage) points, and remove old + * points if necessary. + * + * @param current The current current + * @param voltage The current voltage + * @return The current resistance in ohms + */ + units::ohm_t Calculate(units::ampere_t current, units::volt_t voltage); + + private: + /** + * Operator to apply to OnlineCovariance<>::Calculate() inputs. + */ + enum class Operator { + /// Add point. + kAdd, + /// Remove point. + kRemove + }; + + template + class OnlineCovariance { + public: + /** + * The previously calculated covariance. + */ + decltype(auto) GetCovariance() const { return m_cov / (m_n - 1); } + + /** + * Calculate the covariance based on a new point that may be removed or + * added. + * + * @param x The x value of the point. + * @param y The y value of the point. + * @param op Operator to apply with the point. + * @return The new sample covariance. + */ + decltype(auto) Calculate(X x, Y y, Operator op) { + // From + // https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Covariance + + X dx = x - m_xMean; + Y dy = y - m_yMean; + + if (op == Operator::kAdd) { + ++m_n; + m_xMean += dx / m_n; + m_yMean += dy / m_n; + + // This is supposed to be (y - yMean) and not dy + m_cov += dx * (y - m_yMean); + } else if (op == Operator::kRemove) { + --m_n; + m_xMean -= dx / m_n; + m_yMean -= dy / m_n; + + // This is supposed to be (y - yMean) and not dy + m_cov -= dx * (y - m_yMean); + } + + // Correction for sample variance + return m_cov / (m_n - 1); + } + + private: + /// Number of points covariance is calculated over. + int m_n = 0; + + /// Current mean of x values. + X m_xMean{0.0}; + + /// Current mean of y values. + Y m_yMean{0.0}; + + /// Current approximated population covariance. + decltype(std::declval() * std::declval()) m_cov{0.0}; + }; + + /// Buffers holding the current values that will eventually need to be + /// subtracted from the sum when they leave the window. + wpi::circular_buffer m_currentBuffer{kDefaultBufferSize}; + + /// Buffer holding the voltage values that will eventually need to be + /// subtracted from the sum when they leave the window. + wpi::circular_buffer m_voltageBuffer{kDefaultBufferSize}; + + /// The maximum number of points to take the linear regression over. + int m_bufferSize = kDefaultBufferSize; + + /// The minimum R² value considered significant enough to return the + /// regression slope instead of NaN. + double m_rSquaredThreshold = kDefaultRSquaredThreshold; + + /// Used for approximating current variance. + OnlineCovariance m_currentVariance; + + /// Used for approximating voltage variance. + OnlineCovariance m_voltageVariance; + + /// Used for approximating covariance of current and voltage. + OnlineCovariance m_covariance; +}; + +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/ResistanceCalculatorTest.cpp b/wpilibc/src/test/native/cpp/ResistanceCalculatorTest.cpp new file mode 100644 index 00000000000..e8e56896998 --- /dev/null +++ b/wpilibc/src/test/native/cpp/ResistanceCalculatorTest.cpp @@ -0,0 +1,33 @@ +// 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 + +#include + +#include "frc/ResistanceCalculator.h" + +TEST(ResistanceCalculatorTest, EdgeCase) { + frc::ResistanceCalculator calc; + + // Make sure it doesn't try to do a linear regression with only 1 point + EXPECT_TRUE(std::isnan(calc.Calculate(1_A, 1_V).value())); + + // Make sure points with current 0 are ignored + EXPECT_TRUE(std::isnan(calc.Calculate(0_A, 1_V).value())); + EXPECT_TRUE(std::isnan(calc.Calculate(0_A, 1_V).value())); +} + +TEST(ResistanceCalculatorTest, ResistanceCalculation) { + constexpr double tolerance = 0.5; + frc::ResistanceCalculator calc; + + calc.Calculate(1_A, 1_V); + EXPECT_NEAR(1.1282, calc.Calculate(40_A, 45_V).value(), tolerance); + EXPECT_NEAR(1.0361, calc.Calculate(50_A, 50_V).value(), tolerance); + EXPECT_NEAR(0.7832, calc.Calculate(60_A, 40_V).value(), tolerance); + + // R² should be too low here + EXPECT_TRUE(std::isnan(calc.Calculate(100_A, 0_V).value())); +} diff --git a/wpilibcExamples/src/main/cpp/examples/Resistance/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Resistance/cpp/Robot.cpp new file mode 100644 index 00000000000..cf802aa0abf --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Resistance/cpp/Robot.cpp @@ -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. + +#include +#include +#include +#include +#include +#include + +/** + * Sample program to demonstrate logging the resistance of a particular PDP/PDH + * channel and the robot's resistance. The resistance can be calculated by + * recording the current and voltage of a particular channel and doing a linear + * regression on that data. + */ +class Robot : public frc::TimedRobot { + public: + Robot() { + // Display the PowerDistribution on the dashboard so that the robot + // resistance can be seen. + frc::SmartDashboard::PutData(&m_powerDistribution); + } + + void TeleopPeriodic() override { + // Get the current for channel 1 + units::ampere_t chan1Current(m_powerDistribution.GetCurrent(kChannel)); + // Get the voltage given to the motor plugged into channel 1 + units::volt_t chan1Voltage(m_motor.Get() * + frc::RobotController::GetBatteryVoltage()); + + // Calculate the channel's resistance based on that current and voltage + units::ohm_t resistance = + m_resistCalc.Calculate(chan1Current, chan1Voltage); + + // Log the resistance + frc::SmartDashboard::PutNumber("Channel 1 resistance", resistance()); + } + + private: + /** The channel on the PDP whose resistance will be logged in this example. */ + static const int kChannel = 1; + + /** + * Object representing the PDP or PDH on the robot. The PowerDistribution + * class implements Sendable and logs the robot resistance. It can also be + * used to get the current flowing through a particular channel. + */ + frc::PowerDistribution m_powerDistribution; + + /** Used to calculate the resistance of channel 1. */ + frc::ResistanceCalculator m_resistCalc; + + /** The motor plugged into channel 1. */ + frc::PWMSparkMax m_motor{0}; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index ef53f478c0e..3274a78a338 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -812,5 +812,14 @@ "foldername": "SysId", "gradlebase": "cpp", "commandversion": 2 + }, + { + "name": "Resistance", + "description": "Demonstrates the use of a ResistanceCalculator to calculate a channel's resistance.", + "tags": [], + "foldername": "Resistance", + "gradlebase": "cpp", + "mainclass": "Main", + "commandversion": 2 } ] diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java index 2325fe43422..18119baf148 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java @@ -13,18 +13,25 @@ import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; +import java.util.concurrent.atomic.AtomicReference; /** - * Class for getting voltage, current, temperature, power and energy from the CTRE Power - * Distribution Panel (PDP) or REV Power Distribution Hub (PDH) over CAN. + * Class for getting voltage, current, temperature, power, resistance, and energy from the CTRE + * Power Distribution Panel (PDP) or REV Power Distribution Hub (PDH) over CAN. */ public class PowerDistribution implements Sendable, AutoCloseable { private final int m_handle; private final int m_module; + private final ResistanceCalculator m_totalResistanceCalculator; + private final AtomicReference m_totalResistance = new AtomicReference<>(Double.NaN); + private final Notifier m_resistanceLoop = new Notifier(this::updateResistance); /** Default module number. */ public static final int kDefaultModule = PowerDistributionJNI.DEFAULT_MODULE; + /** Seconds to wait before updating resistance. */ + public static final double kUpdatePeriod = 0.025; + /** Power distribution module type. */ public enum ModuleType { /** CTRE (Cross The Road Electronics) Power Distribution Panel (PDP). */ @@ -48,11 +55,7 @@ public enum ModuleType { */ @SuppressWarnings("this-escape") public PowerDistribution(int module, ModuleType moduleType) { - m_handle = PowerDistributionJNI.initialize(module, moduleType.value); - m_module = PowerDistributionJNI.getModuleNumber(m_handle); - - HAL.report(tResourceType.kResourceType_PDP, m_module + 1); - SendableRegistry.addLW(this, "PowerDistribution", m_module); + this(module, moduleType.value); } /** @@ -62,9 +65,22 @@ public PowerDistribution(int module, ModuleType moduleType) { */ @SuppressWarnings("this-escape") public PowerDistribution() { - m_handle = PowerDistributionJNI.initialize(kDefaultModule, PowerDistributionJNI.AUTOMATIC_TYPE); + this(kDefaultModule, PowerDistributionJNI.AUTOMATIC_TYPE); + } + + /** + * Common constructor to construct a PowerDistribution object. + * + * @param module The CAN ID of the PDP/PDH. + * @param moduleType Module type as an integer (CTRE or REV). + */ + private PowerDistribution(int module, int moduleType) { + m_handle = PowerDistributionJNI.initialize(module, moduleType); m_module = PowerDistributionJNI.getModuleNumber(m_handle); + m_totalResistanceCalculator = new ResistanceCalculator(); + m_resistanceLoop.startPeriodic(PowerDistribution.kUpdatePeriod); + HAL.report(tResourceType.kResourceType_PDP, m_module + 1); SendableRegistry.addLW(this, "PowerDistribution", m_module); } @@ -241,6 +257,21 @@ public PowerDistributionStickyFaults getStickyFaults() { return PowerDistributionJNI.getStickyFaults(m_handle); } + /** + * Get the robot's resistance. + * + * @return The robot's resistance. + */ + public double getTotalResistance() { + return m_totalResistance.get(); + } + + /** Calculate new total resistance. */ + private void updateResistance() { + m_totalResistance.set( + m_totalResistanceCalculator.calculate(this.getTotalCurrent(), this.getVoltage())); + } + @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("PowerDistribution"); @@ -258,5 +289,6 @@ public void initSendable(SendableBuilder builder) { "SwitchableChannel", () -> PowerDistributionJNI.getSwitchableChannelNoError(m_handle), value -> PowerDistributionJNI.setSwitchableChannel(m_handle, value)); + builder.addDoubleProperty("TotalResistance", this::getTotalResistance, null); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ResistanceCalculator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ResistanceCalculator.java new file mode 100644 index 00000000000..1587177e510 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ResistanceCalculator.java @@ -0,0 +1,178 @@ +// 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; + +import edu.wpi.first.util.DoubleCircularBuffer; + +/** + * Finds the resistance of a channel using a running linear regression over a window. + * + *

Must be updated with current and voltage periodically using the {@link + * ResistanceCalculator#calculate(double, double) calculate} method. + * + *

To use this for finding the resistance of a channel, use the calculate method with the battery + * voltage minus the voltage at the motor controller or whatever is plugged in to the PDP at that + * channel. + */ +public final class ResistanceCalculator { + /** Default buffer size. */ + public static final int kDefaultBufferSize = 250; + + /** Default R² threshold. */ + public static final double kDefaultRSquaredThreshold = 0.75; + + /** Buffer holding previous current values. */ + private final DoubleCircularBuffer m_currentBuffer; + + /** Buffer holding previous voltage values. */ + private final DoubleCircularBuffer m_voltageBuffer; + + /** The maximum number of points to take the linear regression over. */ + private final int m_bufferSize; + + /** + * The minimum R² value considered significant enough to return the regression slope instead of + * NaN. + */ + private final double m_rSquaredThreshold; + + /** Used for approximating current variance. */ + private final OnlineCovariance m_currentVariance; + + /** Used for approximating voltage variance. */ + private final OnlineCovariance m_voltageVariance; + + /** Used for approximating covariance of current and voltage. */ + private final OnlineCovariance m_covariance; + + /** Creates a ResistanceCalculator with a default buffer size of 250 and R² threshold of 0.5. */ + public ResistanceCalculator() { + this(kDefaultBufferSize, kDefaultRSquaredThreshold); + } + + /** + * Creates a {@code ResistanceCalculator}. + * + * @param bufferSize The maximum number of points to take the linear regression over. + * @param rSquaredThreshold The minimum R² value considered significant enough to return the + * regression slope instead of NaN. + */ + @SuppressWarnings("ParameterName") + public ResistanceCalculator(int bufferSize, double rSquaredThreshold) { + this.m_rSquaredThreshold = rSquaredThreshold; + this.m_bufferSize = bufferSize; + this.m_currentBuffer = new DoubleCircularBuffer(bufferSize); + this.m_voltageBuffer = new DoubleCircularBuffer(bufferSize); + this.m_currentVariance = new OnlineCovariance(); + this.m_voltageVariance = new OnlineCovariance(); + this.m_covariance = new OnlineCovariance(); + } + + /** + * Recalculates resistance given a new current and voltage. The linear regression is only updated + * if current is nonzero. + * + * @param current The current current, in amperes. + * @param voltage The current voltage, in volts. + * @return The current resistance, in ohms. NaN if fewer than 2 points have been added. + */ + @SuppressWarnings("LocalVariableName") + public double calculate(double current, double voltage) { + if (current != 0) { + if (m_currentBuffer.size() >= m_bufferSize) { + double lastCurrent = m_currentBuffer.removeLast(); + double lastVoltage = m_voltageBuffer.removeLast(); + m_currentVariance.calculate(lastCurrent, lastCurrent, Operator.kRemove); + m_voltageVariance.calculate(lastVoltage, lastVoltage, Operator.kRemove); + m_covariance.calculate(lastCurrent, lastVoltage, Operator.kRemove); + } + + m_currentBuffer.addFirst(current); + m_voltageBuffer.addFirst(voltage); + m_currentVariance.calculate(current, current, Operator.kAdd); + m_voltageVariance.calculate(voltage, voltage, Operator.kAdd); + m_covariance.calculate(current, voltage, Operator.kAdd); + } + + if (m_currentBuffer.size() < 2) { + return Double.NaN; + } + + // Recalculate resistance + double currentVariance = m_currentVariance.getCovariance(); + double voltageVariance = m_voltageVariance.getCovariance(); + double covariance = m_covariance.getCovariance(); + double rSquared = covariance * covariance / (currentVariance * voltageVariance); + + if (rSquared > m_rSquaredThreshold) { + // Resistance is slope of current vs voltage + return covariance / currentVariance; + } else { + return Double.NaN; + } + } + + /** Operator to apply with OnlineCovariance.Calculate() inputs. */ + public enum Operator { + /** Add point. */ + kAdd, + /** Remove point. */ + kRemove + } + + /** A helper that approximates covariance incrementally. */ + private static final class OnlineCovariance { + /** Number of points covariance is calculated over. */ + private int m_n; + + /** Current mean of x values. */ + private double m_xMean; + + /** Current mean of y values. */ + private double m_yMean; + + /** Current approximated population covariance. */ + private double m_cov; + + /** The previously calculated covariance. */ + public double getCovariance() { + return m_cov / (m_n - 1); + } + + /** + * Calculate the covariance based on a new point that may be removed or added. + * + * @param x The x value of the point. + * @param y The y value of the point. + * @param op Operator to apply with the point. + * @return The new sample covariance. + */ + public double calculate(double x, double y, Operator op) { + // From https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Covariance + + double dx = x - m_xMean; + double dy = y - m_yMean; + + if (op == Operator.kAdd) { + ++m_n; + m_xMean += dx / m_n; + m_yMean += dy / m_n; + + // This is supposed to be (y - yMean) and not dy + m_cov += dx * (y - m_yMean); + } else if (op == Operator.kRemove) { + --m_n; + m_xMean -= dx / m_n; + m_yMean -= dy / m_n; + + // This is supposed to be (y - yMean) and not dy + m_cov -= dx * (y - m_yMean); + } + + // Correction for sample variance + return m_cov / (m_n - 1); + } + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/ResistanceCalculatorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/ResistanceCalculatorTest.java new file mode 100644 index 00000000000..47763a68554 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/ResistanceCalculatorTest.java @@ -0,0 +1,37 @@ +// 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; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +class ResistanceCalculatorTest { + @Test + void edgeCaseTest() { + var calc = new ResistanceCalculator(); + + // Make sure it doesn't try to do a linear regression with only 1 point + assertEquals(Double.NaN, calc.calculate(1.0, 1.0)); + + // Make sure points with current 0 are ignored + assertEquals(Double.NaN, calc.calculate(0.0, 1.0)); + assertEquals(Double.NaN, calc.calculate(0.0, 1.0)); + } + + @Test + void resistanceCalculationTest() { + final double tolerance = 0.5; + var calc = new ResistanceCalculator(); + + calc.calculate(1, 1); + assertEquals(1.1282, calc.calculate(40.0, 45.0), tolerance); + assertEquals(1.0361, calc.calculate(50.0, 50.0), tolerance); + assertEquals(0.7832, calc.calculate(60.0, 40.0), tolerance); + + // R² should be too low here + assertEquals(Double.NaN, calc.calculate(100.0, 0.0)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index 8a108ac3540..3872149bc02 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -776,6 +776,15 @@ "mainclass": "Main", "commandversion": 2 }, + { + "name": "Resistance", + "description": "Demonstrates the use of a ResistanceCalculator to calculate a channel's resistance.", + "tags": [], + "foldername": "resistance", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "RomiReference", "description": "An example command-based robot program that can be used with the Romi reference robot design.", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/resistance/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/resistance/Main.java new file mode 100644 index 00000000000..59793aeb153 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/resistance/Main.java @@ -0,0 +1,25 @@ +// 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.examples.resistance; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/resistance/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/resistance/Robot.java new file mode 100644 index 00000000000..d1602481e5a --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/resistance/Robot.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.examples.resistance; + +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.ResistanceCalculator; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Sample program to demonstrate logging the resistance of a particular PDP/PDH channel and the + * robot's resistance. The resistance can be calculated by recording the current and voltage of a + * particular channel and doing a linear regression on that data. + */ +public class Robot extends TimedRobot { + /** The channel on the PDP whose resistance will be logged in this example. */ + private static final int kChannel = 1; + + /** + * Object representing the PDP or PDH on the robot. The PowerDistribution class implements + * Sendable and logs the robot resistance. It can also be used to get the current flowing through + * a particular channel. + */ + private final PowerDistribution m_powerDistribution = new PowerDistribution(); + + /** Used to calculate the resistance of channel 1. */ + private final ResistanceCalculator m_resistCalc1 = new ResistanceCalculator(); + + /** Used to calculate the total resistance of the robot. */ + private final ResistanceCalculator m_resistCalcTotal = new ResistanceCalculator(); + + /** The motor plugged into channel 1. */ + private final MotorController m_motor = new PWMSparkMax(0); + + @Override + public void robotInit() { + // Display the PowerDistribution on the dashboard so that the robot + // resistance can be seen. + SmartDashboard.putData(m_powerDistribution); + } + + @Override + public void robotPeriodic() { + var chan1Current = m_powerDistribution.getCurrent(kChannel); + // Get the voltage given to the motor plugged into channel 1. + var chan1Voltage = m_motor.get() * RobotController.getBatteryVoltage(); + + var robotCurrent = m_powerDistribution.getTotalCurrent(); + var robotVoltage = m_powerDistribution.getVoltage(); + + // Calculate and log channel 1's resistance + SmartDashboard.putNumber( + "Channel 1 resistance", m_resistCalc1.calculate(chan1Current, chan1Voltage)); + SmartDashboard.putNumber( + "Robot resistance", m_resistCalcTotal.calculate(robotCurrent, robotVoltage)); + } +}