diff --git a/glass/src/lib/native/cpp/other/Mechanism2D.cpp b/glass/src/lib/native/cpp/other/Mechanism2D.cpp new file mode 100644 index 0000000000..86e7d898a8 --- /dev/null +++ b/glass/src/lib/native/cpp/other/Mechanism2D.cpp @@ -0,0 +1,257 @@ +// 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/Mechanism2D.h" + +#include +#include +#include +#include + +#include +#include +#include +#include + +#define IMGUI_DEFINE_MATH_OPERATORS +#include +#include +#include +#include +#include +#include +#include +#include + +#include "glass/Context.h" + +using namespace glass; + +namespace gui = wpi::gui; + +namespace { + +// Per-frame data (not persistent) +struct FrameData { + frc::Translation2d GetPosFromScreen(const ImVec2& cursor) const { + return { + units::meter_t{(std::clamp(cursor.x, min.x, max.x) - min.x) / scale}, + units::meter_t{(max.y - std::clamp(cursor.y, min.y, max.y)) / scale}}; + } + ImVec2 GetScreenFromPos(const frc::Translation2d& pos) const { + return {min.x + scale * pos.X().to(), + max.y - scale * pos.Y().to()}; + } + void DrawObject(ImDrawList* drawList, MechanismObjectModel& objModel, + const frc::Pose2d& pose) const; + void DrawGroup(ImDrawList* drawList, MechanismObjectGroup& group, + const frc::Pose2d& pose) const; + + // in screen coordinates + ImVec2 imageMin; + ImVec2 imageMax; + ImVec2 min; + ImVec2 max; + + float scale; // scaling from meters to screen units +}; + +class BackgroundInfo { + public: + BackgroundInfo(); + + void DisplaySettings(); + + void LoadImage(); + FrameData GetFrameData(ImVec2 min, ImVec2 max, frc::Translation2d dims) const; + void Draw(ImDrawList* drawList, const FrameData& frameData, + ImU32 bgColor) const; + + private: + void Reset(); + bool LoadImageImpl(const char* fn); + + std::unique_ptr m_fileOpener; + + std::string* m_pFilename; + gui::Texture m_texture; + + // in image pixels + int m_imageWidth; + int m_imageHeight; +}; + +} // namespace + +BackgroundInfo::BackgroundInfo() { + auto& storage = GetStorage(); + m_pFilename = storage.GetStringRef("image"); +} + +void BackgroundInfo::DisplaySettings() { + if (ImGui::Button("Choose image...")) { + m_fileOpener = std::make_unique( + "Choose background image", "", + std::vector{"Image File", + "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif " + "*.hdr *.pic *.ppm *.pgm"}); + } + if (ImGui::Button("Reset background image")) { + Reset(); + } +} + +void BackgroundInfo::Reset() { + m_texture = gui::Texture{}; + m_pFilename->clear(); + m_imageWidth = 0; + m_imageHeight = 0; +} + +void BackgroundInfo::LoadImage() { + if (m_fileOpener && m_fileOpener->ready(0)) { + auto result = m_fileOpener->result(); + if (!result.empty()) { + LoadImageImpl(result[0].c_str()); + } + m_fileOpener.reset(); + } + if (!m_texture && !m_pFilename->empty()) { + if (!LoadImageImpl(m_pFilename->c_str())) { + m_pFilename->clear(); + } + } +} + +bool BackgroundInfo::LoadImageImpl(const char* fn) { + wpi::outs() << "GUI: loading background image '" << fn << "'\n"; + auto texture = gui::Texture::CreateFromFile(fn); + if (!texture) { + wpi::errs() << "GUI: could not read background image\n"; + return false; + } + m_texture = std::move(texture); + m_imageWidth = m_texture.GetWidth(); + m_imageHeight = m_texture.GetHeight(); + *m_pFilename = fn; + return true; +} + +FrameData BackgroundInfo::GetFrameData(ImVec2 min, ImVec2 max, + frc::Translation2d dims) const { + // fit the image into the window + if (m_texture && m_imageHeight != 0 && m_imageWidth != 0) { + gui::MaxFit(&min, &max, m_imageWidth, m_imageHeight); + } + + FrameData frameData; + frameData.imageMin = min; + frameData.imageMax = max; + + // determine the "active area" + float width = dims.X().to(); + float height = dims.Y().to(); + gui::MaxFit(&min, &max, width, height); + + frameData.min = min; + frameData.max = max; + frameData.scale = (max.x - min.x) / width; + return frameData; +} + +void BackgroundInfo::Draw(ImDrawList* drawList, const FrameData& frameData, + ImU32 bgColor) const { + if (m_texture && m_imageHeight != 0 && m_imageWidth != 0) { + drawList->AddImage(m_texture, frameData.imageMin, frameData.imageMax); + } else { + drawList->AddRectFilled(frameData.min, frameData.max, bgColor); + } +} + +void glass::DisplayMechanism2DSettings(Mechanism2DModel* model) { + auto& storage = GetStorage(); + auto bg = storage.GetData(); + if (!bg) { + storage.SetData(std::make_shared()); + bg = storage.GetData(); + } + bg->DisplaySettings(); +} + +void FrameData::DrawObject(ImDrawList* drawList, MechanismObjectModel& objModel, + const frc::Pose2d& pose) const { + const char* type = objModel.GetType(); + if (wpi::StringRef{type} == "line") { + auto startPose = + pose + frc::Transform2d{frc::Translation2d{}, objModel.GetAngle()}; + auto endPose = + startPose + + frc::Transform2d{frc::Translation2d{objModel.GetLength(), 0_m}, 0_deg}; + drawList->AddLine(GetScreenFromPos(startPose.Translation()), + GetScreenFromPos(endPose.Translation()), + objModel.GetColor(), objModel.GetWeight()); + DrawGroup(drawList, objModel, endPose); + } +} + +void FrameData::DrawGroup(ImDrawList* drawList, MechanismObjectGroup& group, + const frc::Pose2d& pose) const { + group.ForEachObject( + [&](auto& objModel) { DrawObject(drawList, objModel, pose); }); +} + +void glass::DisplayMechanism2D(Mechanism2DModel* model, + const ImVec2& contentSize) { + auto& storage = GetStorage(); + auto bg = storage.GetData(); + if (!bg) { + storage.SetData(std::make_shared()); + bg = storage.GetData(); + } + + if (contentSize.x <= 0 || contentSize.y <= 0) { + return; + } + + // screen coords + ImVec2 cursorPos = ImGui::GetWindowPos() + ImGui::GetCursorPos(); + + ImGui::InvisibleButton("background", contentSize); + + // auto mousePos = ImGui::GetIO().MousePos; + auto drawList = ImGui::GetWindowDrawList(); + // bool isHovered = ImGui::IsItemHovered(); + + // background + bg->LoadImage(); + auto frameData = bg->GetFrameData(cursorPos, cursorPos + contentSize, + model->GetDimensions()); + bg->Draw(drawList, frameData, model->GetBackgroundColor()); + + // elements + model->ForEachRoot([&](auto& rootModel) { + frameData.DrawGroup(drawList, rootModel, + frc::Pose2d{rootModel.GetPosition(), 0_deg}); + }); + +#if 0 + if (target) { + // show tooltip and highlight + auto pos = frameData.GetPosFromScreen(target->poseCenter); + ImGui::SetTooltip( + "%s[%d]\nx: %0.3f y: %0.3f rot: %0.3f", target->name.c_str(), + static_cast(target->index), ConvertDisplayLength(pos.X()), + ConvertDisplayLength(pos.Y()), ConvertDisplayAngle(target->rot)); + } +#endif +} + +void Mechanism2DView::Display() { + if (ImGui::BeginPopupContextItem()) { + DisplayMechanism2DSettings(m_model); + ImGui::EndPopup(); + } + DisplayMechanism2D(m_model, ImGui::GetWindowContentRegionMax() - + ImGui::GetWindowContentRegionMin()); +} diff --git a/glass/src/lib/native/include/glass/other/Mechanism2D.h b/glass/src/lib/native/include/glass/other/Mechanism2D.h new file mode 100644 index 0000000000..58af961143 --- /dev/null +++ b/glass/src/lib/native/include/glass/other/Mechanism2D.h @@ -0,0 +1,63 @@ +// 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 "glass/Model.h" +#include "glass/View.h" + +namespace glass { + +class MechanismObjectModel; + +class MechanismObjectGroup { + public: + virtual const char* GetName() const = 0; + virtual void ForEachObject( + wpi::function_ref func) = 0; +}; + +class MechanismObjectModel : public MechanismObjectGroup { + public: + virtual const char* GetType() const = 0; + virtual ImU32 GetColor() const = 0; + + // line accessors + virtual double GetWeight() const = 0; + virtual frc::Rotation2d GetAngle() const = 0; + virtual units::meter_t GetLength() const = 0; +}; + +class MechanismRootModel : public MechanismObjectGroup { + public: + virtual frc::Translation2d GetPosition() const = 0; +}; + +class Mechanism2DModel : public Model { + public: + virtual frc::Translation2d GetDimensions() const = 0; + virtual ImU32 GetBackgroundColor() const = 0; + virtual void ForEachRoot( + wpi::function_ref func) = 0; +}; + +void DisplayMechanism2D(Mechanism2DModel* model, const ImVec2& contentSize); +void DisplayMechanism2DSettings(Mechanism2DModel* model); + +class Mechanism2DView : public View { + public: + explicit Mechanism2DView(Mechanism2DModel* model) : m_model{model} {} + + void Display() override; + + private: + Mechanism2DModel* m_model; +}; + +} // namespace glass diff --git a/glass/src/libnt/native/cpp/NTMechanism2D.cpp b/glass/src/libnt/native/cpp/NTMechanism2D.cpp new file mode 100644 index 0000000000..99a17e3ef3 --- /dev/null +++ b/glass/src/libnt/native/cpp/NTMechanism2D.cpp @@ -0,0 +1,297 @@ +// 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/NTMechanism2D.h" + +#include +#include + +#include +#include + +#include "glass/other/Mechanism2D.h" + +using namespace glass; + +// Convert "#RRGGBB" hex color to ImU32 color +static void ConvertColor(wpi::StringRef in, ImU32* out) { + if (in.size() != 7 || in[0] != '#') { + return; + } + ImU32 val = 0; + if (in.drop_front().getAsInteger(16, val)) { + return; + } + *out = IM_COL32((val >> 16) & 0xff, (val >> 8) & 0xff, val & 0xff, 255); +} + +namespace { + +class NTMechanismObjectModel; + +class NTMechanismGroupImpl final { + public: + NTMechanismGroupImpl(NT_Inst inst, const wpi::Twine& path, + wpi::StringRef name) + : m_inst{inst}, m_path{path.str()}, m_name{name} {} + + const char* GetName() const { return m_name.c_str(); } + void ForEachObject(wpi::function_ref func); + void NTUpdate(const nt::EntryNotification& event, wpi::StringRef name); + + protected: + NT_Inst m_inst; + std::string m_path; + std::string m_name; + std::vector> m_objects; +}; + +class NTMechanismObjectModel final : public MechanismObjectModel { + public: + NTMechanismObjectModel(NT_Inst inst, const wpi::Twine& path, + wpi::StringRef name) + : m_group{inst, path, name}, + m_type{nt::GetEntry(inst, path + "/.type")}, + m_color{nt::GetEntry(inst, path + "/color")}, + m_weight{nt::GetEntry(inst, path + "/weight")}, + m_angle{nt::GetEntry(inst, path + "/angle")}, + m_length{nt::GetEntry(inst, path + "/length")} {} + + const char* GetName() const final { return m_group.GetName(); } + void ForEachObject( + wpi::function_ref func) final { + m_group.ForEachObject(func); + } + + const char* GetType() const final { return m_typeValue.c_str(); } + ImU32 GetColor() const final { return m_colorValue; } + double GetWeight() const final { return m_weightValue; } + frc::Rotation2d GetAngle() const final { return m_angleValue; } + units::meter_t GetLength() const final { return m_lengthValue; } + + bool NTUpdate(const nt::EntryNotification& event, wpi::StringRef childName); + + private: + NTMechanismGroupImpl m_group; + + NT_Entry m_type; + NT_Entry m_color; + NT_Entry m_weight; + NT_Entry m_angle; + NT_Entry m_length; + + std::string m_typeValue; + ImU32 m_colorValue = IM_COL32_WHITE; + double m_weightValue = 1.0; + frc::Rotation2d m_angleValue; + units::meter_t m_lengthValue = 0.0_m; +}; + +} // namespace + +void NTMechanismGroupImpl::ForEachObject( + wpi::function_ref func) { + for (auto&& obj : m_objects) { + func(*obj); + } +} + +void NTMechanismGroupImpl::NTUpdate(const nt::EntryNotification& event, + wpi::StringRef name) { + if (name.empty()) { + return; + } + wpi::StringRef childName; + std::tie(name, childName) = name.split('/'); + if (childName.empty()) { + return; + } + + auto it = std::lower_bound( + m_objects.begin(), m_objects.end(), name, + [](const auto& e, wpi::StringRef name) { return e->GetName() < name; }); + bool match = it != m_objects.end() && (*it)->GetName() == name; + + if (event.flags & NT_NOTIFY_NEW) { + if (!match) { + it = m_objects.emplace(it, std::make_unique( + m_inst, m_path + "/" + name, name)); + match = true; + } + } + if (match) { + if ((*it)->NTUpdate(event, childName)) { + m_objects.erase(it); + } + } +} + +bool NTMechanismObjectModel::NTUpdate(const nt::EntryNotification& event, + wpi::StringRef childName) { + if (event.entry == m_type) { + if ((event.flags & NT_NOTIFY_DELETE) != 0) { + return true; + } + if (event.value && event.value->IsString()) { + m_typeValue = event.value->GetString(); + } + } else if (event.entry == m_color) { + if (event.value && event.value->IsString()) { + ConvertColor(event.value->GetString(), &m_colorValue); + } + } else if (event.entry == m_weight) { + if (event.value && event.value->IsDouble()) { + m_weightValue = event.value->GetDouble(); + } + } else if (event.entry == m_angle) { + if (event.value && event.value->IsDouble()) { + m_angleValue = units::degree_t{event.value->GetDouble()}; + } + } else if (event.entry == m_length) { + if (event.value && event.value->IsDouble()) { + m_lengthValue = units::meter_t{event.value->GetDouble()}; + } + } else { + m_group.NTUpdate(event, childName); + } + return false; +} + +class NTMechanism2DModel::RootModel final : public MechanismRootModel { + public: + RootModel(NT_Inst inst, const wpi::Twine& path, wpi::StringRef name) + : m_group{inst, path, name}, + m_x{nt::GetEntry(inst, path + "/x")}, + m_y{nt::GetEntry(inst, path + "/y")} {} + + const char* GetName() const final { return m_group.GetName(); } + void ForEachObject( + wpi::function_ref func) final { + m_group.ForEachObject(func); + } + + bool NTUpdate(const nt::EntryNotification& event, wpi::StringRef childName); + + frc::Translation2d GetPosition() const override { return m_pos; }; + + private: + NTMechanismGroupImpl m_group; + NT_Entry m_x; + NT_Entry m_y; + frc::Translation2d m_pos; +}; + +bool NTMechanism2DModel::RootModel::NTUpdate(const nt::EntryNotification& event, + wpi::StringRef childName) { + if ((event.flags & NT_NOTIFY_DELETE) != 0 && + (event.entry == m_x || event.entry == m_y)) { + return true; + } else if (event.entry == m_x) { + if (event.value && event.value->IsDouble()) { + m_pos = frc::Translation2d{units::meter_t{event.value->GetDouble()}, + m_pos.Y()}; + } + } else if (event.entry == m_y) { + if (event.value && event.value->IsDouble()) { + m_pos = frc::Translation2d{m_pos.X(), + units::meter_t{event.value->GetDouble()}}; + } + } else { + m_group.NTUpdate(event, childName); + } + return false; +} + +NTMechanism2DModel::NTMechanism2DModel(wpi::StringRef path) + : NTMechanism2DModel{nt::GetDefaultInstance(), path} {} + +NTMechanism2DModel::NTMechanism2DModel(NT_Inst inst, wpi::StringRef path) + : m_nt{inst}, + m_path{(path + "/").str()}, + m_name{m_nt.GetEntry(path + "/.name")}, + m_dimensions{m_nt.GetEntry(path + "/dims")}, + m_bgColor{m_nt.GetEntry(path + "/backgroundColor")}, + m_dimensionsValue{1_m, 1_m} { + m_nt.AddListener(m_path, NT_NOTIFY_LOCAL | NT_NOTIFY_NEW | NT_NOTIFY_DELETE | + NT_NOTIFY_UPDATE | NT_NOTIFY_IMMEDIATE); +} + +NTMechanism2DModel::~NTMechanism2DModel() = default; + +void NTMechanism2DModel::Update() { + for (auto&& event : m_nt.PollListener()) { + // .name + if (event.entry == m_name) { + if (event.value && event.value->IsString()) { + m_nameValue = event.value->GetString(); + } + continue; + } + + // dims + if (event.entry == m_dimensions) { + if (event.value && event.value->IsDoubleArray()) { + auto arr = event.value->GetDoubleArray(); + if (arr.size() == 2) { + m_dimensionsValue = frc::Translation2d{units::meter_t{arr[0]}, + units::meter_t{arr[1]}}; + } + } + } + + // backgroundColor + if (event.entry == m_bgColor) { + if (event.value && event.value->IsString()) { + ConvertColor(event.value->GetString(), &m_bgColorValue); + } + } + + if (wpi::StringRef{event.name}.startswith(m_path)) { + auto name = wpi::StringRef{event.name}.drop_front(m_path.size()); + if (name.empty() || name[0] == '.') { + continue; + } + wpi::StringRef childName; + std::tie(name, childName) = name.split('/'); + if (childName.empty()) { + continue; + } + + auto it = std::lower_bound(m_roots.begin(), m_roots.end(), name, + [](const auto& e, wpi::StringRef name) { + return e->GetName() < name; + }); + bool match = it != m_roots.end() && (*it)->GetName() == name; + + if (event.flags & NT_NOTIFY_NEW) { + if (!match) { + it = + m_roots.emplace(it, std::make_unique( + m_nt.GetInstance(), m_path + name, name)); + match = true; + } + } + if (match) { + if ((*it)->NTUpdate(event, childName)) { + m_roots.erase(it); + } + } + } + } +} + +bool NTMechanism2DModel::Exists() { + return m_nt.IsConnected() && nt::GetEntryType(m_name) != NT_UNASSIGNED; +} + +bool NTMechanism2DModel::IsReadOnly() { + return false; +} + +void NTMechanism2DModel::ForEachRoot( + wpi::function_ref func) { + for (auto&& obj : m_roots) { + func(*obj); + } +} diff --git a/glass/src/libnt/native/cpp/StandardNetworkTables.cpp b/glass/src/libnt/native/cpp/StandardNetworkTables.cpp index 1de9189a65..0a5f234422 100644 --- a/glass/src/libnt/native/cpp/StandardNetworkTables.cpp +++ b/glass/src/libnt/native/cpp/StandardNetworkTables.cpp @@ -11,6 +11,7 @@ #include "glass/networktables/NTField2D.h" #include "glass/networktables/NTGyro.h" #include "glass/networktables/NTMecanumDrive.h" +#include "glass/networktables/NTMechanism2D.h" #include "glass/networktables/NTPIDController.h" #include "glass/networktables/NTSpeedController.h" #include "glass/networktables/NTStringChooser.h" @@ -117,6 +118,18 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { return MakeFunctionView( [=] { DisplayDrive(static_cast(model)); }); }); + provider.Register( + NTMechanism2DModel::kType, + [](NT_Inst inst, const char* path) { + return std::make_unique(inst, path); + }, + [=](Window* win, Model* model, const char* path) { + win->SetDefaultPos(400, 400); + win->SetDefaultSize(200, 200); + win->SetPadding(0, 0); + return std::make_unique( + static_cast(model)); + }); provider.Register( NTPIDControllerModel::kType, [](NT_Inst inst, const char* path) { diff --git a/glass/src/libnt/native/include/glass/networktables/NTMechanism2D.h b/glass/src/libnt/native/include/glass/networktables/NTMechanism2D.h new file mode 100644 index 0000000000..5157b08345 --- /dev/null +++ b/glass/src/libnt/native/include/glass/networktables/NTMechanism2D.h @@ -0,0 +1,60 @@ +// 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 +#include + +#include "glass/networktables/NetworkTablesHelper.h" +#include "glass/other/Mechanism2D.h" + +namespace glass { + +class NTMechanism2DModel : public Mechanism2DModel { + public: + static constexpr const char* kType = "Mechanism2d"; + + // path is to the table containing ".type", excluding the trailing / + explicit NTMechanism2DModel(wpi::StringRef path); + NTMechanism2DModel(NT_Inst inst, wpi::StringRef path); + ~NTMechanism2DModel() override; + + const char* GetPath() const { return m_path.c_str(); } + const char* GetName() const { return m_nameValue.c_str(); } + + void Update() override; + bool Exists() override; + bool IsReadOnly() override; + + frc::Translation2d GetDimensions() const override { + return m_dimensionsValue; + } + ImU32 GetBackgroundColor() const override { return m_bgColorValue; } + void ForEachRoot( + wpi::function_ref func) override; + + private: + NetworkTablesHelper m_nt; + std::string m_path; + + NT_Entry m_name; + NT_Entry m_dimensions; + NT_Entry m_bgColor; + + std::string m_nameValue; + frc::Translation2d m_dimensionsValue; + ImU32 m_bgColorValue = 0; + + class RootModel; + std::vector> m_roots; +}; + +} // namespace glass diff --git a/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.cpp b/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.cpp deleted file mode 100644 index 01dd90fa04..0000000000 --- a/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.cpp +++ /dev/null @@ -1,330 +0,0 @@ -// 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 "Mechanism2D.h" - -#include -#include - -#include -#include -#include - -#define IMGUI_DEFINE_MATH_OPERATORS -#include -#include -#include -#include -#include -#include - -#include "HALSimGui.h" - -using namespace halsimgui; - -static HAL_SimDeviceHandle devHandle = 0; -static wpi::StringMap colorLookUpTable; -static std::unique_ptr m_fileOpener; -static std::string previousJsonLocation = "Not empty"; -namespace { -struct BodyConfig { - std::string name; - std::string type = "line"; - int length = 100; - std::string color = "green"; - int angle = 0; - std::vector children; - int lineWidth = 1; -}; -} // namespace -static std::vector bodyConfigVector; -namespace { -struct DrawLineStruct { - float xEnd; - float yEnd; - float angle; -}; -} // namespace -static struct NamedColor { - const char* name; - ImColor value; -} staticColors[] = {{"white", IM_COL32(255, 255, 255, 255)}, - {"silver", IM_COL32(192, 192, 192, 255)}, - {"gray", IM_COL32(128, 128, 128, 255)}, - {"black", IM_COL32(0, 0, 0, 255)}, - {"red", IM_COL32(255, 0, 0, 255)}, - {"maroon", IM_COL32(128, 0, 0, 255)}, - {"yellow", IM_COL32(255, 255, 0, 255)}, - {"olive", IM_COL32(128, 128, 0, 255)}, - {"lime", IM_COL32(0, 255, 0, 255)}, - {"green", IM_COL32(0, 128, 0, 255)}, - {"aqua", IM_COL32(0, 255, 255, 255)}, - {"teal", IM_COL32(0, 128, 128, 255)}, - {"blue", IM_COL32(0, 0, 255, 255)}, - {"navy", IM_COL32(0, 0, 128, 255)}, - {"fuchsia", IM_COL32(255, 0, 255, 255)}, - {"purple", IM_COL32(128, 0, 128, 255)}}; - -static void buildColorTable() { - for (auto&& namedColor : staticColors) { - colorLookUpTable.try_emplace(namedColor.name, namedColor.value); - } -} -namespace { -class Mechanism2DInfo { - public: - std::string jsonLocation; -}; -} // namespace - -static Mechanism2DInfo mechanism2DInfo; - -bool ReadIni(wpi::StringRef name, wpi::StringRef value) { - if (name == "jsonLocation") { - mechanism2DInfo.jsonLocation = value; - } else { - return false; - } - return true; -} - -void WriteIni(ImGuiTextBuffer* out) { - out->appendf("[Mechanism2D][Mechanism2D]\njsonLocation=%s\n\n", - mechanism2DInfo.jsonLocation.c_str()); -} - -// read/write settings to ini file -static void* Mechanism2DReadOpen(ImGuiContext* ctx, - ImGuiSettingsHandler* handler, - const char* name) { - if (name == wpi::StringRef{"Mechanism2D"}) { - return &mechanism2DInfo; - } - return nullptr; -} - -static void Mechanism2DReadLine(ImGuiContext* ctx, - ImGuiSettingsHandler* handler, void* entry, - const char* lineStr) { - wpi::StringRef line{lineStr}; - auto [name, value] = line.split('='); - name = name.trim(); - value = value.trim(); - if (entry == &mechanism2DInfo) { - ReadIni(name, value); - } -} - -static void Mechanism2DWriteAll(ImGuiContext* ctx, - ImGuiSettingsHandler* handler, - ImGuiTextBuffer* out_buf) { - WriteIni(out_buf); -} - -static void GetJsonFileLocation() { - if (m_fileOpener && m_fileOpener->ready(0)) { - auto result = m_fileOpener->result(); - if (!result.empty()) { - mechanism2DInfo.jsonLocation = result[0]; - } else { - wpi::errs() << "Can not find json file!!!"; - } - } -} - -DrawLineStruct DrawLine(float startXLocation, float startYLocation, int length, - float angle, ImDrawList* drawList, ImVec2 windowPos, - ImColor color, const BodyConfig& bodyConfig, - const std::string& previousPath) { - DrawLineStruct drawLineStruct; - drawLineStruct.angle = angle; - // Find the current path do the ligament - std::string currentPath = previousPath + bodyConfig.name; - // Find the angle in radians - double radAngle = (drawLineStruct.angle - 90) * wpi::math::pi / 180; - // Get the start X and Y location - drawLineStruct.xEnd = startXLocation + length * std::cos(radAngle); - drawLineStruct.yEnd = startYLocation + length * std::sin(radAngle); - // Add the line to the drawList - drawList->AddLine( - windowPos + ImVec2(startXLocation, startYLocation), - windowPos + ImVec2(drawLineStruct.xEnd, drawLineStruct.yEnd), color, - bodyConfig.lineWidth); - // Return the end X, Y, and angle - return drawLineStruct; -} - -static void buildDrawList(float startXLocation, float startYLocation, - ImDrawList* drawList, float previousAngle, - const std::vector& subBodyConfigs, - ImVec2 windowPos) { - for (BodyConfig const& bodyConfig : subBodyConfigs) { - hal::SimDouble angleHandle; - hal::SimDouble lengthHandle; - float angle = 0; - float length = 0; - // Get the smallest of width or height - double minSize; - // Find the min size of the window - minSize = ImGui::GetWindowHeight() > ImGui::GetWindowWidth() - ? ImGui::GetWindowWidth() - : ImGui::GetWindowHeight(); - if (devHandle == 0) { - devHandle = HALSIM_GetSimDeviceHandle("Mechanism2D"); - } - // Get the length - if (!lengthHandle) { - lengthHandle = HALSIM_GetSimValueHandle( - devHandle, (bodyConfig.name + "/length").c_str()); - } - if (lengthHandle) { - length = lengthHandle.Get(); - } - if (length <= 0) { - length = bodyConfig.length; - } - // Get the angle - if (!angleHandle) { - angleHandle = HALSIM_GetSimValueHandle( - devHandle, (bodyConfig.name + "/angle").c_str()); - } - if (angleHandle) { - angle = angleHandle.Get(); - } - // Calculate the next angle to go to - float angleToGoTo = angle + bodyConfig.angle + previousAngle; - // Draw the first line and get the ending coordinates - - DrawLineStruct drawLine = - DrawLine(startXLocation, startYLocation, minSize / 100 * length, - angleToGoTo, drawList, windowPos, - colorLookUpTable[bodyConfig.color], bodyConfig, ""); - - // If the line has children then draw them with the stating points being the - // end of the parent - if (!bodyConfig.children.empty()) { - buildDrawList(drawLine.xEnd, drawLine.yEnd, drawList, drawLine.angle, - bodyConfig.children, windowPos); - } - } -} - -static BodyConfig readSubJson(const std::string& name, wpi::json const& body) { - BodyConfig c; - try { - c.name = name + "/" + body.at("name").get(); - } catch (const wpi::json::exception& e) { - wpi::errs() << "could not read body name: " << e.what() << '\n'; - } - try { - c.length = body.at("length").get(); - } catch (const wpi::json::exception& e) { - wpi::errs() << "length '" << c.name - << "': could not find length path: " << e.what() << '\n'; - } - try { - c.color = body.at("color").get(); - } catch (const wpi::json::exception& e) { - wpi::errs() << "color '" << c.name - << "': could not find color path: " << e.what() << '\n'; - } - try { - c.angle = body.at("angle").get(); - } catch (const wpi::json::exception& e) { - wpi::errs() << "angle '" << c.name - << "': could not find angle path: " << e.what() << '\n'; - } - try { - c.lineWidth = body.at("lineWidth").get(); - } catch (const wpi::json::exception& e) { - wpi::errs() << "lineWidth '" << c.name - << "': could not find lineWidth path: " << e.what() << '\n'; - } - try { - for (wpi::json const& child : body.at("children")) { - c.children.push_back(readSubJson(c.name, child)); - wpi::outs() << "Reading Child with name " << c.name << '\n'; - } - } catch (const wpi::json::exception& e) { - wpi::errs() << "could not read body: " << e.what() << '\n'; - } - return c; -} - -static void readJson(std::string jFile) { - std::error_code ec; - std::string name; - wpi::raw_fd_istream is(jFile, ec); - if (ec) { - wpi::errs() << "could not open '" << jFile << "': " << ec.message() << '\n'; - } - // parse file - wpi::json j; - try { - j = wpi::json::parse(is); - } catch (const wpi::json::parse_error& e) { - wpi::errs() << "byte " << e.byte << ": " << e.what() << '\n'; - } - // top level must be an object - if (!j.is_object()) { - wpi::errs() << "must be JSON object\n"; - } - try { - name = j.at("name").get(); - } catch (const wpi::json::exception& e) { - wpi::errs() << "name '" << name - << "': could not find name path: " << e.what() << '\n'; - } - try { - for (wpi::json const& body : j.at("body")) { - bodyConfigVector.push_back(readSubJson(name, body)); - } - } catch (const wpi::json::exception& e) { - wpi::errs() << "could not read body: " << e.what() << '\n'; - } -} - -static void DisplayAssembly2D() { - if (ImGui::BeginPopupContextItem()) { - if (ImGui::MenuItem("Load Json")) { - m_fileOpener = std::make_unique( - "Choose Mechanism2D json", "", std::vector{"*.json"}); - } - ImGui::EndPopup(); - } - - GetJsonFileLocation(); - if (!mechanism2DInfo.jsonLocation.empty()) { - // Only read the json file if it changed - if (mechanism2DInfo.jsonLocation != previousJsonLocation) { - bodyConfigVector.clear(); - readJson(mechanism2DInfo.jsonLocation); - } - previousJsonLocation = mechanism2DInfo.jsonLocation; - ImVec2 windowPos = ImGui::GetWindowPos(); - ImDrawList* drawList = ImGui::GetWindowDrawList(); - buildDrawList(ImGui::GetWindowWidth() / 2, ImGui::GetWindowHeight(), - drawList, 0, bodyConfigVector, windowPos); - } -} - -void Mechanism2D::Initialize() { - // hook ini handler to save settings - ImGuiSettingsHandler iniHandler; - iniHandler.TypeName = "Mechanism2D"; - iniHandler.TypeHash = ImHashStr(iniHandler.TypeName); - iniHandler.ReadOpenFn = Mechanism2DReadOpen; - iniHandler.ReadLineFn = Mechanism2DReadLine; - iniHandler.WriteAllFn = Mechanism2DWriteAll; - ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler); - - buildColorTable(); - if (auto win = - HALSimGui::manager.AddWindow("Mechanism 2D", DisplayAssembly2D)) { - win->SetVisibility(glass::Window::kHide); - win->SetDefaultPos(200, 200); - win->SetDefaultSize(600, 600); - win->SetPadding(0, 0); - } -} diff --git a/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.h b/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.h deleted file mode 100644 index 499c95c250..0000000000 --- a/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.h +++ /dev/null @@ -1,14 +0,0 @@ -// 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 - -namespace halsimgui { - -class Mechanism2D { - public: - static void Initialize(); -}; - -} // namespace halsimgui diff --git a/simulation/halsim_gui/src/main/native/cpp/main.cpp b/simulation/halsim_gui/src/main/native/cpp/main.cpp index 26fb2554a9..2950b4d6df 100644 --- a/simulation/halsim_gui/src/main/native/cpp/main.cpp +++ b/simulation/halsim_gui/src/main/native/cpp/main.cpp @@ -21,7 +21,6 @@ #include "DriverStationGui.h" #include "EncoderSimGui.h" #include "HALSimGui.h" -#include "Mechanism2D.h" #include "NetworkTablesSimGui.h" #include "PCMSimGui.h" #include "PDPSimGui.h" @@ -60,7 +59,6 @@ __declspec(dllexport) gui::AddInit(AnalogInputSimGui::Initialize); gui::AddInit(AnalogOutputSimGui::Initialize); gui::AddInit(DIOSimGui::Initialize); - gui::AddInit(Mechanism2D::Initialize); gui::AddInit(NetworkTablesSimGui::Initialize); gui::AddInit(PCMSimGui::Initialize); gui::AddInit(PDPSimGui::Initialize); diff --git a/wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp b/wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp deleted file mode 100644 index 30b5ce21d7..0000000000 --- a/wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// 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 "frc/simulation/Mechanism2D.h" - -#include -#include -#include - -using namespace frc; - -Mechanism2D::Mechanism2D() : m_device{"Mechanism2D"} {} - -void Mechanism2D::SetLigamentAngle(const wpi::Twine& ligamentPath, - float angle) { - if (m_device) { - wpi::SmallString<64> fullPathBuf; - wpi::StringRef fullPath = - (ligamentPath + "/angle").toNullTerminatedStringRef(fullPathBuf); - if (!createdItems.count(fullPath)) { - createdItems[fullPath] = - m_device.CreateDouble(fullPath.data(), false, angle); - } - createdItems[fullPath].Set(angle); - } -} - -void Mechanism2D::SetLigamentLength(const wpi::Twine& ligamentPath, - float length) { - if (m_device) { - wpi::SmallString<64> fullPathBuf; - wpi::StringRef fullPath = - (ligamentPath + "/length").toNullTerminatedStringRef(fullPathBuf); - if (!createdItems.count(fullPath)) { - createdItems[fullPath] = - m_device.CreateDouble(fullPath.data(), false, length); - } - createdItems[fullPath].Set(length); - } -} diff --git a/wpilibc/src/main/native/cpp/smartdashboard/Mechanism2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/Mechanism2d.cpp new file mode 100644 index 0000000000..38f73f71dc --- /dev/null +++ b/wpilibc/src/main/native/cpp/smartdashboard/Mechanism2d.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 "frc/smartdashboard/Mechanism2d.h" + +#include + +#include "frc/smartdashboard/SendableBuilder.h" + +using namespace frc; + +static constexpr char kBackgroundColor[] = "backgroundColor"; +static constexpr char kDims[] = "dims"; + +Mechanism2d::Mechanism2d(double width, double height, + const Color8Bit& backgroundColor) + : m_width{width}, m_height{height} { + SetBackgroundColor(backgroundColor); +} + +MechanismRoot2d* Mechanism2d::GetRoot(wpi::StringRef name, double x, double y) { + auto& obj = m_roots[name]; + if (obj) { + return obj.get(); + } + obj = std::make_unique(name, x, y, + MechanismRoot2d::private_init{}); + if (m_table) { + obj->Update(m_table->GetSubTable(name)); + } + return obj.get(); +} + +void Mechanism2d::SetBackgroundColor(const Color8Bit& color) { + std::snprintf(m_color, sizeof(m_color), "#%02X%02X%02X", color.red, + color.green, color.blue); + if (m_table) { + m_table->GetEntry(kBackgroundColor).SetString(m_color); + } +} + +void Mechanism2d::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Mechanism2d"); + m_table = builder.GetTable(); + + m_table->GetEntry(kDims).SetDoubleArray({m_width, m_height}); + m_table->GetEntry(kBackgroundColor).SetString(m_color); + + std::scoped_lock lock(m_mutex); + for (const auto& entry : m_roots) { + const auto& root = entry.getValue().get(); + root->Update(m_table->GetSubTable(entry.getKey())); + } +} diff --git a/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp new file mode 100644 index 0000000000..7d42b99c21 --- /dev/null +++ b/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp @@ -0,0 +1,80 @@ +// 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 "frc/smartdashboard/MechanismLigament2d.h" + +#include + +using namespace frc; + +MechanismLigament2d::MechanismLigament2d(const wpi::Twine& name, double length, + units::degree_t angle, + double lineWeight, + const frc::Color8Bit& color) + : MechanismObject2d(name), + m_length{length}, + m_angle{angle.to()}, + m_weight{lineWeight} { + SetColor(color); +} + +void MechanismLigament2d::UpdateEntries(std::shared_ptr table) { + table->GetEntry(".type").SetString("line"); + + m_colorEntry = table->GetEntry("color"); + m_angleEntry = table->GetEntry("angle"); + m_weightEntry = table->GetEntry("weight"); + m_lengthEntry = table->GetEntry("length"); + Flush(); +} + +void MechanismLigament2d::SetColor(const Color8Bit& color) { + std::scoped_lock lock(m_mutex); + std::snprintf(m_color, sizeof(m_color), "#%02X%02X%02X", color.red, + color.green, color.blue); + Flush(); +} + +void MechanismLigament2d::SetAngle(units::degree_t angle) { + std::scoped_lock lock(m_mutex); + m_angle = angle.to(); + Flush(); +} + +void MechanismLigament2d::SetLineWeight(double lineWidth) { + std::scoped_lock lock(m_mutex); + m_weight = lineWidth; + Flush(); +} + +double MechanismLigament2d::GetAngle() { + if (m_angleEntry) { + m_angle = m_angleEntry.GetDouble(0.0); + } + return m_angle; +} + +double MechanismLigament2d::GetLength() { + if (m_lengthEntry) { + m_length = m_lengthEntry.GetDouble(0.0); + } + return m_length; +} + +void MechanismLigament2d::SetLength(double length) { + std::scoped_lock lock(m_mutex); + m_length = length; + Flush(); +} + +#define SAFE_WRITE(data, Type) \ + if (m_##data##Entry) { \ + m_##data##Entry.Set##Type(m_##data); \ + } +void MechanismLigament2d::Flush() { + SAFE_WRITE(color, String) + SAFE_WRITE(angle, Double) + SAFE_WRITE(length, Double) + SAFE_WRITE(weight, Double) +} diff --git a/wpilibc/src/main/native/cpp/smartdashboard/MechanismObject2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/MechanismObject2d.cpp new file mode 100644 index 0000000000..72192c32d4 --- /dev/null +++ b/wpilibc/src/main/native/cpp/smartdashboard/MechanismObject2d.cpp @@ -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. + +#include "frc/smartdashboard/MechanismObject2d.h" + +using namespace frc; + +MechanismObject2d::MechanismObject2d(const wpi::Twine& name) + : m_name{name.str()} {} + +const std::string& MechanismObject2d::GetName() const { + return m_name; +} + +void MechanismObject2d::Update(std::shared_ptr table) { + std::scoped_lock lock(m_mutex); + m_table = table; + UpdateEntries(m_table); + for (const wpi::StringMapEntry>& entry : + m_objects) { + entry.getValue()->Update(m_table->GetSubTable(entry.getKey())); + } +} diff --git a/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp new file mode 100644 index 0000000000..48cc9a78ef --- /dev/null +++ b/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp @@ -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. + +#include "frc/smartdashboard/MechanismRoot2d.h" + +#include "frc/smartdashboard/Sendable.h" +#include "frc/smartdashboard/SendableHelper.h" +#include "frc/util/Color8Bit.h" + +using namespace frc; + +static constexpr char kPosition[] = "pos"; + +MechanismRoot2d::MechanismRoot2d(const wpi::Twine& name, double x, double y, + const private_init&) + : MechanismObject2d(name.str()), m_x{x}, m_y{y} {} + +void MechanismRoot2d::SetPosition(double x, double y) { + std::scoped_lock lock(m_mutex); + m_x = x; + m_y = y; + Flush(); +} + +void MechanismRoot2d::UpdateEntries(std::shared_ptr table) { + m_posEntry = table->GetEntry(kPosition); + Flush(); +} + +inline void MechanismRoot2d::Flush() { + if (m_posEntry) { + m_posEntry.SetDoubleArray({m_x, m_y}); + } +} diff --git a/wpilibc/src/main/native/include/frc/simulation/Mechanism2D.h b/wpilibc/src/main/native/include/frc/simulation/Mechanism2D.h deleted file mode 100644 index 57be484a0d..0000000000 --- a/wpilibc/src/main/native/include/frc/simulation/Mechanism2D.h +++ /dev/null @@ -1,42 +0,0 @@ -// 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 "frc/geometry/Pose2d.h" -#include "frc/geometry/Rotation2d.h" - -namespace frc { -class Mechanism2D { - public: - Mechanism2D(); - - /** - * Set/Create the angle of a ligament - * - * @param ligamentPath json path to the ligament - * @param angle to set the ligament - */ - void SetLigamentAngle(const wpi::Twine& ligamentPath, float angle); - - /** - * Set/Create the length of a ligament - * - * @param ligamentPath json path to the ligament - * @param length to set the ligament - */ - void SetLigamentLength(const wpi::Twine& ligamentPath, float length); - - private: - wpi::StringMap createdItems; - hal::SimDevice m_device; -}; - -} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/Mechanism2d.h b/wpilibc/src/main/native/include/frc/smartdashboard/Mechanism2d.h new file mode 100644 index 0000000000..9311b54641 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/smartdashboard/Mechanism2d.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 +#include + +#include +#include +#include + +#include "frc/smartdashboard/MechanismRoot2d.h" +#include "frc/smartdashboard/Sendable.h" +#include "frc/smartdashboard/SendableHelper.h" +#include "frc/util/Color8Bit.h" + +namespace frc { + +/** + * Visual 2D representation of arms, elevators, and general mechanisms; through + * a node-based API. + * + * A Mechanism2d object is published and contains at least one root node. Other + * nodes (such as ligaments) are recursively based on other nodes. + * + * Except for the Mechanism2d container object, none of the objects should be + * passed or interacted with by value! Obtain pointers from factory methods such + * as Mechanism2d.GetRoot() and MechanismObject2d.Append<>(). The Mechanism2d + * container object owns the root nodes, and each node internally owns the nodes + * based on it. Beware not to let the Mechanism2d object out of scope - all + * nodes will be recursively destructed! + * + * @see MechanismObject2d + * @see MechanismLigament2d + * @see MechanismRoot2d + */ +class Mechanism2d : public Sendable, public SendableHelper { + public: + /** + * Create a new Mechanism2d with the given dimensions and background color. + * + * @param width the width + * @param height the height + */ + Mechanism2d(double width, double height, + const Color8Bit& backgroundColor = {0, 0, 32}); + + /** + * Get or create a root in this Mechanism2d with the given name and + * position. + * + *

If a root with the given name already exists, the given x and y + * coordinates are not used. + * + * @param name the root name + * @param x the root x coordinate + * @param y the root y coordinate + * @return a new root object, or the existing one with the given name. + */ + MechanismRoot2d* GetRoot(wpi::StringRef name, double x, double y); + + /** + * Set the Mechanism2d background color. + * + * @param color the new background color + */ + void SetBackgroundColor(const Color8Bit& color); + + void InitSendable(SendableBuilder& builder) override; + + private: + double m_width; + double m_height; + char m_color[10]; + mutable wpi::mutex m_mutex; + std::shared_ptr m_table; + wpi::StringMap> m_roots; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h b/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h new file mode 100644 index 0000000000..dd47fd0937 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h @@ -0,0 +1,84 @@ +// 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 "frc/smartdashboard/MechanismObject2d.h" +#include "frc/util/Color8Bit.h" + +namespace frc { + +/** + * Ligament node on a Mechanism2d. + * + * @see Mechanism2d + */ +class MechanismLigament2d : public MechanismObject2d { + public: + MechanismLigament2d(const wpi::Twine& name, double length, + units::degree_t angle, double lineWidth = 6, + const frc::Color8Bit& color = {235, 137, 52}); + + /** + * Set the ligament color. + * + * @param color the color of the line + */ + void SetColor(const frc::Color8Bit& color); + + /** + * Set the ligament's length. + * + * @param length the line length + */ + void SetLength(double length); + + /** + * Get the ligament length. + * + * @return the line length + */ + double GetLength(); + + /** + * Set the ligament's angle relative to its parent. + * + * @param degrees the angle + */ + void SetAngle(units::degree_t angle); + + /** + * Get the ligament's angle relative to its parent. + * + * @return the angle + */ + double GetAngle(); + + /** + * Set the line thickness. + * + * @param weight the line thickness + */ + void SetLineWeight(double lineWidth); + + protected: + void UpdateEntries(std::shared_ptr table) override; + + private: + void Flush(); + double m_length; + nt::NetworkTableEntry m_lengthEntry; + double m_angle; + nt::NetworkTableEntry m_angleEntry; + double m_weight; + nt::NetworkTableEntry m_weightEntry; + char m_color[10]; + nt::NetworkTableEntry m_colorEntry; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/MechanismObject2d.h b/wpilibc/src/main/native/include/frc/smartdashboard/MechanismObject2d.h new file mode 100644 index 0000000000..ad0d9860ea --- /dev/null +++ b/wpilibc/src/main/native/include/frc/smartdashboard/MechanismObject2d.h @@ -0,0 +1,88 @@ +// 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 +#include +#include + +namespace frc { + +/** + * Common base class for all Mechanism2d node types. + * + * To append another node, call Append with the type of node and its + * construction parameters. None of the node types are designed to be + * constructed directly, and are owned by their parent node/container - obtain + * pointers from the Append function or similar factory methods. + * + * @see Mechanism2d. + */ +class MechanismObject2d { + friend class Mechanism2d; + + protected: + explicit MechanismObject2d(const wpi::Twine& name); + + /** + * Update all entries with new ones from a new table. + * + * @param table the new table. + */ + virtual void UpdateEntries(std::shared_ptr table) = 0; + + mutable wpi::mutex m_mutex; + + public: + virtual ~MechanismObject2d() = default; + + /** + * Retrieve the object's name. + * + * @return the object's name relative to its parent. + */ + const std::string& GetName() const; + + /** + * Append a Mechanism object that is based on this one. + * + * @param name the name of the new object. + * @param args constructor arguments of the object type. + * @return the constructed and appended object, useful for variable + * assignments and call chaining. + * @throw if an object with the given name already exists. + */ + template >> + T* Append(wpi::StringRef name, Args&&... args) { + std::scoped_lock lock(m_mutex); + auto& obj = m_objects[name]; + if (obj) { + throw std::runtime_error(("MechanismObject names must be unique! `" + + name + "` was inserted twice!") + .str()); + } + obj = std::make_unique(name, std::forward(args)...); + T* ex = static_cast(obj.get()); + if (m_table) { + ex->Update(m_table->GetSubTable(name)); + } + return ex; + } + + private: + std::string m_name; + wpi::StringMap> m_objects; + std::shared_ptr m_table; + void Update(std::shared_ptr table); +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/MechanismRoot2d.h b/wpilibc/src/main/native/include/frc/smartdashboard/MechanismRoot2d.h new file mode 100644 index 0000000000..12b27a5781 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/smartdashboard/MechanismRoot2d.h @@ -0,0 +1,49 @@ +// 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 "MechanismObject2d.h" + +namespace frc { + +/** + * Root Mechanism2d node. + * + * Do not create objects of this class directly! Obtain pointers from the + * Mechanism2d.GetRoot() factory method. + */ +class MechanismRoot2d : private MechanismObject2d { + friend class Mechanism2d; + struct private_init {}; + + public: + MechanismRoot2d(const wpi::Twine& name, double x, double y, + const private_init&); + + /** + * Set the root's position. + * + * @param x new x coordinate + * @param y new y coordinate + */ + void SetPosition(double x, double y); + + using MechanismObject2d::GetName; + + using MechanismObject2d::Append; + + private: + void UpdateEntries(std::shared_ptr table) override; + inline void Flush(); + double m_x; + double m_y; + nt::NetworkTableEntry m_posEntry; +}; +} // namespace frc diff --git a/wpilibcExamples/src/main/cpp/examples/Mechanism2d/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Mechanism2d/cpp/Robot.cpp new file mode 100644 index 0000000000..407d3e0799 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Mechanism2d/cpp/Robot.cpp @@ -0,0 +1,73 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * This sample program shows how to use Mechanism2d - a visual representation of + * arms, elevators, and other mechanisms on dashboards; driven by a node-based + * API. + * + *

Ligaments are based on other ligaments or roots, and roots are contained + * in the base Mechanism2d object. Use pointers for nodes, and beware not to let + * the container out of scope - the appended nodes will be recursively + * destructed! + */ +class Robot : public frc::TimedRobot { + static constexpr double kMetersPerPulse = 0.01; + static constexpr double kElevatorMinimumLength = 0.5; + + public: + void RobotInit() override { + m_elevatorEncoder.SetDistancePerPulse(kMetersPerPulse); + + // publish to dashboard + frc::SmartDashboard::PutData("Mech2d", &m_mech); + } + + void RobotPeriodic() override { + // update the dashboard mechanism's state + m_elevator->SetLength(kElevatorMinimumLength + + m_elevatorEncoder.GetDistance()); + m_wrist->SetAngle(units::degree_t(m_wristPotentiometer.Get())); + } + + void TeleopPeriodic() override { + m_elevatorMotor.Set(m_joystick.GetRawAxis(0)); + m_wristMotor.Set(m_joystick.GetRawAxis(1)); + } + + private: + frc::PWMSparkMax m_elevatorMotor{0}; + frc::PWMSparkMax m_wristMotor{1}; + frc::Encoder m_elevatorEncoder{0, 1}; + frc::AnalogPotentiometer m_wristPotentiometer{1, 90}; + frc::Joystick m_joystick{0}; + + // the main mechanism object + frc::Mechanism2d m_mech{3, 3}; + // the mechanism root node + frc::MechanismRoot2d* m_root = m_mech.GetRoot("climber", 3, 3); + // MechanismLigament2d objects represent each "section"/"stage" of the + // mechanism, and are based off the root node or another ligament object + frc::MechanismLigament2d* m_elevator = + m_root->Append("elevator", 1, 90_deg); + frc::MechanismLigament2d* m_wrist = + m_elevator->Append("wrist", 0.5, 90_deg); +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index f533d36259..1db36011e1 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -51,6 +51,14 @@ "gradlebase": "cpp", "commandversion": 2 }, + { + "name":"Mechanism2d", + "foldername":"Mechanism2d", + "gradlebase":"cpp", + "description":"An example usage of Mechanism2d to display mechanism states on a dashboard.", + "tags":[], + "commandversion": 2 + }, { "name": "Solenoids", "description": "Demonstrate controlling a single and double solenoid from Joystick buttons.", diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Mechanism2D.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Mechanism2D.java deleted file mode 100644 index 5d722a5f1a..0000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Mechanism2D.java +++ /dev/null @@ -1,50 +0,0 @@ -// 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. - -package edu.wpi.first.wpilibj.simulation; - -import edu.wpi.first.hal.SimDevice; -import edu.wpi.first.hal.SimDevice.Direction; -import edu.wpi.first.hal.SimDouble; -import java.util.HashMap; -import java.util.Map; - -public class Mechanism2D { - private static final SimDevice m_device = SimDevice.create("Mechanism2D"); - private static final Map m_createdItems = new HashMap(); - - /** - * Set/Create the angle of a ligament. - * - * @param ligamentPath json path to the ligament - * @param angle to set the ligament - */ - public void setLigamentAngle(String ligamentPath, float angle) { - ligamentPath = ligamentPath + "/angle"; - if (m_device != null) { - if (!m_createdItems.containsKey(ligamentPath)) { - m_createdItems.put( - ligamentPath, m_device.createDouble(ligamentPath, Direction.kInput, angle)); - } - m_createdItems.get(ligamentPath).set(angle); - } - } - - /** - * Set/Create the length of a ligament. - * - * @param ligamentPath json path to the ligament - * @param length to set the ligament - */ - public void setLigamentLength(String ligamentPath, float length) { - ligamentPath = ligamentPath + "/length"; - if (m_device != null) { - if (!m_createdItems.containsKey(ligamentPath)) { - m_createdItems.put( - ligamentPath, m_device.createDouble(ligamentPath, Direction.kInput, length)); - } - m_createdItems.get(ligamentPath).set(length); - } - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Mechanism2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Mechanism2d.java new file mode 100644 index 0000000000..3646efeab9 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Mechanism2d.java @@ -0,0 +1,107 @@ +// 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. + +package edu.wpi.first.wpilibj.smartdashboard; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.util.Color8Bit; +import java.util.HashMap; +import java.util.Map; +import java.util.Map.Entry; + +/** + * Visual 2D representation of arms, elevators, and general mechanisms; through a node-based API. + * + *

A Mechanism2d object is published and contains at least one root node. Other nodes (such as + * ligaments) are recursively based on other nodes. + * + * @see MechanismObject2d + * @see MechanismLigament2d + * @see MechanismRoot2d + */ +public final class Mechanism2d implements Sendable { + private static final String kBackgroundColor = "backgroundColor"; + private NetworkTable m_table; + private final Map m_roots; + private final double[] m_dims = new double[2]; + private String m_color; + + /** + * Create a new Mechanism2d with the given dimensions and default color (dark blue). + * + * @param width the width + * @param height the height + */ + public Mechanism2d(double width, double height) { + this(width, height, new Color8Bit(0, 0, 32)); + } + + /** + * Create a new Mechanism2d with the given dimensions. + * + * @param width the width + * @param height the height + * @param backgroundColor the background color. Defaults to dark blue. + */ + public Mechanism2d(double width, double height, Color8Bit backgroundColor) { + m_roots = new HashMap<>(); + m_dims[0] = width; + m_dims[1] = height; + setBackgroundColor(backgroundColor); + } + + /** + * Get or create a root in this Mechanism2d with the given name and position. + * + *

If a root with the given name already exists, the given x and y coordinates are not used. + * + * @param name the root name + * @param x the root x coordinate + * @param y the root y coordinate + * @return a new root joint object, or the existing one with the given name. + */ + public synchronized MechanismRoot2d getRoot(String name, double x, double y) { + var existing = m_roots.get(name); + if (existing != null) { + return existing; + } + + var root = new MechanismRoot2d(name, x, y); + m_roots.put(name, root); + if (m_table != null) { + root.update(m_table.getSubTable(name)); + } + return root; + } + + /** + * Set the Mechanism2d background color. + * + * @param color the new color + */ + public synchronized void setBackgroundColor(Color8Bit color) { + this.m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue); + if (m_table != null) { + m_table.getEntry(kBackgroundColor).setString(m_color); + } + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Mechanism2d"); + m_table = builder.getTable(); + m_table.getEntry("dims").setDoubleArray(m_dims); + m_table.getEntry(kBackgroundColor).setString(m_color); + synchronized (this) { + for (Entry entry : m_roots.entrySet()) { + String name = entry.getKey(); + MechanismRoot2d root = entry.getValue(); + synchronized (root) { + root.update(m_table.getSubTable(name)); + } + } + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java new file mode 100644 index 0000000000..1a2dda0bb4 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java @@ -0,0 +1,140 @@ +// 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. + +package edu.wpi.first.wpilibj.smartdashboard; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.util.Color8Bit; + +/** + * Ligament node on a Mechanism2d. + * + * @see Mechanism2d + */ +public class MechanismLigament2d extends MechanismObject2d { + private double m_angle; + private NetworkTableEntry m_angleEntry; + private String m_color; + private NetworkTableEntry m_colorEntry; + private double m_length; + private NetworkTableEntry m_lengthEntry; + private double m_weight; + private NetworkTableEntry m_weightEntry; + + /** Create a new ligament. */ + public MechanismLigament2d( + String name, double length, double angle, double lineWidth, Color8Bit color) { + super(name); + setColor(color); + setLength(length); + setAngle(angle); + setLineWeight(lineWidth); + } + + /** Create a new ligament with the default color (dark blue) and thickness (6). */ + public MechanismLigament2d(String name, double length, double angle) { + this(name, length, angle, 10, new Color8Bit(235, 137, 52)); + } + + /** + * Set the ligament's angle relative to its parent. + * + * @param degrees the angle, in degrees + */ + public synchronized void setAngle(double degrees) { + m_angle = degrees; + flush(); + } + + /** + * Set the ligament's angle relative to its parent. + * + * @param angle the angle + */ + public synchronized void setAngle(Rotation2d angle) { + setAngle(angle.getDegrees()); + } + + /** + * Get the ligament's angle relative to its parent. + * + * @return the angle, in degrees + */ + public synchronized double getAngle() { + if (m_angleEntry != null) { + m_angle = m_angleEntry.getDouble(0.0); + } + return m_angle; + } + + /** + * Set the ligament's length. + * + * @param length the line length + */ + public synchronized void setLength(double length) { + m_length = length; + flush(); + } + + /** + * Get the ligament length. + * + * @return the line length + */ + public synchronized double getLength() { + if (m_lengthEntry != null) { + m_length = m_lengthEntry.getDouble(0.0); + } + return m_length; + } + + /** + * Set the ligament color. + * + * @param color the color of the line + */ + public synchronized void setColor(Color8Bit color) { + m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue); + flush(); + } + + /** + * Set the line thickness. + * + * @param weight the line thickness + */ + public synchronized void setLineWeight(double weight) { + m_weight = weight; + flush(); + } + + @Override + protected void updateEntries(NetworkTable table) { + table.getEntry(".type").setString("line"); + m_angleEntry = table.getEntry("angle"); + m_lengthEntry = table.getEntry("length"); + m_colorEntry = table.getEntry("color"); + m_weightEntry = table.getEntry("weight"); + flush(); + } + + /** Flush latest data to NT. */ + private synchronized void flush() { + if (m_angleEntry != null) { + m_angleEntry.setDouble(m_angle); + } + if (m_lengthEntry != null) { + m_lengthEntry.setDouble(m_length); + } + if (m_colorEntry != null) { + m_colorEntry.setString(m_color); + } + if (m_weightEntry != null) { + m_weightEntry.setDouble(m_weight); + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismObject2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismObject2d.java new file mode 100644 index 0000000000..e470789910 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismObject2d.java @@ -0,0 +1,72 @@ +// 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. + +package edu.wpi.first.wpilibj.smartdashboard; + +import edu.wpi.first.networktables.NetworkTable; +import java.util.HashMap; +import java.util.Map; + +/** + * Common base class for all Mechanism2d node types. + * + *

To append another node, call {@link #append(MechanismObject2d)}. Objects that aren't appended + * to a published {@link Mechanism2d} container are nonfunctional. + * + * @see Mechanism2d + */ +public abstract class MechanismObject2d { + /** Relative to parent. */ + private final String m_name; + + private NetworkTable m_table; + private final Map m_objects = new HashMap<>(1); + + /** + * Create a new Mechanism node object. + * + * @param name the node's name, must be unique. + */ + protected MechanismObject2d(String name) { + m_name = name; + } + + /** + * Append a Mechanism object that is based on this one. + * + * @param object the object to add. + * @return the object given as a parameter, useful for variable assignments and call chaining. + * @throws UnsupportedOperationException if the object's name is already used - object names must + * be unique. + */ + public final synchronized T append(T object) { + if (m_objects.containsKey(object.getName())) { + throw new UnsupportedOperationException("Mechanism object names must be unique!"); + } + m_objects.put(object.getName(), object); + if (m_table != null) { + object.update(m_table.getSubTable(object.getName())); + } + return object; + } + + final void update(NetworkTable table) { + m_table = table; + updateEntries(m_table); + for (MechanismObject2d obj : m_objects.values()) { + obj.update(m_table.getSubTable(obj.m_name)); + } + } + + /** + * Update all entries with new ones from a new table. + * + * @param table the new table. + */ + protected abstract void updateEntries(NetworkTable table); + + public final String getName() { + return m_name; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismRoot2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismRoot2d.java new file mode 100644 index 0000000000..6edca1fe01 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismRoot2d.java @@ -0,0 +1,94 @@ +// 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. + +package edu.wpi.first.wpilibj.smartdashboard; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import java.util.HashMap; +import java.util.Map; + +/** + * Root Mechanism2d node. + * + *

Do not create objects of this class directly! Obtain instances from the {@link + * Mechanism2d#getRoot(String, double, double)} factory method. + * + *

Append other nodes by using {@link #append(MechanismObject2d)}. + */ +public final class MechanismRoot2d { + private final String m_name; + private NetworkTable m_table; + private final Map m_objects = new HashMap<>(1); + private double m_x; + private NetworkTableEntry m_xEntry; + private double m_y; + private NetworkTableEntry m_yEntry; + + /** + * Package-private constructor for roots. + * + * @param name name + * @param x x coordinate of root (provide only when constructing a root node) + * @param y y coordinate of root (provide only when constructing a root node) + */ + MechanismRoot2d(String name, double x, double y) { + m_name = name; + m_x = x; + m_y = y; + } + + /** + * Append a Mechanism object that is based on this one. + * + * @param object the object to add. + * @return the object given as a parameter, useful for variable assignments and call chaining. + * @throws UnsupportedOperationException if the object's name is already used - object names must + * be unique. + */ + public synchronized T append(T object) { + if (m_objects.containsKey(object.getName())) { + throw new UnsupportedOperationException("Mechanism object names must be unique!"); + } + m_objects.put(object.getName(), object); + if (m_table != null) { + object.update(m_table.getSubTable(object.getName())); + } + return object; + } + + /** + * Set the root's position. + * + * @param x new x coordinate + * @param y new y coordinate + */ + public synchronized void setPosition(double x, double y) { + m_x = x; + m_y = y; + } + + void update(NetworkTable table) { + m_table = table; + m_xEntry = m_table.getEntry("x"); + m_yEntry = m_table.getEntry("y"); + flush(); + for (MechanismObject2d obj : m_objects.values()) { + obj.update(m_table.getSubTable(obj.getName())); + } + } + + public String getName() { + return m_name; + } + + private void flush() { + if (m_xEntry != null) { + m_xEntry.setDouble(m_x); + } + if (m_yEntry != null) { + m_yEntry.setDouble(m_y); + } + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index f188f103ea..110988d4cc 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -209,6 +209,14 @@ "mainclass": "Main", "commandversion": 2 }, + {"name": "Mechanism2d", + "description": "An example usage of Mechanism2d to display mechanism states on a dashboard.", + "tags": [], + "foldername": "mechanism2d", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "Motor Controller", "description": "Demonstrate controlling a single motor with a joystick", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Main.java new file mode 100644 index 0000000000..a477a03490 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Main.java @@ -0,0 +1,25 @@ +// 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. + +package edu.wpi.first.wpilibj.examples.mechanism2d; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Robot.java new file mode 100644 index 0000000000..a7b8644786 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Robot.java @@ -0,0 +1,67 @@ +// 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. + +package edu.wpi.first.wpilibj.examples.mechanism2d; + +import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PWMSparkMax; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * This sample program shows how to use Mechanism2d - a visual representation of arms, elevators, + * and other mechanisms on dashboards; driven by a node-based API. + * + *

Ligaments are based on other ligaments or roots, and roots are contained in the base + * Mechanism2d object. + */ +public class Robot extends TimedRobot { + private static final double kMetersPerPulse = 0.01; + private static final double kElevatorMinimumLength = 0.5; + + private final PWMSparkMax m_elevatorMotor = new PWMSparkMax(0); + private final PWMSparkMax m_wristMotor = new PWMSparkMax(1); + private final AnalogPotentiometer m_wristPot = new AnalogPotentiometer(1, 90); + private final Encoder m_elevatorEncoder = new Encoder(0, 1); + private final Joystick m_joystick = new Joystick(0); + + private MechanismLigament2d m_elevator; + private MechanismLigament2d m_wrist; + + @Override + public void robotInit() { + m_elevatorEncoder.setDistancePerPulse(kMetersPerPulse); + + // the main mechanism object + Mechanism2d mech = new Mechanism2d(200, 200); + // the mechanism root node + MechanismRoot2d root = mech.getRoot("climber", 80, 100); + + // MechanismLigament2d objects represent each "section"/"stage" of the mechanism, and are based + // off the root node or another ligament object + m_elevator = root.append(new MechanismLigament2d("elevator", 10, 90)); + m_wrist = m_elevator.append(new MechanismLigament2d("wrist", 6, 90)); + + // post the mechanism to the dashboard + SmartDashboard.putData("Mech2d", mech); + } + + @Override + public void robotPeriodic() { + // update the dashboard mechanism's state + m_elevator.setLength(kElevatorMinimumLength + m_elevatorEncoder.getDistance()); + m_wrist.setAngle(m_wristPot.get()); + } + + @Override + public void teleopPeriodic() { + m_elevatorMotor.set(m_joystick.getRawAxis(0)); + m_wristMotor.set(m_joystick.getRawAxis(1)); + } +}