[glass] Add drive class widgets (#2975)

This adds widgets for DifferentialDrive and MecanumDrive.

glass/src/lib/native/cpp/other/Drive.cpp

@@ -0,0 +1,127 @@
+// 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 "glass/other/Drive.h"
+
+#include <array>
+#include <cmath>
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <wpi/math>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+void glass::DisplayDrive(DriveModel* m) {
+  // Check if the model exists.
+  if (!m->Exists()) {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+    ImGui::Text("Unknown Drive");
+    ImGui::PopStyleColor();
+    return;
+  }
+
+  const auto& wheels = m->GetWheels();
+  ImDrawList* draw = ImGui::GetWindowDrawList();
+  ImColor color = ImGui::GetStyle().Colors[ImGuiCol_Text];
+
+  // Get window position and size.
+  ImVec2 pos = ImGui::GetWindowPos();
+  ImVec2 size = ImGui::GetWindowSize();
+
+  // Calculate corners for drivetrain body.
+  float x1 = pos.x + 60.0f;
+  float y1 = pos.y + ImGui::GetFontSize() * 2.0f;
+  float x2 = pos.x + size.x - 60.0f;
+  float y2 = pos.y + size.y - ImGui::GetFontSize() * 2.0f * wheels.size();
+
+  // Draw the primary rectangle.
+  draw->AddRect(ImVec2(x1, y1), ImVec2(x2, y2), color);
+
+  // Display the speed vector.
+  ImVec2 center{(x1 + x2) / 2.0f, (y1 + y2) / 2.0f};
+  ImVec2 speed = m->GetSpeedVector();
+  ImVec2 arrow = center + speed * 50.0f;
+
+  draw->AddLine(center, arrow, color, 2.0f);
+
+  auto drawArrow = [draw, &color](const ImVec2& arrowPos, float angle) {
+    draw->AddTriangleFilled(
+        arrowPos,
+        arrowPos + ImRotate(ImVec2(0.0f, 7.5f),
+                            std::cos(angle + wpi::math::pi / 4),
+                            std::sin(angle + wpi::math::pi / 4)),
+        arrowPos + ImRotate(ImVec2(0.0f, 7.5f),
+                            std::cos(angle - wpi::math::pi / 4),
+                            std::sin(angle - wpi::math::pi / 4)),
+        color);
+  };
+
+  // Draw the arrow if there is any translation; draw an X otherwise.
+  if (std::abs(speed.y) > 0 || std::abs(speed.x) > 0) {
+    drawArrow(arrow, std::atan2(speed.x, -speed.y));
+  } else {
+    ImVec2 a{7.5f, +7.5f};
+    ImVec2 b{7.5f, -7.5f};
+    draw->AddLine(center + a, center - a, color);
+    draw->AddLine(center + b, center - b, color);
+  }
+
+  // Calculate the positions of the top-left corner of the wheels.
+  std::array<ImVec2, 4> corners{
+      ImVec2(x1 - 25.0f, y1 + 10.0f), ImVec2(x1 - 25.0f, y2 - 70.0f),
+      ImVec2(x2 + 00.0f, y1 + 10.0f), ImVec2(x2 + 00.0f, y2 - 70.0f)};
+
+  // Draw the wheels.
+  for (auto&& corner : corners) {
+    draw->AddRect(corner, corner + ImVec2(25.0f, 60.0f), color);
+  }
+
+  // Show rotation
+  double rotation = m->GetRotation();
+  if (rotation != 0) {
+    float radius = 60.0f;
+    double a1 = 0.0;
+    double a2 = wpi::math::pi / 2 * rotation;
+
+    draw->PathArcTo(center, radius, a1, a2, 20);
+    draw->PathStroke(color, false);
+    draw->PathArcTo(center, radius, a1 + wpi::math::pi, a2 + wpi::math::pi, 20);
+    draw->PathStroke(color, false);
+
+    double adder = rotation < 0 ? wpi::math::pi : 0;
+
+    auto arrowPos =
+        center + ImVec2(radius * -std::cos(a2), radius * -std::sin(a2));
+    drawArrow(arrowPos, a2 + adder);
+
+    a2 += wpi::math::pi;
+    arrowPos = center + ImVec2(radius * -std::cos(a2), radius * -std::sin(a2));
+    drawArrow(arrowPos, a2 + adder);
+  }
+
+  // Add sliders for the wheel percentages.
+  ImGui::SetCursorPosY(y2 - pos.y + ImGui::GetFontSize() * 0.5);
+  for (auto&& wheel : wheels) {
+    if (wheel.percent) {
+      ImGui::PushID(wheel.name.c_str());
+      if (ImGui::Button("Zero")) {
+        wheel.setter(0.0);
+      }
+      ImGui::PopID();
+
+      ImGui::SameLine();
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8.0f);
+      float value = wheel.percent->GetValue();
+      if (wheel.percent->SliderFloat(wheel.name.c_str(), &value, -1.0f, 1.0f)) {
+        wheel.setter(value);
+      }
+    }
+  }
+}

glass/src/lib/native/include/glass/other/Drive.h

@@ -0,0 +1,41 @@
+// 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 <string>
+#include <utility>
+#include <vector>
+
+#include <wpi/StringRef.h>
+
+#include "glass/Model.h"
+
+struct ImVec2;
+
+namespace glass {
+class DataSource;
+class DriveModel : public Model {
+ public:
+  struct WheelInfo {
+    std::string name;
+    DataSource* percent;
+    std::function<void(double)> setter;
+
+    WheelInfo(wpi::StringRef name, DataSource* percent,
+              std::function<void(double)> setter)
+        : name(name), percent(percent), setter(std::move(setter)) {}
+  };
+
+  virtual const char* GetName() const = 0;
+  virtual const std::vector<WheelInfo>& GetWheels() const = 0;
+
+  virtual ImVec2 GetSpeedVector() const = 0;
+
+  // Clamped between -1 and 1 with -1 being full CCW.
+  virtual double GetRotation() const = 0;
+};
+void DisplayDrive(DriveModel* m);
+}  // namespace glass

glass/src/libnt/native/cpp/NTDifferentialDrive.cpp

@@ -0,0 +1,59 @@
+// 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 "glass/networktables/NTDifferentialDrive.h"
+
+#include <imgui.h>
+#include <wpi/MathExtras.h>
+
+using namespace glass;
+
+NTDifferentialDriveModel::NTDifferentialDriveModel(wpi::StringRef path)
+    : NTDifferentialDriveModel(nt::GetDefaultInstance(), path) {}
+
+NTDifferentialDriveModel::NTDifferentialDriveModel(NT_Inst instance,
+                                                   wpi::StringRef path)
+    : m_nt(instance),
+      m_name(m_nt.GetEntry(path + "/.name")),
+      m_lPercent(m_nt.GetEntry(path + "/Left Motor Speed")),
+      m_rPercent(m_nt.GetEntry(path + "/Right Motor Speed")),
+      m_nameValue(path.rsplit('/').second),
+      m_lPercentData("NTDiffDriveL:" + path),
+      m_rPercentData("NTDiffDriveR:" + path) {
+  m_nt.AddListener(m_name);
+  m_nt.AddListener(m_lPercent);
+  m_nt.AddListener(m_rPercent);
+
+  m_wheels.emplace_back("L % Output", &m_lPercentData, [this](auto value) {
+    nt::SetEntryValue(m_lPercent, nt::NetworkTableValue::MakeDouble(value));
+  });
+
+  m_wheels.emplace_back("R % Output", &m_rPercentData, [this](auto value) {
+    nt::SetEntryValue(m_rPercent, nt::NetworkTableValue::MakeDouble(value));
+  });
+}
+
+void NTDifferentialDriveModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_name && event.value && event.value->IsString()) {
+      m_nameValue = event.value->GetString();
+    } else if (event.entry == m_lPercent && event.value &&
+               event.value->IsDouble()) {
+      m_lPercentData.SetValue(event.value->GetDouble());
+    } else if (event.entry == m_rPercent && event.value &&
+               event.value->IsDouble()) {
+      m_rPercentData.SetValue(event.value->GetDouble());
+    }
+  }
+
+  double l = m_lPercentData.GetValue();
+  double r = m_rPercentData.GetValue();
+
+  m_speedVector = ImVec2(0.0, -(l + r) / 2.0);
+  m_rotation = (l - r) / 2.0;
+}
+
+bool NTDifferentialDriveModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_lPercent) != NT_UNASSIGNED;
+}

glass/src/libnt/native/cpp/NTMecanumDrive.cpp

@@ -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.
+
+#include "glass/networktables/NTMecanumDrive.h"
+
+#include <imgui.h>
+#include <wpi/MathExtras.h>
+
+using namespace glass;
+
+NTMecanumDriveModel::NTMecanumDriveModel(wpi::StringRef path)
+    : NTMecanumDriveModel(nt::GetDefaultInstance(), path) {}
+
+NTMecanumDriveModel::NTMecanumDriveModel(NT_Inst instance, wpi::StringRef path)
+    : m_nt(instance),
+      m_name(m_nt.GetEntry(path + "/.name")),
+      m_flPercent(m_nt.GetEntry(path + "/Front Left Motor Speed")),
+      m_frPercent(m_nt.GetEntry(path + "/Front Right Motor Speed")),
+      m_rlPercent(m_nt.GetEntry(path + "/Rear Left Motor Speed")),
+      m_rrPercent(m_nt.GetEntry(path + "/Rear Right Motor Speed")),
+      m_nameValue(path.rsplit('/').second),
+      m_flPercentData("NTMcnmDriveFL:" + path),
+      m_frPercentData("NTMcnmDriveFR:" + path),
+      m_rlPercentData("NTMcnmDriveRL:" + path),
+      m_rrPercentData("NTMcnmDriveRR:" + path) {
+  m_nt.AddListener(m_name);
+  m_nt.AddListener(m_flPercent);
+  m_nt.AddListener(m_frPercent);
+  m_nt.AddListener(m_rlPercent);
+  m_nt.AddListener(m_rrPercent);
+
+  m_wheels.emplace_back("FL % Output", &m_flPercentData, [this](auto value) {
+    nt::SetEntryValue(m_flPercent, nt::NetworkTableValue::MakeDouble(value));
+  });
+
+  m_wheels.emplace_back("FR % Output", &m_frPercentData, [this](auto value) {
+    nt::SetEntryValue(m_frPercent, nt::NetworkTableValue::MakeDouble(value));
+  });
+
+  m_wheels.emplace_back("RL % Output", &m_rlPercentData, [this](auto value) {
+    nt::SetEntryValue(m_rlPercent, nt::NetworkTableValue::MakeDouble(value));
+  });
+
+  m_wheels.emplace_back("RR % Output", &m_rrPercentData, [this](auto value) {
+    nt::SetEntryValue(m_rrPercent, nt::NetworkTableValue::MakeDouble(value));
+  });
+}
+
+void NTMecanumDriveModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_name && event.value && event.value->IsString()) {
+      m_nameValue = event.value->GetString();
+    } else if (event.entry == m_flPercent && event.value &&
+               event.value->IsDouble()) {
+      m_flPercentData.SetValue(event.value->GetDouble());
+    } else if (event.entry == m_frPercent && event.value &&
+               event.value->IsDouble()) {
+      m_frPercentData.SetValue(event.value->GetDouble());
+    } else if (event.entry == m_rlPercent && event.value &&
+               event.value->IsDouble()) {
+      m_rlPercentData.SetValue(event.value->GetDouble());
+    } else if (event.entry == m_rrPercent && event.value &&
+               event.value->IsDouble()) {
+      m_rrPercentData.SetValue(event.value->GetDouble());
+    }
+  }
+
+  double fl = m_flPercentData.GetValue();
+  double fr = m_frPercentData.GetValue();
+  double rl = m_rlPercentData.GetValue();
+  double rr = m_rrPercentData.GetValue();
+
+  m_speedVector =
+      ImVec2((fl - fr - rl + rr) / 4.0f, -(fl + fr + rl + rr) / 4.0f);
+  m_rotation = -(-fl + fr - rl + rr) / 4;
+}
+
+bool NTMecanumDriveModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_flPercent) != NT_UNASSIGNED;
+}

glass/src/libnt/native/cpp/StandardNetworkTables.cpp

@@ -4,11 +4,13 @@
 
 #include "glass/networktables/NTCommandScheduler.h"
 #include "glass/networktables/NTCommandSelector.h"
+#include "glass/networktables/NTDifferentialDrive.h"
 #include "glass/networktables/NTDigitalInput.h"
 #include "glass/networktables/NTDigitalOutput.h"
 #include "glass/networktables/NTFMS.h"
 #include "glass/networktables/NTField2D.h"
 #include "glass/networktables/NTGyro.h"
+#include "glass/networktables/NTMecanumDrive.h"
 #include "glass/networktables/NTPIDController.h"
 #include "glass/networktables/NTSpeedController.h"
 #include "glass/networktables/NTStringChooser.h"
@@ -40,6 +42,17 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) {
           DisplayCommandSelector(static_cast<NTCommandSelectorModel*>(model));
         });
       });
+  provider.Register(
+      NTDifferentialDriveModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTDifferentialDriveModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char*) {
+        win->SetDefaultSize(300, 350);
+        return MakeFunctionView([=] {
+          DisplayDrive(static_cast<NTDifferentialDriveModel*>(model));
+        });
+      });
   provider.Register(
       NTFMSModel::kType,
       [](NT_Inst inst, const char* path) {
@@ -94,6 +107,16 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) {
         return MakeFunctionView(
             [=] { DisplayGyro(static_cast<NTGyroModel*>(model)); });
       });
+  provider.Register(
+      NTMecanumDriveModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTMecanumDriveModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char*) {
+        win->SetDefaultSize(300, 350);
+        return MakeFunctionView(
+            [=] { DisplayDrive(static_cast<NTMecanumDriveModel*>(model)); });
+      });
   provider.Register(
       NTPIDControllerModel::kType,
       [](NT_Inst inst, const char* path) {

glass/src/libnt/native/include/glass/networktables/NTDifferentialDrive.h

@@ -0,0 +1,52 @@
+// 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 <string>
+#include <vector>
+
+#include <ntcore_cpp.h>
+#include <wpi/STLExtras.h>
+#include <wpi/StringRef.h>
+
+#include "glass/DataSource.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/other/Drive.h"
+
+namespace glass {
+class NTDifferentialDriveModel : public DriveModel {
+ public:
+  static constexpr const char* kType = "DifferentialDrive";
+
+  explicit NTDifferentialDriveModel(wpi::StringRef path);
+  NTDifferentialDriveModel(NT_Inst instance, wpi::StringRef path);
+
+  const char* GetName() const override { return m_nameValue.c_str(); }
+  const std::vector<DriveModel::WheelInfo>& GetWheels() const override {
+    return m_wheels;
+  }
+
+  ImVec2 GetSpeedVector() const override { return m_speedVector; }
+  double GetRotation() const override { return m_rotation; }
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return false; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_name;
+  NT_Entry m_lPercent;
+  NT_Entry m_rPercent;
+
+  std::string m_nameValue;
+  DataSource m_lPercentData;
+  DataSource m_rPercentData;
+
+  std::vector<DriveModel::WheelInfo> m_wheels;
+  ImVec2 m_speedVector;
+  double m_rotation;
+};
+}  // namespace glass

glass/src/libnt/native/include/glass/networktables/NTMecanumDrive.h

@@ -0,0 +1,56 @@
+// 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 <string>
+#include <vector>
+
+#include <ntcore_cpp.h>
+#include <wpi/STLExtras.h>
+#include <wpi/StringRef.h>
+
+#include "glass/DataSource.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/other/Drive.h"
+
+namespace glass {
+class NTMecanumDriveModel : public DriveModel {
+ public:
+  static constexpr const char* kType = "MecanumDrive";
+
+  explicit NTMecanumDriveModel(wpi::StringRef path);
+  NTMecanumDriveModel(NT_Inst instance, wpi::StringRef path);
+
+  const char* GetName() const override { return m_nameValue.c_str(); }
+  const std::vector<DriveModel::WheelInfo>& GetWheels() const override {
+    return m_wheels;
+  }
+
+  ImVec2 GetSpeedVector() const override { return m_speedVector; }
+  double GetRotation() const override { return m_rotation; }
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return false; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_name;
+  NT_Entry m_flPercent;
+  NT_Entry m_frPercent;
+  NT_Entry m_rlPercent;
+  NT_Entry m_rrPercent;
+
+  std::string m_nameValue;
+  DataSource m_flPercentData;
+  DataSource m_frPercentData;
+  DataSource m_rlPercentData;
+  DataSource m_rrPercentData;
+
+  std::vector<DriveModel::WheelInfo> m_wheels;
+  ImVec2 m_speedVector;
+  double m_rotation;
+};
+}  // namespace glass