[examples] Hatchbots: Add telemetry (#5011)

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

@@ -4,10 +4,19 @@
 
 #include "Robot.h"
 
+#include <frc/DataLogManager.h>
+#include <frc/DriverStation.h>
 #include <frc/smartdashboard/SmartDashboard.h>
 #include <frc2/command/CommandScheduler.h>
 
-void Robot::RobotInit() {}
+void Robot::RobotInit() {
+  // Start recording to data log
+  frc::DataLogManager::Start();
+
+  // Record DS control and joystick data.
+  // Change to `false` to not record joystick data.
+  frc::DriverStation::StartDataLog(frc::DataLogManager::GetLog(), true);
+}
 
 /**
  * This function is called every 20 ms, no matter the mode. Use

wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp

@@ -16,6 +16,35 @@ RobotContainer::RobotContainer() {
 
   // Put the chooser on the dashboard
   frc::Shuffleboard::GetTab("Autonomous").Add(m_chooser);
+  // Put subsystems to dashboard.
+  frc::Shuffleboard::GetTab("Drivetrain").Add(m_drive);
+  frc::Shuffleboard::GetTab("HatchSubsystem").Add(m_hatch);
+
+  // Log Shuffleboard events for command initialize, execute, finish, interrupt
+  frc2::CommandScheduler::GetInstance().OnCommandInitialize(
+      [](const frc2::Command& command) {
+        frc::Shuffleboard::AddEventMarker(
+            "Command initialized", command.GetName(),
+            frc::ShuffleboardEventImportance::kNormal);
+      });
+  frc2::CommandScheduler::GetInstance().OnCommandExecute(
+      [](const frc2::Command& command) {
+        frc::Shuffleboard::AddEventMarker(
+            "Command executed", command.GetName(),
+            frc::ShuffleboardEventImportance::kNormal);
+      });
+  frc2::CommandScheduler::GetInstance().OnCommandFinish(
+      [](const frc2::Command& command) {
+        frc::Shuffleboard::AddEventMarker(
+            "Command finished", command.GetName(),
+            frc::ShuffleboardEventImportance::kNormal);
+      });
+  frc2::CommandScheduler::GetInstance().OnCommandInterrupt(
+      [](const frc2::Command& command) {
+        frc::Shuffleboard::AddEventMarker(
+            "Command interrupted", command.GetName(),
+            frc::ShuffleboardEventImportance::kNormal);
+      });
 
   // Configure the button bindings
   ConfigureButtonBindings();

wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp

@@ -4,6 +4,8 @@
 
 #include "subsystems/DriveSubsystem.h"
 
+#include <wpi/sendable/SendableBuilder.h>
+
 using namespace DriveConstants;
 
 DriveSubsystem::DriveSubsystem()
@@ -40,14 +42,17 @@ double DriveSubsystem::GetAverageEncoderDistance() {
   return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
 }
 
-frc::Encoder& DriveSubsystem::GetLeftEncoder() {
-  return m_leftEncoder;
-}
-
-frc::Encoder& DriveSubsystem::GetRightEncoder() {
-  return m_rightEncoder;
-}
-
 void DriveSubsystem::SetMaxOutput(double maxOutput) {
   m_drive.SetMaxOutput(maxOutput);
 }
+
+void DriveSubsystem::InitSendable(wpi::SendableBuilder& builder) {
+  SubsystemBase::InitSendable(builder);
+
+  // Publish encoder distances to telemetry.
+  builder.AddDoubleProperty(
+      "leftDistance", [this] { return m_leftEncoder.GetDistance(); }, nullptr);
+  builder.AddDoubleProperty(
+      "rightDistance", [this] { return m_rightEncoder.GetDistance(); },
+      nullptr);
+}

wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp

@@ -4,6 +4,8 @@
 
 #include "subsystems/HatchSubsystem.h"
 
+#include <wpi/sendable/SendableBuilder.h>
+
 using namespace HatchConstants;
 
 HatchSubsystem::HatchSubsystem()
@@ -21,3 +23,13 @@ frc2::CommandPtr HatchSubsystem::ReleaseHatchCommand() {
   return this->RunOnce(
       [this] { m_hatchSolenoid.Set(frc::DoubleSolenoid::kReverse); });
 }
+
+void HatchSubsystem::InitSendable(wpi::SendableBuilder& builder) {
+  SubsystemBase::InitSendable(builder);
+
+  // Publish the solenoid state to telemetry.
+  builder.AddBooleanProperty(
+      "extended",
+      [this] { return m_hatchSolenoid.Get() == frc::DoubleSolenoid::kForward; },
+      nullptr);
+}

wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h

@@ -43,20 +43,6 @@ class DriveSubsystem : public frc2::SubsystemBase {
    */
   double GetAverageEncoderDistance();
 
-  /**
-   * Gets the left drive encoder.
-   *
-   * @return the left drive encoder
-   */
-  frc::Encoder& GetLeftEncoder();
-
-  /**
-   * Gets the right drive encoder.
-   *
-   * @return the right drive encoder
-   */
-  frc::Encoder& GetRightEncoder();
-
   /**
    * Sets the max output of the drive.  Useful for scaling the drive to drive
    * more slowly.
@@ -65,6 +51,8 @@ class DriveSubsystem : public frc2::SubsystemBase {
    */
   void SetMaxOutput(double maxOutput);
 
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
  private:
   // Components (e.g. motor controllers and sensors) should generally be
   // declared private and exposed only through public methods.

wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h

@@ -27,6 +27,8 @@ class HatchSubsystem : public frc2::SubsystemBase {
    */
   frc2::CommandPtr ReleaseHatchCommand();
 
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
  private:
   // Components (e.g. motor controllers and sensors) should generally be
   // declared private and exposed only through public methods.