diff --git a/glass/src/lib/native/cpp/other/Drive.cpp b/glass/src/lib/native/cpp/other/Drive.cpp new file mode 100644 index 0000000000..8dba6b5bb5 --- /dev/null +++ b/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 +#include + +#define IMGUI_DEFINE_MATH_OPERATORS + +#include +#include +#include + +#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 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); + } + } + } +} diff --git a/glass/src/lib/native/include/glass/other/Drive.h b/glass/src/lib/native/include/glass/other/Drive.h new file mode 100644 index 0000000000..33f8687362 --- /dev/null +++ b/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 +#include +#include +#include + +#include + +#include "glass/Model.h" + +struct ImVec2; + +namespace glass { +class DataSource; +class DriveModel : public Model { + public: + struct WheelInfo { + std::string name; + DataSource* percent; + std::function setter; + + WheelInfo(wpi::StringRef name, DataSource* percent, + std::function setter) + : name(name), percent(percent), setter(std::move(setter)) {} + }; + + virtual const char* GetName() const = 0; + virtual const std::vector& 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 diff --git a/glass/src/libnt/native/cpp/NTDifferentialDrive.cpp b/glass/src/libnt/native/cpp/NTDifferentialDrive.cpp new file mode 100644 index 0000000000..fb3d80dca9 --- /dev/null +++ b/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 +#include + +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; +} diff --git a/glass/src/libnt/native/cpp/NTMecanumDrive.cpp b/glass/src/libnt/native/cpp/NTMecanumDrive.cpp new file mode 100644 index 0000000000..6bb88c8102 --- /dev/null +++ b/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 +#include + +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; +} diff --git a/glass/src/libnt/native/cpp/StandardNetworkTables.cpp b/glass/src/libnt/native/cpp/StandardNetworkTables.cpp index 39f981d522..1de9189a65 100644 --- a/glass/src/libnt/native/cpp/StandardNetworkTables.cpp +++ b/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(model)); }); }); + provider.Register( + NTDifferentialDriveModel::kType, + [](NT_Inst inst, const char* path) { + return std::make_unique(inst, path); + }, + [](Window* win, Model* model, const char*) { + win->SetDefaultSize(300, 350); + return MakeFunctionView([=] { + DisplayDrive(static_cast(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(model)); }); }); + provider.Register( + NTMecanumDriveModel::kType, + [](NT_Inst inst, const char* path) { + return std::make_unique(inst, path); + }, + [](Window* win, Model* model, const char*) { + win->SetDefaultSize(300, 350); + return MakeFunctionView( + [=] { DisplayDrive(static_cast(model)); }); + }); provider.Register( NTPIDControllerModel::kType, [](NT_Inst inst, const char* path) { diff --git a/glass/src/libnt/native/include/glass/networktables/NTDifferentialDrive.h b/glass/src/libnt/native/include/glass/networktables/NTDifferentialDrive.h new file mode 100644 index 0000000000..2e7fe7f907 --- /dev/null +++ b/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 +#include + +#include +#include +#include + +#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& 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 m_wheels; + ImVec2 m_speedVector; + double m_rotation; +}; +} // namespace glass diff --git a/glass/src/libnt/native/include/glass/networktables/NTMecanumDrive.h b/glass/src/libnt/native/include/glass/networktables/NTMecanumDrive.h new file mode 100644 index 0000000000..8c2226a21d --- /dev/null +++ b/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 +#include + +#include +#include +#include + +#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& 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 m_wheels; + ImVec2 m_speedVector; + double m_rotation; +}; +} // namespace glass