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

Fix C++ Ultrasonic to return correct units.

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

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

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

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