From 2cd9be413fa40259465e4fea9098556b3c4e3302 Mon Sep 17 00:00:00 2001 From: Starlight220 <53231611+Starlight220@users.noreply.github.com> Date: Mon, 9 Jan 2023 02:33:07 +0200 Subject: [PATCH] [wpilib, examples] Cleanup PotentiometerPID, Ultrasonic, UltrasonicPID examples (#4893) Fix C++ Ultrasonic to return correct units. --- wpilibc/src/main/native/cpp/Ultrasonic.cpp | 7 +- .../native/cpp/simulation/UltrasonicSim.cpp | 9 +- .../include/frc/simulation/UltrasonicSim.h | 18 +- .../src/test/native/cpp/UltrasonicTest.cpp | 19 +- .../examples/PotentiometerPID/cpp/Robot.cpp | 86 +++------ .../examples/PotentiometerPID/include/Robot.h | 55 ++++++ .../cpp/examples/Ultrasonic/cpp/Robot.cpp | 93 +++++----- .../cpp/examples/Ultrasonic/include/Robot.h | 25 +++ .../cpp/examples/UltrasonicPID/cpp/Robot.cpp | 68 ++----- .../examples/UltrasonicPID/include/Robot.h | 51 ++++++ .../src/main/cpp/examples/examples.json | 22 ++- .../cpp/PotentiometerPIDTest.cpp | 167 +++++++++++++++++ .../examples/PotentiometerPID/cpp/main.cpp | 17 ++ .../UltrasonicPID/cpp/UltrasonicPIDTest.cpp | 111 +++++++++++ .../cpp/examples/UltrasonicPID/cpp/main.cpp | 17 ++ .../edu/wpi/first/wpilibj/Ultrasonic.java | 21 ++- .../wpilibj/simulation/UltrasonicSim.java | 14 +- .../edu/wpi/first/wpilibj/UltrasonicTest.java | 22 +++ .../wpi/first/wpilibj/examples/examples.json | 17 +- .../examples/potentiometerpid/Robot.java | 64 +++---- .../wpilibj/examples/ultrasonic/Robot.java | 81 ++++---- .../wpilibj/examples/ultrasonicpid/Robot.java | 61 +++--- .../PotentiometerPIDTest.java | 173 ++++++++++++++++++ .../ultrasonicpid/UltrasonicPIDTest.java | 135 ++++++++++++++ 24 files changed, 1052 insertions(+), 301 deletions(-) create mode 100644 wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/Ultrasonic/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h create mode 100644 wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp create mode 100644 wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/main.cpp create mode 100644 wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp create mode 100644 wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/main.cpp create mode 100644 wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java create mode 100644 wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java diff --git a/wpilibc/src/main/native/cpp/Ultrasonic.cpp b/wpilibc/src/main/native/cpp/Ultrasonic.cpp index a034fc3630..ec4c9c3c14 100644 --- a/wpilibc/src/main/native/cpp/Ultrasonic.cpp +++ b/wpilibc/src/main/native/cpp/Ultrasonic.cpp @@ -85,10 +85,7 @@ int Ultrasonic::GetEchoChannel() const { } void Ultrasonic::Ping() { - if (m_automaticEnabled) { - throw FRC_MakeError(err::IncompatibleMode, - "cannot call Ping() in automatic mode"); - } + SetAutomaticMode(false); // turn off automatic round-robin if pinging // Reset the counter to zero (invalid data now) m_counter.Reset(); @@ -138,7 +135,7 @@ void Ultrasonic::SetAutomaticMode(bool enabling) { units::meter_t Ultrasonic::GetRange() const { if (IsRangeValid()) { if (m_simRange) { - return units::meter_t{m_simRange.Get()}; + return units::inch_t{m_simRange.Get()}; } return m_counter.GetPeriod() * kSpeedOfSound / 2.0; } else { diff --git a/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp b/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp index d2d1687faa..30a565039c 100644 --- a/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp @@ -9,8 +9,11 @@ using namespace frc::sim; -UltrasonicSim::UltrasonicSim(const frc::Ultrasonic& ultrasonic) { - frc::sim::SimDeviceSim deviceSim{"Ultrasonic", ultrasonic.GetEchoChannel()}; +UltrasonicSim::UltrasonicSim(const frc::Ultrasonic& ultrasonic) + : UltrasonicSim(0, ultrasonic.GetEchoChannel()) {} + +UltrasonicSim::UltrasonicSim(int ping, int echo) { + frc::sim::SimDeviceSim deviceSim{"Ultrasonic", echo}; m_simRangeValid = deviceSim.GetBoolean("Range Valid"); m_simRange = deviceSim.GetDouble("Range (in)"); } @@ -19,6 +22,6 @@ void UltrasonicSim::SetRangeValid(bool isValid) { m_simRangeValid.Set(isValid); } -void UltrasonicSim::SetRange(units::meter_t range) { +void UltrasonicSim::SetRange(units::inch_t range) { m_simRange.Set(range.value()); } diff --git a/wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h b/wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h index efa9b3700b..5db41f7f4d 100644 --- a/wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h @@ -14,16 +14,24 @@ class Ultrasonic; namespace sim { /** - * Class to control a simulated ADXRS450 gyroscope. + * Class to control a simulated {@link Ultrasonic}. */ class UltrasonicSim { public: /** - * Constructs from a ADXRS450_Gyro object. + * Constructor. * - * @param gyro ADXRS450_Gyro to simulate + * @param ultrasonic The real ultrasonic to simulate */ - explicit UltrasonicSim(const Ultrasonic& gyro); + explicit UltrasonicSim(const Ultrasonic& ultrasonic); + + /** + * Constructor. + * + * @param ping unused. + * @param echo the ultrasonic's echo channel. + */ + UltrasonicSim(int ping, int echo); /** * Sets if the range measurement is valid. @@ -37,7 +45,7 @@ class UltrasonicSim { * * @param range The range */ - void SetRange(units::meter_t range); + void SetRange(units::inch_t range); private: hal::SimBoolean m_simRangeValid; diff --git a/wpilibc/src/test/native/cpp/UltrasonicTest.cpp b/wpilibc/src/test/native/cpp/UltrasonicTest.cpp index 72d72d2b0d..0cc49ba9c9 100644 --- a/wpilibc/src/test/native/cpp/UltrasonicTest.cpp +++ b/wpilibc/src/test/native/cpp/UltrasonicTest.cpp @@ -6,7 +6,7 @@ #include "frc/simulation/UltrasonicSim.h" #include "gtest/gtest.h" -namespace frc { +using namespace frc; TEST(UltrasonicTest, SimDevices) { Ultrasonic ultrasonic{0, 1}; @@ -23,4 +23,19 @@ TEST(UltrasonicTest, SimDevices) { EXPECT_EQ(0, ultrasonic.GetRange().value()); } -} // namespace frc +TEST(UltrasonicTest, AutomaticModeToggle) { + frc::Ultrasonic ultrasonic{0, 1}; + EXPECT_NO_THROW({ + frc::Ultrasonic::SetAutomaticMode(true); + frc::Ultrasonic::SetAutomaticMode(false); + frc::Ultrasonic::SetAutomaticMode(true); + }); +} + +TEST(UltrasonicTest, AutomaticModeOnWithZeroInstances) { + EXPECT_NO_THROW({ frc::Ultrasonic::SetAutomaticMode(true); }); +} + +TEST(UltrasonicTest, AutomaticModeOffWithZeroInstances) { + EXPECT_NO_THROW({ frc::Ultrasonic::SetAutomaticMode(false); }); +} diff --git a/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp index 5b6b024ec1..69cebaf145 100644 --- a/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp @@ -2,72 +2,32 @@ // 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 "Robot.h" -#include -#include -#include -#include -#include +void Robot::TeleopInit() { + // Move to the bottom setpoint when teleop starts + m_index = 0; + m_pidController.SetSetpoint(kSetpoints[m_index].value()); +} -/** - * This is a sample program to demonstrate how to use a soft potentiometer and a - * PID Controller to reach and maintain position setpoints on an elevator - * mechanism. - */ -class Robot : public frc::TimedRobot { - public: - void RobotInit() override { - m_pidController.SetSetpoint(kSetPoints[m_index]); +void Robot::TeleopPeriodic() { + // Read from the sensor + units::meter_t position = units::meter_t{m_potentiometer.Get()}; + + // Run the PID Controller + double pidOut = m_pidController.Calculate(position.value()); + + // Apply PID output + m_elevatorMotor.Set(pidOut); + + // when the button is pressed once, the selected elevator setpoint is + // incremented + if (m_joystick.GetTriggerPressed()) { + // index of the elevator setpoint wraps around. + m_index = (m_index + 1) % kSetpoints.size(); + m_pidController.SetSetpoint(kSetpoints[m_index].value()); } - - void TeleopPeriodic() override { - // When the button is pressed once, the selected elevator setpoint is - // incremented. - bool currentButtonValue = m_joystick.GetTrigger(); - if (currentButtonValue && !m_previousButtonValue) { - // Index of the elevator setpoint wraps around - m_index = (m_index + 1) % (sizeof(kSetPoints) / 8); - m_pidController.SetSetpoint(kSetPoints[m_index]); - } - m_previousButtonValue = currentButtonValue; - - double output = - m_pidController.Calculate(m_potentiometer.GetAverageVoltage()); - m_elevatorMotor.Set(output); - } - - private: - static constexpr int kPotChannel = 1; - static constexpr int kMotorChannel = 7; - static constexpr int kJoystickChannel = 0; - - // Bottom, middle, and top elevator setpoints - static constexpr std::array kSetPoints = {{1.0, 2.6, 4.3}}; - - /* Proportional, integral, and derivative speed constants; motor inverted. - * - * DANGER: When tuning PID constants, high/inappropriate values for pGain, - * iGain, and dGain may cause dangerous, uncontrollable, or undesired - * behavior! - * - * These may need to be positive for a non-inverted motor. - */ - static constexpr double kP = -5.0; - static constexpr double kI = -0.02; - static constexpr double kD = -2.0; - - int m_index = 0; - bool m_previousButtonValue = false; - - frc::AnalogInput m_potentiometer{kPotChannel}; - frc::Joystick m_joystick{kJoystickChannel}; - frc::PWMSparkMax m_elevatorMotor{kMotorChannel}; - - frc2::PIDController m_pidController{kP, kI, kD}; -}; - -constexpr std::array Robot::kSetPoints; +} #ifndef RUNNING_FRC_TESTS int main() { diff --git a/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h new file mode 100644 index 0000000000..f0002bc78c --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h @@ -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. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +/** + * This is a sample program to demonstrate how to use a soft potentiometer and a + * PID controller to reach and maintain position setpoints on an elevator + * mechanism. + */ +class Robot : public frc::TimedRobot { + public: + void TeleopInit() override; + void TeleopPeriodic() override; + + static constexpr int kPotChannel = 1; + static constexpr int kMotorChannel = 7; + static constexpr int kJoystickChannel = 0; + + // The elevator can move 1.5 meters from top to bottom + static constexpr units::meter_t kFullHeight = 1.5_m; + + // Bottom, middle, and top elevator setpoints + static constexpr std::array kSetpoints = { + {0.2_m, 0.8_m, 1.4_m}}; + + private: + // proportional speed constant + // negative because applying positive voltage will bring us closer to the + // target + static constexpr double kP = 0.7; + // integral speed constant + static constexpr double kI = 0.35; + // derivative speed constant + static constexpr double kD = 0.25; + + // Scaling is handled internally + frc::AnalogPotentiometer m_potentiometer{kPotChannel, kFullHeight.value()}; + + frc::PWMSparkMax m_elevatorMotor{kMotorChannel}; + frc2::PIDController m_pidController{kP, kI, kD}; + frc::Joystick m_joystick{kJoystickChannel}; + + size_t m_index; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp index e8f805d8d0..ac47856ee2 100644 --- a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp @@ -2,57 +2,54 @@ // 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 "Robot.h" -/** - * This is a sample program demonstrating how to use an ultrasonic sensor and - * proportional control to maintain a set distance from an object. - */ -class Robot : public frc::TimedRobot { - public: - /** - * Tells the robot to drive to a set distance (in inches) from an object using - * proportional control. - */ - void TeleopPeriodic() override { - // Sensor returns a value from 0-4095 that is scaled to inches - // returned value is filtered with a rolling median filter, since - // ultrasonics tend to be quite noisy and susceptible to sudden outliers - double currentDistance = - m_filter.Calculate(m_ultrasonic.GetVoltage()) * kValueToInches; - // Convert distance error to a motor speed - double currentSpeed = (kHoldDistance - currentDistance) * kP; - // Drive robot - m_robotDrive.ArcadeDrive(currentSpeed, 0); +#include +#include +#include + +void Robot::RobotInit() { + // Add the ultrasonic on the "Sensors" tab of the dashboard + // Data will update automatically + frc::Shuffleboard::GetTab("Sensors").Add(m_rangeFinder); +} + +void Robot::TeleopPeriodic() { + // We can read the distance + units::meter_t distance = m_rangeFinder.GetRange(); + // units auto-convert + units::millimeter_t distanceMillimeters = distance; + units::inch_t distanceInches = distance; + + // We can also publish the data itself periodically + frc::SmartDashboard::PutNumber("Distance[mm]", distanceMillimeters.value()); + frc::SmartDashboard::PutNumber("Distance[inch]", distanceInches.value()); +} + +void Robot::TestInit() { + // By default, the Ultrasonic class polls all ultrasonic sensors in a + // round-robin to prevent them from interfering from one another. However, + // manual polling is also possible -- note that this disables automatic mode! + m_rangeFinder.Ping(); +} + +void Robot::TestPeriodic() { + if (m_rangeFinder.IsRangeValid()) { + // Data is valid, publish it + units::millimeter_t distanceMillimeters = m_rangeFinder.GetRange(); + units::inch_t distanceInches = m_rangeFinder.GetRange(); + frc::SmartDashboard::PutNumber("Distance[mm]", distanceMillimeters.value()); + frc::SmartDashboard::PutNumber("Distance[inch]", distanceInches.value()); + + // Ping for next measurement + m_rangeFinder.Ping(); } +} - private: - // Distance in inches the robot wants to stay from an object - static constexpr int kHoldDistance = 12; - - // Factor to convert sensor values to a distance in inches - static constexpr double kValueToInches = 0.125; - - // Proportional speed constant - static constexpr double kP = 0.05; - - static constexpr int kLeftMotorPort = 0; - static constexpr int kRightMotorPort = 1; - static constexpr int kUltrasonicPort = 0; - - // median filter to discard outliers; filters over 10 samples - frc::MedianFilter m_filter{10}; - - frc::AnalogInput m_ultrasonic{kUltrasonicPort}; - - frc::PWMSparkMax m_left{kLeftMotorPort}; - frc::PWMSparkMax m_right{kRightMotorPort}; - frc::DifferentialDrive m_robotDrive{m_left, m_right}; -}; +void Robot::TestExit() { + // Enable automatic mode + frc::Ultrasonic::SetAutomaticMode(true); +} #ifndef RUNNING_FRC_TESTS int main() { diff --git a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/include/Robot.h new file mode 100644 index 0000000000..2da0c6ffa5 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/include/Robot.h @@ -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. + +#pragma once + +#include +#include + +/** + * This is a sample program demonstrating how to read from a ping-response + * ultrasonic sensor with the {@link Ultrasonic class}. + */ +class Robot : public frc::TimedRobot { + public: + void RobotInit() override; + void TeleopPeriodic() override; + void TestInit() override; + void TestPeriodic() override; + void TestExit() override; + + private: + // Creates a ping-response Ultrasonic object on DIO 1 and 2. + frc::Ultrasonic m_rangeFinder{1, 2}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp index 62d4106d8d..9d2b5c2003 100644 --- a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp @@ -2,65 +2,21 @@ // 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 +#include "Robot.h" -/** - * This is a sample program demonstrating how to use an ultrasonic sensor and - * proportional control to maintain a set distance from an object. - */ -class Robot : public frc::TimedRobot { - public: - /** - * Drives the robot a set distance from an object using PID control and the - * ultrasonic sensor. - */ - void TeleopInit() override { - // Set setpoint of the PID Controller - m_pidController.SetSetpoint(kHoldDistance * kValueToInches); - } +void Robot::AutonomousInit() { + // Set setpoint of the pid controller + m_pidController.SetSetpoint(kHoldDistance.value()); +} - void TeleopPeriodic() override { - double output = - m_pidController.Calculate(m_filter.Calculate(m_ultrasonic.GetValue())); - m_robotDrive.ArcadeDrive(output, 0); - } +void Robot::AutonomousPeriodic() { + units::millimeter_t measurement = m_ultrasonic.GetRange(); + units::millimeter_t filteredMeasurement = m_filter.Calculate(measurement); + double pidOutput = m_pidController.Calculate(filteredMeasurement.value()); - private: - // Distance in inches the robot wants to stay from an object - static constexpr int kHoldDistance = 12; - - // Factor to convert sensor values to a distance in inches - static constexpr double kValueToInches = 0.125; - - // proportional speed constant - static constexpr double kP = 7.0; - - // integral speed constant - static constexpr double kI = 0.018; - - // derivative speed constant - static constexpr double kD = 1.5; - - static constexpr int kLeftMotorPort = 0; - static constexpr int kRightMotorPort = 1; - static constexpr int kUltrasonicPort = 0; - - // median filter to discard outliers; filters over 5 samples - frc::MedianFilter m_filter{5}; - - frc::AnalogInput m_ultrasonic{kUltrasonicPort}; - - frc::PWMSparkMax m_left{kLeftMotorPort}; - frc::PWMSparkMax m_right{kRightMotorPort}; - frc::DifferentialDrive m_robotDrive{m_left, m_right}; - - frc2::PIDController m_pidController{kP, kI, kD}; -}; + // disable input squaring -- PID output is linear + m_robotDrive.ArcadeDrive(pidOutput, 0, false); +} #ifndef RUNNING_FRC_TESTS int main() { diff --git a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h new file mode 100644 index 0000000000..d40f2b57ab --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h @@ -0,0 +1,51 @@ +// 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 +#include +#include + +/** + * This is a sample program to demonstrate the use of a PIDController with an + * ultrasonic sensor to reach and maintain a set distance from an object. + */ +class Robot : public frc::TimedRobot { + public: + void AutonomousInit() override; + void AutonomousPeriodic() override; + + // distance the robot wants to stay from an object + static constexpr units::millimeter_t kHoldDistance = 1_m; + + static constexpr int kLeftMotorPort = 0; + static constexpr int kRightMotorPort = 1; + static constexpr int kUltrasonicPingPort = 0; + static constexpr int kUltrasonicEchoPort = 1; + + private: + // proportional speed constant + // negative because applying positive voltage will bring us closer to the + // target + static constexpr double kP = -0.001; + // integral speed constant + static constexpr double kI = 0.0; + // derivative speed constant + static constexpr double kD = 0.0; + + // Ultrasonic sensors tend to be quite noisy and susceptible to sudden + // outliers, so measurements are filtered with a 5-sample median filter + frc::MedianFilter m_filter{5}; + + frc::Ultrasonic m_ultrasonic{kUltrasonicPingPort, kUltrasonicEchoPort}; + frc::PWMSparkMax m_left{kLeftMotorPort}; + frc::PWMSparkMax m_right{kRightMotorPort}; + frc::DifferentialDrive m_robotDrive{m_left, m_right}; + frc2::PIDController m_pidController{kP, kI, kD}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index aaec45def6..5f30ac0760 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -122,12 +122,13 @@ }, { "name": "Ultrasonic", - "description": "Demonstrate maintaining a set distance using an ultrasonic sensor.", + "description": "Demonstrate using the Ultrasonic class with a ping-response ultrasonic sensor.", "tags": [ - "Robot and Motor", - "Complete List", "Sensors", - "Analog" + "Hardware", + "Ultrasonic", + "SmartDashboard", + "Shuffleboard" ], "foldername": "Ultrasonic", "gradlebase": "cpp", @@ -135,12 +136,12 @@ }, { "name": "UltrasonicPID", - "description": "Demonstrate maintaining a set distance using an ultrasonic sensor and PID control.", + "description": "Demonstrate maintaining a set distance using an ultrasonic sensor and PID Control.", "tags": [ - "Robot and Motor", - "Complete List", "Sensors", - "Analog" + "Ultrasonic", + "PID", + "Differential Drive" ], "foldername": "UltrasonicPID", "gradlebase": "cpp", @@ -186,11 +187,12 @@ }, { "name": "PotentiometerPID", - "description": "An example to demonstrate the use of a potentiometer and PID control to reach elevator position setpoints.", + "description": "An example to demonstrate the use of a potentiometer and PID control to maintain elevator position setpoints.", "tags": [ "Joystick", "Actuators", - "Complete List", + "PID", + "Elevator", "Sensors", "Analog" ], diff --git a/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp b/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp new file mode 100644 index 0000000000..92b1affe7d --- /dev/null +++ b/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp @@ -0,0 +1,167 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Robot.h" + +class PotentiometerPIDTest : public testing::Test { + frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4); + static constexpr double kElevatorGearing = 10.0; + static constexpr units::meter_t kElevatorDrumRadius = 2.0_in; + static constexpr units::kilogram_t kCarriageMass = 4.0_kg; + + Robot m_robot; + std::optional m_thread; + + protected: + frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox, + kElevatorGearing, + kCarriageMass, + kElevatorDrumRadius, + 0.0_m, + Robot::kFullHeight, + true}; + frc::sim::PWMSim m_motorSim{Robot::kMotorChannel}; + frc::sim::AnalogInputSim m_analogSim{Robot::kPotChannel}; + frc::sim::JoystickSim m_joystickSim{Robot::kJoystickChannel}; + int32_t m_callback; + int32_t m_port; + + public: + void SimPeriodicBefore() { + m_elevatorSim.SetInputVoltage(m_motorSim.GetSpeed() * + frc::RobotController::GetBatteryVoltage()); + m_elevatorSim.Update(20_ms); + + /* + meters = (v / 5v) * range + meters / range = v / 5v + 5v * (meters / range) = v + */ + m_analogSim.SetVoltage( + (frc::RobotController::GetVoltage5V() * + (m_elevatorSim.GetPosition().value() / Robot::kFullHeight)) + .value()); + } + + static void CallSimPeriodicBefore(void* param) { + static_cast(param)->SimPeriodicBefore(); + } + + void SetUp() override { + frc::sim::PauseTiming(); + frc::sim::DriverStationSim::ResetData(); + + m_joystickSim.SetButtonCount(12); + + m_callback = + HALSIM_RegisterSimPeriodicBeforeCallback(CallSimPeriodicBefore, this); + + m_thread = std::thread([&] { m_robot.StartCompetition(); }); + frc::sim::StepTiming(0.0_ms); // Wait for Notifiers + } + + void TearDown() override { + m_robot.EndCompetition(); + m_thread->join(); + + HALSIM_CancelSimPeriodicBeforeCallback(m_callback); + m_analogSim.ResetData(); + m_motorSim.ResetData(); + } +}; + +TEST_F(PotentiometerPIDTest, Teleop) { + // teleop init + { + frc::sim::DriverStationSim::SetAutonomous(false); + frc::sim::DriverStationSim::SetEnabled(true); + frc::sim::DriverStationSim::NotifyNewData(); + + EXPECT_TRUE(m_motorSim.GetInitialized()); + EXPECT_TRUE(m_analogSim.GetInitialized()); + } + + // first setpoint + { + // advance 50 timesteps + frc::sim::StepTiming(1_s); + + EXPECT_NEAR(Robot::kSetpoints[0].value(), + m_elevatorSim.GetPosition().value(), 0.1); + } + + // second setpoint + { + // press button to advance setpoint + m_joystickSim.SetTrigger(true); + m_joystickSim.NotifyNewData(); + + // advance 50 timesteps + frc::sim::StepTiming(1_s); + + EXPECT_NEAR(Robot::kSetpoints[1].value(), + m_elevatorSim.GetPosition().value(), 0.1); + } + + // we need to unpress the button + { + m_joystickSim.SetTrigger(false); + m_joystickSim.NotifyNewData(); + + // advance 10 timesteps + frc::sim::StepTiming(0.2_s); + } + + // third setpoint + { + // press button to advance setpoint + m_joystickSim.SetTrigger(true); + m_joystickSim.NotifyNewData(); + + // advance 50 timesteps + frc::sim::StepTiming(1_s); + + EXPECT_NEAR(Robot::kSetpoints[2].value(), + m_elevatorSim.GetPosition().value(), 0.1); + } + + // we need to unpress the button + { + m_joystickSim.SetTrigger(false); + m_joystickSim.NotifyNewData(); + + // advance 10 timesteps + frc::sim::StepTiming(0.2_s); + } + + // rollover: first setpoint + { + // press button to advance setpoint + m_joystickSim.SetTrigger(true); + m_joystickSim.NotifyNewData(); + + // advance 60 timesteps + frc::sim::StepTiming(1.2_s); + EXPECT_NEAR(Robot::kSetpoints[0].value(), + m_elevatorSim.GetPosition().value(), 0.1); + } +} diff --git a/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/main.cpp b/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/main.cpp new file mode 100644 index 0000000000..285c1d5267 --- /dev/null +++ b/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/main.cpp @@ -0,0 +1,17 @@ +// 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 "gtest/gtest.h" + +/** + * Runs all unit tests. + */ +int main(int argc, char** argv) { + HAL_Initialize(500, 0); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp b/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp new file mode 100644 index 0000000000..47d3ac386d --- /dev/null +++ b/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp @@ -0,0 +1,111 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Robot.h" + +class UltrasonicPIDTest : public testing::TestWithParam { + frc::DCMotor m_gearbox = frc::DCMotor::Falcon500(2); + static constexpr auto kGearing = + frc::sim::DifferentialDrivetrainSim::KitbotGearing::k10p71; + static constexpr auto kvLinear = 1.98 * 1_V / 1_mps; + static constexpr auto kaLinear = 0.2 * 1_V / 1_mps_sq; + static constexpr auto kvVoltAngular = 1.5 * 1_V / 1_rad_per_s; + static constexpr auto kaAngular = 0.3 * 1_V / 1_rad_per_s_sq; + static constexpr auto kWheelDiameter = 0.15_m; + static constexpr auto kTrackwidth = 0.7_m; + + Robot m_robot; + std::optional m_thread; + + protected: + frc::sim::DifferentialDrivetrainSim m_driveSim{ + frc::LinearSystemId::IdentifyDrivetrainSystem( + kvLinear, kaLinear, kvVoltAngular, kaAngular, kTrackwidth), + kTrackwidth, m_gearbox, kGearing, kWheelDiameter / 2.0}; + frc::sim::PWMSim m_leftMotorSim{Robot::kLeftMotorPort}; + frc::sim::PWMSim m_rightMotorSim{Robot::kRightMotorPort}; + frc::sim::UltrasonicSim m_ultrasonicSim{Robot::kUltrasonicPingPort, + Robot::kUltrasonicEchoPort}; + int32_t m_callback; + + units::millimeter_t m_distance; + + public: + void SimPeriodicBefore() { + m_driveSim.SetInputs( + m_leftMotorSim.GetSpeed() * frc::RobotController::GetBatteryVoltage(), + m_rightMotorSim.GetSpeed() * frc::RobotController::GetBatteryVoltage()); + m_driveSim.Update(20_ms); + + auto startingDistance = units::meter_t{GetParam()}; + m_distance = startingDistance - m_driveSim.GetLeftPosition(); + + m_ultrasonicSim.SetRange(m_distance); + } + + static void CallSimPeriodicBefore(void* param) { + static_cast(param)->SimPeriodicBefore(); + } + + void SetUp() override { + frc::sim::PauseTiming(); + frc::sim::DriverStationSim::ResetData(); + + m_callback = + HALSIM_RegisterSimPeriodicBeforeCallback(CallSimPeriodicBefore, this); + + m_thread = std::thread([&] { m_robot.StartCompetition(); }); + frc::sim::StepTiming(0.0_ms); // Wait for Notifiers + } + + void TearDown() override { + m_robot.EndCompetition(); + m_thread->join(); + + HALSIM_CancelSimPeriodicBeforeCallback(m_callback); + m_leftMotorSim.ResetData(); + m_rightMotorSim.ResetData(); + } +}; + +TEST_P(UltrasonicPIDTest, Auto) { + // auto init + { + frc::sim::DriverStationSim::SetAutonomous(true); + frc::sim::DriverStationSim::SetEnabled(true); + frc::sim::DriverStationSim::NotifyNewData(); + + EXPECT_TRUE(m_leftMotorSim.GetInitialized()); + EXPECT_TRUE(m_rightMotorSim.GetInitialized()); + } + + { + // advance 100 timesteps + frc::sim::StepTiming(2_s); + + EXPECT_NEAR(Robot::kHoldDistance.value(), m_distance.value(), 10.0); + } +} + +INSTANTIATE_TEST_SUITE_P(UltrasonicPIDTests, UltrasonicPIDTest, + testing::Values(1.3, 0.5, 5.0)); diff --git a/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/main.cpp b/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/main.cpp new file mode 100644 index 0000000000..285c1d5267 --- /dev/null +++ b/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/main.cpp @@ -0,0 +1,17 @@ +// 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 "gtest/gtest.h" + +/** + * Runs all unit tests. + */ +int main(int argc, char** argv) { + HAL_Initialize(500, 0); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java index d7b326a2bf..fd2b05bf45 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java @@ -90,9 +90,6 @@ public class Ultrasonic implements Sendable, AutoCloseable { m_pingChannel.setSimDevice(m_simDevice); m_echoChannel.setSimDevice(m_simDevice); } - if (m_task == null) { - m_task = new UltrasonicChecker(); - } final boolean originalMode = m_automaticEnabled; setAutomaticMode(false); // kill task when adding a new sensor m_sensors.add(this); @@ -202,7 +199,7 @@ public class Ultrasonic implements Sendable, AutoCloseable { * sensors fire at the same time. If another scheduling algorithm is preferred, it can be * implemented by pinging the sensors manually and waiting for the results to come back. */ - public static void setAutomaticMode(boolean enabling) { + public static synchronized void setAutomaticMode(boolean enabling) { if (enabling == m_automaticEnabled) { return; // ignore the case of no change } @@ -217,14 +214,18 @@ public class Ultrasonic implements Sendable, AutoCloseable { } // Start round robin task + m_task = new UltrasonicChecker(); m_task.start(); } else { - // Wait for background task to stop running - try { - m_task.join(); - } catch (InterruptedException ex) { - Thread.currentThread().interrupt(); - ex.printStackTrace(); + if (m_task != null) { + // Wait for background task to stop running + try { + m_task.join(); + m_task = null; + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + ex.printStackTrace(); + } } /* Clear all the counters (data now invalid) since automatic mode is diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/UltrasonicSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/UltrasonicSim.java index 3781281272..dc4ebaade1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/UltrasonicSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/UltrasonicSim.java @@ -9,6 +9,7 @@ import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Ultrasonic; +/** Class to control a simulated {@link edu.wpi.first.wpilibj.Ultrasonic}. */ public class UltrasonicSim { private final SimBoolean m_simRangeValid; private final SimDouble m_simRange; @@ -19,7 +20,18 @@ public class UltrasonicSim { * @param ultrasonic The real ultrasonic to simulate */ public UltrasonicSim(Ultrasonic ultrasonic) { - SimDeviceSim simDevice = new SimDeviceSim("Ultrasonic", ultrasonic.getEchoChannel()); + // ping parameter is unused + this(-1, ultrasonic.getEchoChannel()); + } + + /** + * Constructor. + * + * @param ping unused. + * @param echo the ultrasonic's echo channel. + */ + public UltrasonicSim(@SuppressWarnings("unused") int ping, int echo) { + SimDeviceSim simDevice = new SimDeviceSim("Ultrasonic", echo); m_simRangeValid = simDevice.getBoolean("Range Valid"); m_simRange = simDevice.getDouble("Range (in)"); } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/UltrasonicTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/UltrasonicTest.java index a7ab0ca2fe..da5eac71b3 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/UltrasonicTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/UltrasonicTest.java @@ -4,12 +4,15 @@ package edu.wpi.first.wpilibj; +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.wpilibj.simulation.UltrasonicSim; import org.junit.jupiter.api.Test; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.ValueSource; class UltrasonicTest { @Test @@ -28,4 +31,23 @@ class UltrasonicTest { assertEquals(0, ultrasonic.getRangeInches()); } } + + @Test + void automaticModeToggle() { + try (@SuppressWarnings("unused") + Ultrasonic ultrasonic = new Ultrasonic(0, 1)) { + assertDoesNotThrow( + () -> { + Ultrasonic.setAutomaticMode(true); + Ultrasonic.setAutomaticMode(false); + Ultrasonic.setAutomaticMode(true); + }); + } + } + + @ValueSource(booleans = {true, false}) + @ParameterizedTest + void automaticModeWithZeroInstances(boolean enabling) { + assertDoesNotThrow(() -> Ultrasonic.setAutomaticMode(enabling)); + } } 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 01f79e7745..551348221d 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 @@ -115,11 +115,13 @@ }, { "name": "Ultrasonic", - "description": "Demonstrate maintaining a set distance using an ultrasonic sensor.", + "description": "Demonstrate using the Ultrasonic class with a ping-response ultrasonic sensor.", "tags": [ "Sensors", - "Robot and Motor", - "Analog" + "Hardware", + "Ultrasonic", + "SmartDashboard", + "Shuffleboard" ], "foldername": "ultrasonic", "gradlebase": "java", @@ -131,8 +133,9 @@ "description": "Demonstrate maintaining a set distance using an ultrasonic sensor and PID Control.", "tags": [ "Sensors", - "Robot and Motor", - "Analog" + "Ultrasonic", + "PID", + "Differential Drive" ], "foldername": "ultrasonicpid", "gradlebase": "java", @@ -141,11 +144,13 @@ }, { "name": "Potentiometer PID", - "description": "An example to demonstrate the use of a potentiometer and PID control to reach elevator position setpoints.", + "description": "An example to demonstrate the use of a potentiometer and PID control to maintain elevator position setpoints.", "tags": [ "Sensors", "Actuators", "Analog", + "Elevator", + "PID", "Joystick" ], "foldername": "potentiometerpid", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java index a1bb38fcb2..9be35540b0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java @@ -5,7 +5,7 @@ package edu.wpi.first.wpilibj.examples.potentiometerpid; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.AnalogPotentiometer; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.motorcontrol.MotorController; @@ -16,53 +16,55 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; * reach and maintain position setpoints on an elevator mechanism. */ public class Robot extends TimedRobot { - private static final int kPotChannel = 1; - private static final int kMotorChannel = 7; - private static final int kJoystickChannel = 0; + static final int kPotChannel = 1; + static final int kMotorChannel = 7; + static final int kJoystickChannel = 0; - // bottom, middle, and top elevator setpoints - private static final double[] kSetPoints = {1.0, 2.6, 4.3}; + // The elevator can move 1.5 meters from top to bottom + static final double kFullHeightMeters = 1.5; - // proportional, integral, and derivative speed constants; motor inverted + // Bottom, middle, and top elevator setpoints + static final double[] kSetpointsMeters = {0.2, 0.8, 1.4}; + + // proportional, integral, and derivative speed constants // DANGER: when tuning PID constants, high/inappropriate values for kP, kI, // and kD may cause dangerous, uncontrollable, or undesired behavior! - // these may need to be positive for a non-inverted motor - private static final double kP = -5.0; - private static final double kI = -0.02; - private static final double kD = -2.0; + private static final double kP = 0.7; + private static final double kI = 0.35; + private static final double kD = 0.25; - private PIDController m_pidController; - private AnalogInput m_potentiometer; - private MotorController m_elevatorMotor; - private Joystick m_joystick; + private final PIDController m_pidController = new PIDController(kP, kI, kD); + // Scaling is handled internally + private final AnalogPotentiometer m_potentiometer = + new AnalogPotentiometer(kPotChannel, kFullHeightMeters); + private final MotorController m_elevatorMotor = new PWMSparkMax(kMotorChannel); + private final Joystick m_joystick = new Joystick(kJoystickChannel); private int m_index; - private boolean m_previousButtonValue; @Override - public void robotInit() { - m_potentiometer = new AnalogInput(kPotChannel); - m_elevatorMotor = new PWMSparkMax(kMotorChannel); - m_joystick = new Joystick(kJoystickChannel); - - m_pidController = new PIDController(kP, kI, kD); - m_pidController.setSetpoint(kSetPoints[m_index]); + public void teleopInit() { + // Move to the bottom setpoint when teleop starts + m_index = 0; + m_pidController.setSetpoint(kSetpointsMeters[m_index]); } @Override public void teleopPeriodic() { + // Read from the sensor + double position = m_potentiometer.get(); + // Run the PID Controller - double pidOut = m_pidController.calculate(m_potentiometer.getAverageVoltage()); + double pidOut = m_pidController.calculate(position); + + // Apply PID output m_elevatorMotor.set(pidOut); - // when the button is pressed once, the selected elevator setpoint - // is incremented - boolean currentButtonValue = m_joystick.getTrigger(); - if (currentButtonValue && !m_previousButtonValue) { + // when the button is pressed once, the selected elevator setpoint is incremented + if (m_joystick.getTriggerPressed()) { // index of the elevator setpoint wraps around. - m_index = (m_index + 1) % kSetPoints.length; - m_pidController.setSetpoint(kSetPoints[m_index]); + m_index = (m_index + 1) % kSetpointsMeters.length; + m_pidController.setSetpoint(kSetpointsMeters[m_index]); } - m_previousButtonValue = currentButtonValue; } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java index 199d6f6a93..edc81ab724 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java @@ -4,52 +4,61 @@ package edu.wpi.first.wpilibj.examples.ultrasonic; -import edu.wpi.first.math.filter.MedianFilter; -import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.Ultrasonic; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** - * This is a sample program demonstrating how to use an ultrasonic sensor and proportional control - * to maintain a set distance from an object. + * This is a sample program demonstrating how to read from a ping-response ultrasonic sensor with + * the {@link Ultrasonic class}. */ public class Robot extends TimedRobot { - // distance in inches the robot wants to stay from an object - private static final double kHoldDistance = 12.0; + // Creates a ping-response Ultrasonic object on DIO 1 and 2. + Ultrasonic m_rangeFinder = new Ultrasonic(1, 2); - // factor to convert sensor values to a distance in inches - private static final double kValueToInches = 0.125; + @Override + public void robotInit() { + // Add the ultrasonic on the "Sensors" tab of the dashboard + // Data will update automatically + Shuffleboard.getTab("Sensors").add(m_rangeFinder); + } - // proportional speed constant - private static final double kP = 0.05; - - private static final int kLeftMotorPort = 0; - private static final int kRightMotorPort = 1; - private static final int kUltrasonicPort = 0; - - // median filter to discard outliers; filters over 10 samples - private final MedianFilter m_filter = new MedianFilter(10); - - private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort); - private final DifferentialDrive m_robotDrive = - new DifferentialDrive(new PWMSparkMax(kLeftMotorPort), new PWMSparkMax(kRightMotorPort)); - - /** - * Tells the robot to drive to a set distance (in inches) from an object using proportional - * control. - */ @Override public void teleopPeriodic() { - // sensor returns a value from 0-4095 that is scaled to inches - // returned value is filtered with a rolling median filter, since ultrasonics - // tend to be quite noisy and susceptible to sudden outliers - double currentDistance = m_filter.calculate(m_ultrasonic.getValue()) * kValueToInches; + // We can read the distance in millimeters + double distanceMillimeters = m_rangeFinder.getRangeMM(); + // ... or in inches + double distanceInches = m_rangeFinder.getRangeInches(); - // convert distance error to a motor speed - double currentSpeed = (kHoldDistance - currentDistance) * kP; + // We can also publish the data itself periodically + SmartDashboard.putNumber("Distance[mm]", distanceMillimeters); + SmartDashboard.putNumber("Distance[inch]", distanceInches); + } - // drive robot - m_robotDrive.arcadeDrive(currentSpeed, 0); + @Override + public void testInit() { + // By default, the Ultrasonic class polls all ultrasonic sensors in a round-robin to prevent + // them from interfering from one another. + // However, manual polling is also possible -- note that this disables automatic mode! + m_rangeFinder.ping(); + } + + @Override + public void testPeriodic() { + if (m_rangeFinder.isRangeValid()) { + // Data is valid, publish it + SmartDashboard.putNumber("Distance[mm]", m_rangeFinder.getRangeMM()); + SmartDashboard.putNumber("Distance[inch]", m_rangeFinder.getRangeInches()); + + // Ping for next measurement + m_rangeFinder.ping(); + } + } + + @Override + public void testExit() { + // Enable automatic mode + Ultrasonic.setAutomaticMode(true); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java index 526ac53919..532e9ac448 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java @@ -6,8 +6,8 @@ package edu.wpi.first.wpilibj.examples.ultrasonicpid; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.MedianFilter; -import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Ultrasonic; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; @@ -16,45 +16,56 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; * reach and maintain a set distance from an object. */ public class Robot extends TimedRobot { - // distance in inches the robot wants to stay from an object - private static final double kHoldDistance = 12.0; - - // factor to convert sensor values to a distance in inches - private static final double kValueToInches = 0.125; + // distance the robot wants to stay from an object + // (one meter) + static final double kHoldDistanceMillimeters = 1.0e3; // proportional speed constant - private static final double kP = 7.0; - + // negative because applying positive voltage will bring us closer to the target + private static final double kP = -0.001; // integral speed constant - private static final double kI = 0.018; - + private static final double kI = 0.0; // derivative speed constant - private static final double kD = 1.5; + private static final double kD = 0.0; - private static final int kLeftMotorPort = 0; - private static final int kRightMotorPort = 1; - private static final int kUltrasonicPort = 0; + static final int kLeftMotorPort = 0; + static final int kRightMotorPort = 1; - // median filter to discard outliers; filters over 5 samples + static final int kUltrasonicPingPort = 0; + static final int kUltrasonicEchoPort = 1; + + // Ultrasonic sensors tend to be quite noisy and susceptible to sudden outliers, + // so measurements are filtered with a 5-sample median filter private final MedianFilter m_filter = new MedianFilter(5); - private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort); - private final DifferentialDrive m_robotDrive = - new DifferentialDrive(new PWMSparkMax(kLeftMotorPort), new PWMSparkMax(kRightMotorPort)); + private final Ultrasonic m_ultrasonic = new Ultrasonic(kUltrasonicPingPort, kUltrasonicEchoPort); + private final PWMSparkMax m_leftMotor = new PWMSparkMax(kLeftMotorPort); + private final PWMSparkMax m_rightMotor = new PWMSparkMax(kRightMotorPort); + private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor); private final PIDController m_pidController = new PIDController(kP, kI, kD); @Override - public void teleopInit() { + public void autonomousInit() { // Set setpoint of the pid controller - m_pidController.setSetpoint(kHoldDistance * kValueToInches); + m_pidController.setSetpoint(kHoldDistanceMillimeters); } @Override - public void teleopPeriodic() { - // returned value is filtered with a rolling median filter, since ultrasonics - // tend to be quite noisy and susceptible to sudden outliers - double pidOutput = m_pidController.calculate(m_filter.calculate(m_ultrasonic.getVoltage())); + public void autonomousPeriodic() { + double measurement = m_ultrasonic.getRangeMM(); + double filteredMeasurement = m_filter.calculate(measurement); + double pidOutput = m_pidController.calculate(filteredMeasurement); - m_robotDrive.arcadeDrive(pidOutput, 0); + // disable input squaring -- PID output is linear + m_robotDrive.arcadeDrive(pidOutput, 0, false); + } + + @Override + public void close() { + m_leftMotor.close(); + m_rightMotor.close(); + m_ultrasonic.close(); + m_robotDrive.close(); + super.close(); } } diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java new file mode 100644 index 0000000000..c2b5311141 --- /dev/null +++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java @@ -0,0 +1,173 @@ +// 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.potentiometerpid; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.AnalogInputSim; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.simulation.JoystickSim; +import edu.wpi.first.wpilibj.simulation.PWMSim; +import edu.wpi.first.wpilibj.simulation.SimHooks; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.parallel.ResourceLock; + +@ResourceLock("timing") +class PotentiometerPIDTest { + private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4); + private static final double kElevatorGearing = 10.0; + private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0); + private static final double kCarriageMassKg = 4.0; // kg + + private Robot m_robot; + private Thread m_thread; + + private ElevatorSim m_elevatorSim; + private PWMSim m_motorSim; + private AnalogInputSim m_analogSim; + private SimPeriodicBeforeCallback m_callback; + private JoystickSim m_joystickSim; + + @BeforeEach + void startThread() { + HAL.initialize(500, 0); + SimHooks.pauseTiming(); + DriverStationSim.resetData(); + m_robot = new Robot(); + m_thread = new Thread(m_robot::startCompetition); + m_elevatorSim = + new ElevatorSim( + m_elevatorGearbox, + kElevatorGearing, + kCarriageMassKg, + kElevatorDrumRadius, + 0.0, + Robot.kFullHeightMeters, + true, + null); + m_analogSim = new AnalogInputSim(Robot.kPotChannel); + m_motorSim = new PWMSim(Robot.kMotorChannel); + m_joystickSim = new JoystickSim(Robot.kJoystickChannel); + + m_callback = + HAL.registerSimPeriodicBeforeCallback( + () -> { + m_elevatorSim.setInputVoltage( + m_motorSim.getSpeed() * RobotController.getBatteryVoltage()); + m_elevatorSim.update(0.02); + + /* + meters = (v / 5v) * range + meters / range = v / 5v + 5v * (meters / range) = v + */ + m_analogSim.setVoltage( + RobotController.getVoltage5V() + * (m_elevatorSim.getPositionMeters() / Robot.kFullHeightMeters)); + }); + + m_thread.start(); + SimHooks.stepTiming(0.0); // Wait for Notifiers + } + + @AfterEach + void stopThread() { + m_robot.endCompetition(); + try { + m_thread.interrupt(); + m_thread.join(); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + m_robot.close(); + m_callback.close(); + m_analogSim.resetData(); + m_motorSim.resetData(); + } + + @Test + void teleopTest() { + // teleop init + { + DriverStationSim.setAutonomous(false); + DriverStationSim.setEnabled(true); + DriverStationSim.notifyNewData(); + + assertTrue(m_motorSim.getInitialized()); + assertTrue(m_analogSim.getInitialized()); + } + + // first setpoint + { + // advance 50 timesteps + SimHooks.stepTiming(1); + + assertEquals(Robot.kSetpointsMeters[0], m_elevatorSim.getPositionMeters(), 0.1); + } + + // second setpoint + { + // press button to advance setpoint + m_joystickSim.setTrigger(true); + m_joystickSim.notifyNewData(); + + // advance 50 timesteps + SimHooks.stepTiming(1); + + assertEquals(Robot.kSetpointsMeters[1], m_elevatorSim.getPositionMeters(), 0.1); + } + + // we need to unpress the button + { + m_joystickSim.setTrigger(false); + m_joystickSim.notifyNewData(); + + // advance 10 timesteps + SimHooks.stepTiming(0.2); + } + + // third setpoint + { + // press button to advance setpoint + m_joystickSim.setTrigger(true); + m_joystickSim.notifyNewData(); + + // advance 50 timesteps + SimHooks.stepTiming(1); + + assertEquals(Robot.kSetpointsMeters[2], m_elevatorSim.getPositionMeters(), 0.1); + } + + // we need to unpress the button + { + m_joystickSim.setTrigger(false); + m_joystickSim.notifyNewData(); + + // advance 10 timesteps + SimHooks.stepTiming(0.2); + } + + // rollover: first setpoint + { + // press button to advance setpoint + m_joystickSim.setTrigger(true); + m_joystickSim.notifyNewData(); + + // advance 60 timesteps + SimHooks.stepTiming(1.2); + + assertEquals(Robot.kSetpointsMeters[0], m_elevatorSim.getPositionMeters(), 0.1); + } + } +} diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java new file mode 100644 index 0000000000..342daa2922 --- /dev/null +++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java @@ -0,0 +1,135 @@ +// 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.ultrasonicpid; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; +import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim.KitbotGearing; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj.simulation.PWMSim; +import edu.wpi.first.wpilibj.simulation.SimHooks; +import edu.wpi.first.wpilibj.simulation.UltrasonicSim; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.parallel.ResourceLock; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.ValueSource; + +@ResourceLock("timing") +class UltrasonicPIDTest { + private final DCMotor m_gearbox = DCMotor.getFalcon500(2); + private static final double kGearing = KitbotGearing.k10p71.value; + public static final double kvVoltSecondsPerMeter = 1.98; + public static final double kaVoltSecondsSquaredPerMeter = 0.2; + private static final double kvVoltSecondsPerRadian = 1.5; + private static final double kaVoltSecondsSquaredPerRadian = 0.3; + private static final double kWheelDiameterMeters = 0.15; + private static final double kTrackwidthMeters = 0.7; + + private Robot m_robot; + private Thread m_thread; + + private DifferentialDrivetrainSim m_driveSim; + private PWMSim m_leftMotorSim; + private PWMSim m_rightMotorSim; + private UltrasonicSim m_ultrasonicSim; + private SimPeriodicBeforeCallback m_callback; + + // distance between the robot's starting position and the object + // we will update this in a moment + private double m_startToObject = Double.POSITIVE_INFINITY; + private double m_distanceMM; + + // We're not using @BeforeEach so m_startToObject gets initialized properly + private void startThread() { + HAL.initialize(500, 0); + SimHooks.pauseTiming(); + DriverStationSim.resetData(); + m_robot = new Robot(); + m_thread = new Thread(m_robot::startCompetition); + m_driveSim = + new DifferentialDrivetrainSim( + LinearSystemId.identifyDrivetrainSystem( + kvVoltSecondsPerMeter, + kaVoltSecondsSquaredPerMeter, + kvVoltSecondsPerRadian, + kaVoltSecondsSquaredPerRadian), + m_gearbox, + kGearing, + kTrackwidthMeters, + kWheelDiameterMeters / 2.0, + null); + m_ultrasonicSim = new UltrasonicSim(Robot.kUltrasonicPingPort, Robot.kUltrasonicEchoPort); + m_leftMotorSim = new PWMSim(Robot.kLeftMotorPort); + m_rightMotorSim = new PWMSim(Robot.kRightMotorPort); + + m_callback = + HAL.registerSimPeriodicBeforeCallback( + () -> { + m_driveSim.setInputs( + m_leftMotorSim.getSpeed() * RobotController.getBatteryVoltage(), + m_rightMotorSim.getSpeed() * RobotController.getBatteryVoltage()); + m_driveSim.update(0.02); + + double startingDistance = m_startToObject; + double range = startingDistance - m_driveSim.getLeftPositionMeters(); + + m_ultrasonicSim.setRangeMeters(range); + m_distanceMM = range * 1.0e3; + }); + + m_thread.start(); + SimHooks.stepTiming(0.0); // Wait for Notifiers + SimHooks.stepTiming(0.02); // Have once iteration on disabled + } + + @AfterEach + void stopThread() { + m_robot.endCompetition(); + try { + m_thread.interrupt(); + m_thread.join(); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + m_robot.close(); + m_callback.close(); + m_leftMotorSim.resetData(); + m_rightMotorSim.resetData(); + } + + @ValueSource(doubles = {1.3, 0.5, 5.0}) + @ParameterizedTest + void autoTest(double distance) { + // set up distance + { + m_startToObject = distance; + } + startThread(); + + // auto init + { + DriverStationSim.setAutonomous(true); + DriverStationSim.setEnabled(true); + DriverStationSim.notifyNewData(); + + assertTrue(m_leftMotorSim.getInitialized()); + assertTrue(m_rightMotorSim.getInitialized()); + } + + { + // advance 100 timesteps + SimHooks.stepTiming(2.0); + + assertEquals(Robot.kHoldDistanceMillimeters, m_distanceMM, 10); + } + } +}