Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib, examples] Cleanup PotentiometerPID, Ultrasonic, UltrasonicPID examples (#4893)

Browse Source 
Fix C++ Ultrasonic to return correct units.
This commit is contained in:
Starlight220
2023-01-09 02:33:07 +02:00
committed by GitHub
parent babb0c1fcf
commit 2cd9be413f
24 changed files with 1052 additions and 301 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
wpilibc/src/main/native/cpp/Ultrasonic.cpp
View File

@@ -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 {

9
wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp
View File

@@ -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());
}

18
wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h
View File

@@ -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;

19
wpilibc/src/test/native/cpp/UltrasonicTest.cpp
View File

@@ -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); });
}

86
wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
View File

@@ -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 <array>
#include "Robot.h"
#include <frc/AnalogInput.h>
#include <frc/Joystick.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
#include <frc/motorcontrol/PWMSparkMax.h>
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<double, 3> 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<double, 3> Robot::kSetPoints;
}
#ifndef RUNNING_FRC_TESTS
int main() {

55
wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h Normal file
View File

@@ -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 <array>
#include <frc/AnalogPotentiometer.h>
#include <frc/Joystick.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <units/length.h>
/**
* 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<units::meter_t, 3> 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;
};

93
wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
View File

@@ -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 <frc/AnalogInput.h>
#include <frc/TimedRobot.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/filter/MedianFilter.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#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 <frc/shuffleboard/Shuffleboard.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <units/length.h>
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<double> 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() {

25
wpilibcExamples/src/main/cpp/examples/Ultrasonic/include/Robot.h Normal file
View File

@@ -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 <frc/TimedRobot.h>
#include <frc/Ultrasonic.h>
/**
* 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};
};

68
wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
View File

@@ -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 <frc/AnalogInput.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/filter/MedianFilter.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#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<double> 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() {

51
wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h Normal file
View File

@@ -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 <frc/TimedRobot.h>
#include <frc/Ultrasonic.h>
#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/filter/MedianFilter.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <units/length.h>
/**
* 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<units::millimeter_t> 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};
};

22
wpilibcExamples/src/main/cpp/examples/examples.json
View File

@@ -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"
],

167
wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include <string>
#include <thread>
#include <frc/RobotController.h>
#include <frc/simulation/AnalogInputSim.h>
#include <frc/simulation/DriverStationSim.h>
#include <frc/simulation/ElevatorSim.h>
#include <frc/simulation/JoystickSim.h>
#include <frc/simulation/PWMSim.h>
#include <frc/simulation/SimHooks.h>
#include <frc/system/plant/DCMotor.h>
#include <hal/simulation/MockHooks.h>
#include <units/length.h>
#include <units/mass.h>
#include <units/time.h>
#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<std::thread> 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<PotentiometerPIDTest*>(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);
}
}

17
wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/main.cpp Normal file
View File

@@ -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 <hal/HALBase.h>
#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;
}

111
wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include <string>
#include <thread>
#include <frc/RobotController.h>
#include <frc/simulation/DifferentialDrivetrainSim.h>
#include <frc/simulation/DriverStationSim.h>
#include <frc/simulation/PWMSim.h>
#include <frc/simulation/SimHooks.h>
#include <frc/simulation/UltrasonicSim.h>
#include <frc/system/plant/DCMotor.h>
#include <frc/system/plant/LinearSystemId.h>
#include <hal/simulation/MockHooks.h>
#include <units/angle.h>
#include <units/length.h>
#include <units/mass.h>
#include <units/time.h>
#include "Robot.h"
class UltrasonicPIDTest : public testing::TestWithParam<double> {
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<std::thread> 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<UltrasonicPIDTest*>(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));

17
wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/main.cpp Normal file
View File

@@ -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 <hal/HALBase.h>
#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;
}

21
wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
View File

@@ -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

14
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/UltrasonicSim.java
View File

@@ -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)");
}

22
wpilibj/src/test/java/edu/wpi/first/wpilibj/UltrasonicTest.java
View File

@@ -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));
}
}

17
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
View File

@@ -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",

64
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
View File

@@ -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;
}
}

81
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
View File

@@ -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);
}
}

61
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
View File

@@ -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();
}
}

173
wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java Normal file
View File

@@ -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);
}
}
}

135
wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java Normal file
View File

@@ -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);
}
}
}