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

Fix C++ Ultrasonic to return correct units.

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 <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() {

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