[wpilib] Add SysIdRoutine logging utility and command factory (#6033)

Co-authored-by: Drew Williams <williams.r.drew@gmail.com>
Co-authored-by: Peter Johnson <johnson.peter@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpilibcExamples/src/main/cpp/examples/SysId/cpp/Robot.cpp

@@ -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.
+
+#include "Robot.h"
+
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
+
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+void Robot::DisabledExit() {}
+
+void Robot::AutonomousInit() {
+  m_autonomousCommand = m_container.GetAutonomousCommand();
+
+  if (m_autonomousCommand) {
+    m_autonomousCommand->Schedule();
+  }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::AutonomousExit() {}
+
+void Robot::TeleopInit() {
+  if (m_autonomousCommand) {
+    m_autonomousCommand->Cancel();
+  }
+}
+
+void Robot::TeleopPeriodic() {}
+
+void Robot::TeleopExit() {}
+
+void Robot::TestInit() {
+  frc2::CommandScheduler::GetInstance().CancelAll();
+}
+
+void Robot::TestPeriodic() {}
+
+void Robot::TestExit() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp

@@ -0,0 +1,30 @@
+// 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 "SysIdRoutineBot.h"
+
+#include <frc2/command/Commands.h>
+
+SysIdRoutineBot::SysIdRoutineBot() {
+  ConfigureBindings();
+}
+
+void SysIdRoutineBot::ConfigureBindings() {
+  m_drive.SetDefaultCommand(m_drive.ArcadeDriveCommand(
+      [this] { return -m_driverController.GetLeftY(); },
+      [this] { return -m_driverController.GetRightX(); }));
+
+  m_driverController.A().WhileTrue(
+      m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward));
+  m_driverController.B().WhileTrue(
+      m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
+  m_driverController.X().WhileTrue(
+      m_drive.SysIdDynamic(frc2::sysid::Direction::kForward));
+  m_driverController.Y().WhileTrue(
+      m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse));
+}
+
+frc2::CommandPtr SysIdRoutineBot::GetAutonomousCommand() {
+  return m_drive.Run([] {});
+}

wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Drive.cpp

@@ -0,0 +1,37 @@
+// 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 "subsystems/Drive.h"
+
+#include <frc2/command/Commands.h>
+
+Drive::Drive() {
+  m_leftMotor.AddFollower(frc::PWMSparkMax{constants::drive::kLeftMotor2Port});
+  m_rightMotor.AddFollower(
+      frc::PWMSparkMax{constants::drive::kRightMotor2Port});
+
+  m_rightMotor.SetInverted(true);
+
+  m_leftEncoder.SetDistancePerPulse(
+      constants::drive::kEncoderDistancePerPulse.value());
+  m_rightEncoder.SetDistancePerPulse(
+      constants::drive::kEncoderDistancePerPulse.value());
+
+  m_drive.SetSafetyEnabled(false);
+}
+
+frc2::CommandPtr Drive::ArcadeDriveCommand(std::function<double()> fwd,
+                                           std::function<double()> rot) {
+  return frc2::cmd::Run([this, fwd, rot] { m_drive.ArcadeDrive(fwd(), rot()); },
+                        {this})
+      .WithName("Arcade Drive");
+}
+
+frc2::CommandPtr Drive::SysIdQuasistatic(frc2::sysid::Direction direction) {
+  return m_sysIdRoutine.Quasistatic(direction);
+}
+
+frc2::CommandPtr Drive::SysIdDynamic(frc2::sysid::Direction direction) {
+  return m_sysIdRoutine.Dynamic(direction);
+}

wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h

@@ -0,0 +1,81 @@
+// 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 <numbers>
+
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/voltage.h>
+
+namespace constants {
+namespace drive {
+inline constexpr int kLeftMotor1Port = 0;
+inline constexpr int kLeftMotor2Port = 1;
+inline constexpr int kRightMotor1Port = 2;
+inline constexpr int kRightMotor2Port = 3;
+
+inline constexpr std::array<int, 2> kLeftEncoderPorts = {0, 1};
+inline constexpr std::array<int, 2> kRightEncoderPorts = {2, 3};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kRightEncoderReversed = true;
+
+inline constexpr int kEncoderCpr = 1024;
+inline constexpr units::meter_t kWheelDiameter = 6_in;
+inline constexpr units::meter_t kEncoderDistancePerPulse =
+    (kWheelDiameter * std::numbers::pi) / static_cast<double>(kEncoderCpr);
+}  // namespace drive
+
+namespace shooter {
+
+using kv_unit =
+    units::compound_unit<units::compound_unit<units::volts, units::seconds>,
+                         units::inverse<units::turns>>;
+using kv_unit_t = units::unit_t<kv_unit>;
+
+inline constexpr std::array<int, 2> kEncoderPorts = {4, 5};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr int kEncoderCpr = 1024;
+inline constexpr units::turn_t kEncoderDistancePerPulse =
+    units::turn_t{1.0} / static_cast<double>(kEncoderCpr);
+
+inline constexpr int kShooterMotorPort = 4;
+inline constexpr int kFeederMotorPort = 5;
+
+inline constexpr units::turns_per_second_t kShooterFreeSpeed = 5300_tps;
+inline constexpr units::turns_per_second_t kShooterTargetSpeed = 4000_tps;
+inline constexpr units::turns_per_second_t kShooterTolerance = 50_tps;
+
+inline constexpr double kP = 1.0;
+
+inline constexpr units::volt_t kS = 0.05_V;
+inline constexpr kv_unit_t kV = (12_V) / kShooterFreeSpeed;
+
+inline constexpr double kFeederSpeed = 0.5;
+}  // namespace shooter
+
+namespace intake {
+inline constexpr int kMotorPort = 6;
+inline constexpr std::array<int, 2> kSolenoidPorts = {2, 3};
+}  // namespace intake
+
+namespace storage {
+inline constexpr int kMotorPort = 7;
+inline constexpr int kBallSensorPort = 6;
+}  // namespace storage
+
+namespace autonomous {
+inline constexpr units::second_t kTimeout = 3_s;
+inline constexpr units::meter_t kDriveDistance = 2_m;
+inline constexpr double kDriveSpeed = 0.5;
+}  // namespace autonomous
+
+namespace oi {
+inline constexpr int kDriverControllerPort = 0;
+}  // namespace oi
+}  // namespace constants

wpilibcExamples/src/main/cpp/examples/SysId/include/Robot.h

@@ -0,0 +1,35 @@
+// 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 <optional>
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/CommandPtr.h>
+
+#include "SysIdRoutineBot.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void DisabledExit() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void AutonomousExit() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TeleopExit() override;
+  void TestInit() override;
+  void TestPeriodic() override;
+  void TestExit() override;
+
+ private:
+  std::optional<frc2::CommandPtr> m_autonomousCommand;
+
+  SysIdRoutineBot m_container;
+};

wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h

@@ -0,0 +1,24 @@
+// 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 <frc2/command/CommandPtr.h>
+#include <frc2/command/button/CommandXboxController.h>
+
+#include "Constants.h"
+#include "subsystems/Drive.h"
+
+class SysIdRoutineBot {
+ public:
+  SysIdRoutineBot();
+
+  frc2::CommandPtr GetAutonomousCommand();
+
+ private:
+  void ConfigureBindings();
+  frc2::CommandXboxController m_driverController{
+      constants::oi::kDriverControllerPort};
+  Drive m_drive{};
+};

wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Drive.h

@@ -0,0 +1,62 @@
+// 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 <functional>
+
+#include <frc/Encoder.h>
+#include <frc/RobotController.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc2/command/SubsystemBase.h>
+#include <frc2/command/sysid/SysIdRoutine.h>
+
+#include "Constants.h"
+
+class Drive : public frc2::SubsystemBase {
+ public:
+  Drive();
+
+  frc2::CommandPtr ArcadeDriveCommand(std::function<double()> fwd,
+                                      std::function<double()> rot);
+  frc2::CommandPtr SysIdQuasistatic(frc2::sysid::Direction direction);
+  frc2::CommandPtr SysIdDynamic(frc2::sysid::Direction direction);
+
+ private:
+  frc::PWMSparkMax m_leftMotor{constants::drive::kLeftMotor1Port};
+  frc::PWMSparkMax m_rightMotor{constants::drive::kRightMotor1Port};
+  frc::DifferentialDrive m_drive{[this](auto val) { m_leftMotor.Set(val); },
+                                 [this](auto val) { m_rightMotor.Set(val); }};
+
+  frc::Encoder m_leftEncoder{constants::drive::kLeftEncoderPorts[0],
+                             constants::drive::kLeftEncoderPorts[1],
+                             constants::drive::kLeftEncoderReversed};
+
+  frc::Encoder m_rightEncoder{constants::drive::kRightEncoderPorts[0],
+                              constants::drive::kRightEncoderPorts[1],
+                              constants::drive::kRightEncoderReversed};
+
+  frc2::sysid::SysIdRoutine m_sysIdRoutine{
+      frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt,
+                          std::nullopt},
+      frc2::sysid::Mechanism{
+          [this](units::volt_t driveVoltage) {
+            m_leftMotor.SetVoltage(driveVoltage);
+            m_rightMotor.SetVoltage(driveVoltage);
+          },
+          [this](frc::sysid::SysIdRoutineLog* log) {
+            log->Motor("drive-left")
+                .voltage(m_leftMotor.Get() *
+                         frc::RobotController::GetBatteryVoltage())
+                .position(units::meter_t{m_leftEncoder.GetDistance()})
+                .velocity(units::meters_per_second_t{m_leftEncoder.GetRate()});
+            log->Motor("drive-right")
+                .voltage(m_rightMotor.Get() *
+                         frc::RobotController::GetBatteryVoltage())
+                .position(units::meter_t{m_rightEncoder.GetDistance()})
+                .velocity(units::meters_per_second_t{m_rightEncoder.GetRate()});
+          },
+          this}};
+};

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -927,5 +927,17 @@
     "foldername": "FlywheelBangBangController",
     "gradlebase": "cpp",
     "commandversion": 2
+  },
+  {
+    "name": "SysIdRoutine",
+    "description": "A sample command-based robot demonstrating use of the SysIdRoutine command factory",
+    "tags": [
+      "SysId",
+      "Command-based",
+      "DataLog"
+    ],
+    "foldername": "SysId",
+    "gradlebase": "cpp",
+    "commandversion": 2
   }
 ]