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

Fix C++ Ultrasonic to return correct units.

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

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