mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
85 lines
2.9 KiB
C++
85 lines
2.9 KiB
C++
|
/*----------------------------------------------------------------------------*/
|
||
|
|
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||
|
|
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||
|
|
/* the project. */
|
||
|
|
/*----------------------------------------------------------------------------*/
|
||
|
|
|
||
|
|
#include "glass/hardware/Gyro.h"
|
||
|
|
|
||
|
|
#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::DisplayGyro(GyroModel* m) {
|
||
|
|
static const auto kColorWhite = IM_COL32(255, 255, 255, 255);
|
||
|
|
static const auto kColorGray = IM_COL32(200, 200, 200, 255);
|
||
|
|
static const auto kColorPurple = IM_COL32(200, 200, 255, 255);
|
||
|
|
|
||
|
|
auto angle = m->GetAngleData();
|
||
|
|
if (!angle || !m->Exists()) {
|
||
|
|
ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
|
||
|
|
ImGui::Text("Unknown Gyro");
|
||
|
|
ImGui::PopStyleColor();
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Display the numeric angle value. This can be editable in some cases (i.e.
|
||
|
|
// running from HALSIM).
|
||
|
|
auto flags =
|
||
|
|
m->IsReadOnly() ? ImGuiInputTextFlags_ReadOnly : ImGuiInputTextFlags_None;
|
||
|
|
auto value = angle->GetValue();
|
||
|
|
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
|
||
|
|
if (ImGui::InputDouble("Gyro Angle (Deg)", &value, 0.0, 0.0, "%.4f", flags))
|
||
|
|
m->SetAngle(value);
|
||
|
|
|
||
|
|
// Draw the gyro indicator.
|
||
|
|
ImDrawList* draw = ImGui::GetWindowDrawList();
|
||
|
|
ImVec2 window = ImGui::GetWindowPos();
|
||
|
|
float w = ImGui::GetWindowWidth();
|
||
|
|
float h = ImGui::GetWindowHeight();
|
||
|
|
|
||
|
|
float radius = (w < h) ? w * 0.3 : h * 0.3;
|
||
|
|
ImVec2 center = window + ImVec2(w / 2, h / 2 + ImGui::GetFontSize());
|
||
|
|
|
||
|
|
// Add the primary circle.
|
||
|
|
draw->AddCircle(center, radius, kColorWhite, 100, 1.5);
|
||
|
|
|
||
|
|
// Draw the spokes at every 5 degrees and a "major" spoke every 45 degrees.
|
||
|
|
for (int i = -175; i <= 180; i += 5) {
|
||
|
|
double radians = i * 2 * wpi::math::pi / 360.0;
|
||
|
|
ImVec2 direction(std::sin(radians), -std::cos(radians));
|
||
|
|
|
||
|
|
bool major = i % 45 == 0;
|
||
|
|
auto color = major ? kColorWhite : kColorGray;
|
||
|
|
|
||
|
|
draw->AddLine(center + (direction * radius),
|
||
|
|
center + (direction * radius * (major ? 1.07f : 1.03f)),
|
||
|
|
color, 1.2f);
|
||
|
|
if (major) {
|
||
|
|
char txt[16];
|
||
|
|
std::snprintf(txt, sizeof(txt), "%d°", i);
|
||
|
|
draw->AddText(
|
||
|
|
center + (direction * radius * 1.25) - ImGui::CalcTextSize(txt) * 0.5,
|
||
|
|
kColorWhite, txt, nullptr);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
draw->AddCircleFilled(center, radius * 0.075, kColorPurple, 50);
|
||
|
|
|
||
|
|
double radians = value * 2 * wpi::math::pi / 360.0;
|
||
|
|
draw->AddLine(
|
||
|
|
center - ImVec2(1, 0),
|
||
|
|
center + ImVec2(std::sin(radians), -std::cos(radians)) * radius * 0.95f,
|
||
|
|
kColorPurple, 3);
|
||
|
|
}
|