YAMS
Yet Another Mechanism System
Loading...
Searching...
No Matches
SwerveInputStreamTelemetry.hpp
Go to the documentation of this file.
1// Copyright (c) 2026 Yet Another Software Suite
2// SPDX-License-Identifier: LGPL-3.0-or-later
3
4#pragma once
5
6#include <cmath>
7#include <functional>
8#include <limits>
9#include <optional>
10#include <string>
11#include <string_view>
12#include <unordered_map>
13#include <wpi/commands2/CommandPtr.hpp>
14#include <wpi/commands2/Commands.hpp>
15#include <wpi/nt/NetworkTable.hpp>
16#include <wpi/nt/NetworkTableInstance.hpp>
17#include <wpi/tunables/Tunables.hpp>
18
24
26
49template <size_t NumModules = 4>
51 public:
53
87
107
112 };
113
123 TelemetryVerbosity verbosity)
124 : m_stream{&stream},
125 m_liveTuningPath{"Tuning/SwerveInputStream/" + std::string{name} + "/Live Tuning"} {
126 constexpr double kNaN = std::numeric_limits<double>::quiet_NaN();
127 AddField(DoubleTelemetryField::Deadband, "deadband", 0.0, false, "none");
128 AddField(DoubleTelemetryField::TranslationScale, "translationScale", 1.0, false, "none");
129 AddField(DoubleTelemetryField::RotationScale, "rotationScale", 1.0, false, "none");
130 AddField(DoubleTelemetryField::MaxLinearVelocity, "maxLinearVelocity", 0.0, false,
131 "meters_per_second");
132 AddField(DoubleTelemetryField::MaxAngularVelocity, "maxAngularVelocity", 0.0, false,
133 "radians_per_second");
134 AddField(DoubleTelemetryField::RotationControllerMeasurement, "rotationController/measurement",
135 kNaN, false, "radians");
136 AddField(DoubleTelemetryField::RotationControllerSetpoint, "rotationController/setpoint", kNaN,
137 false, "radians");
138 AddField(DoubleTelemetryField::TunableDeadband, "deadband", 0.0, true, "none");
139 AddField(DoubleTelemetryField::TunableTranslationScale, "translationScale", 1.0, true, "none");
140 AddField(DoubleTelemetryField::TunableRotationScale, "rotationScale", 1.0, true, "none");
141 AddField(DoubleTelemetryField::TunableMaxLinearVelocity, "maxLinearVelocity", 0.0, true,
142 "meters_per_second");
143 AddField(DoubleTelemetryField::TunableMaxAngularVelocity, "maxAngularVelocity", 0.0, true,
144 "radians_per_second");
145 AddField(BooleanTelemetryField::TranslationCube, "translationCube", false);
146 AddField(BooleanTelemetryField::RotationCube, "rotationCube", false);
147 AddField(BooleanTelemetryField::AllianceRelative, "allianceRelative", false);
148 AddField(BooleanTelemetryField::RobotRelative, "robotRelative", false);
149 AddField(BooleanTelemetryField::TunableTranslationCube, "translationCube", true);
150 AddField(BooleanTelemetryField::TunableRotationCube, "rotationCube", true);
151 AddField(BooleanTelemetryField::TunableAllianceRelative, "allianceRelative", true);
152 AddField(BooleanTelemetryField::TunableRobotRelative, "robotRelative", true);
153 m_stringFields.emplace(StringTelemetryField::Mode,
155 "mode", "", StringTelemetryField::Mode, false});
156
157 switch (verbosity) {
158 case TelemetryVerbosity::HIGH:
159 m_doubleFields.at(DoubleTelemetryField::TunableDeadband).Enable();
160 m_doubleFields.at(DoubleTelemetryField::TunableTranslationScale).Enable();
161 m_doubleFields.at(DoubleTelemetryField::TunableRotationScale).Enable();
162 m_doubleFields.at(DoubleTelemetryField::TunableMaxLinearVelocity).Enable();
163 m_doubleFields.at(DoubleTelemetryField::TunableMaxAngularVelocity).Enable();
164 m_boolFields.at(BooleanTelemetryField::TunableTranslationCube).Enable();
165 m_boolFields.at(BooleanTelemetryField::TunableRotationCube).Enable();
166 m_boolFields.at(BooleanTelemetryField::TunableAllianceRelative).Enable();
167 m_boolFields.at(BooleanTelemetryField::TunableRobotRelative).Enable();
168 [[fallthrough]];
169 case TelemetryVerbosity::MEDIUM:
170 m_doubleFields.at(DoubleTelemetryField::Deadband).Enable();
171 m_doubleFields.at(DoubleTelemetryField::TranslationScale).Enable();
172 m_doubleFields.at(DoubleTelemetryField::RotationScale).Enable();
173 m_doubleFields.at(DoubleTelemetryField::MaxLinearVelocity).Enable();
174 m_doubleFields.at(DoubleTelemetryField::MaxAngularVelocity).Enable();
175 m_doubleFields.at(DoubleTelemetryField::RotationControllerMeasurement).Enable();
176 m_doubleFields.at(DoubleTelemetryField::RotationControllerSetpoint).Enable();
177 m_boolFields.at(BooleanTelemetryField::TranslationCube).Enable();
178 m_boolFields.at(BooleanTelemetryField::RotationCube).Enable();
179 m_boolFields.at(BooleanTelemetryField::AllianceRelative).Enable();
180 m_boolFields.at(BooleanTelemetryField::RobotRelative).Enable();
181 [[fallthrough]];
182 case TelemetryVerbosity::LOW:
183 m_stringFields.at(StringTelemetryField::Mode).Enable();
184 break;
185 case TelemetryVerbosity::NONE:
186 break;
187 }
188
189 auto instance = wpi::nt::NetworkTableInstance::GetDefault();
190 auto dataTable = instance.GetTable("SwerveInputStream")->GetSubTable(name);
191 auto tuningTable = instance.GetTable("Tuning")->GetSubTable("SwerveInputStream")->GetSubTable(name);
192 for (auto& [field, dt] : m_doubleFields) {
193 if (dt.IsEnabled()) {
194 dt.SetupNetworkTables(dataTable, tuningTable);
195 // Skip a value an earlier telemetry left on the dashboard, so the stream's own value is
196 // published instead.
197 dt.IsTunable();
198 }
199 }
200 for (auto& [field, bt] : m_boolFields) {
201 if (bt.IsEnabled()) {
202 bt.SetupNetworkTables(dataTable, tuningTable);
203 bt.IsTunable();
204 }
205 }
206 for (auto& [field, st] : m_stringFields) {
207 if (st.IsEnabled()) {
208 st.SetupNetworkTables(dataTable, tuningTable);
209 }
210 }
211
212 if (verbosity == TelemetryVerbosity::HIGH) {
213 // Publish the stream's values to the Tuning table.
215 // No requirements, so tuning does not interrupt the command driving with the stream.
216 m_liveTuningCommand.emplace(
217 wpi::cmd::Run([this] { ApplyTuningValues(); }).WithName("Live Tuning"));
219 wpi::tunables::Publish(m_liveTuningPath, *m_liveTuningCommand->get());
220 }
222 }
223
226
229 if (m_liveTuningCommand) {
230 m_liveTuningCommand->Cancel();
231 wpi::tunables::Remove(m_liveTuningPath);
232 }
233 for (auto& [field, dt] : m_doubleFields) {
234 dt.Close();
235 }
236 for (auto& [field, bt] : m_boolFields) {
237 bt.Close();
238 }
239 for (auto& [field, st] : m_stringFields) {
240 st.Close();
241 }
242 }
243
249 for (auto& [field, st] : m_stringFields) {
250 if (!st.IsEnabled()) continue;
251 switch (field) {
253 st.Set(m_stream->GetCurrentModeName());
254 break;
255 }
256 }
257 for (auto& [field, dt] : m_doubleFields) {
258 if (!dt.IsEnabled()) continue;
259 switch (field) {
261 dt.Set(m_stream->GetAxisDeadband());
262 break;
264 dt.Set(m_stream->GetTranslationAxisScale());
265 break;
267 dt.Set(m_stream->GetOmegaAxisScale());
268 break;
270 dt.Set(m_stream->GetMaximumChassisLinearVelocity().value());
271 break;
273 dt.Set(m_stream->GetMaximumChassisAngularVelocity().value());
274 break;
276 auto measurement = m_stream->GetRotationControllerMeasurement();
277 dt.Set(measurement ? measurement->value() : std::numeric_limits<double>::quiet_NaN());
278 break;
279 }
281 auto setpoint = m_stream->GetRotationControllerSetpoint();
282 dt.Set(setpoint ? setpoint->value() : std::numeric_limits<double>::quiet_NaN());
283 break;
284 }
285 default:
286 // Tunable fields are published by ApplyTuningValues.
287 break;
288 }
289 }
290 for (auto& [field, bt] : m_boolFields) {
291 if (!bt.IsEnabled()) continue;
292 switch (field) {
294 bt.Set(m_stream->IsTranslationCubeEnabled());
295 break;
297 bt.Set(m_stream->IsOmegaCubeEnabled());
298 break;
300 bt.Set(m_stream->IsAllianceRelativeEnabled());
301 break;
303 bt.Set(m_stream->IsRobotRelativeEnabled());
304 break;
305 default:
306 // Tunable fields are published by ApplyTuningValues.
307 break;
308 }
309 }
310 }
311
317 for (auto& [field, dt] : m_doubleFields) {
318 if (!dt.IsEnabled()) continue;
319 switch (field) {
321 if (dt.IsTunable() && dt.Get() >= 0.0 && dt.Get() < 1.0) {
322 m_stream->SetAxisDeadband(dt.Get());
323 }
324 PublishStreamValue(dt, m_stream->GetAxisDeadband());
325 break;
327 if (dt.IsTunable() && dt.Get() > 0.0 && dt.Get() <= 1.0) {
328 m_stream->SetTranslationAxisScale(dt.Get());
329 }
330 PublishStreamValue(dt, m_stream->GetTranslationAxisScale());
331 break;
333 if (dt.IsTunable() && dt.Get() > 0.0 && dt.Get() <= 1.0) {
334 m_stream->SetOmegaAxisScale(dt.Get());
335 }
336 PublishStreamValue(dt, m_stream->GetOmegaAxisScale());
337 break;
339 if (dt.IsTunable() && dt.Get() > 0.0 && std::isfinite(dt.Get())) {
340 m_stream->SetMaximumChassisLinearVelocity(wpi::units::meters_per_second_t{dt.Get()});
341 }
342 PublishStreamValue(dt, m_stream->GetMaximumChassisLinearVelocity().value());
343 break;
345 if (dt.IsTunable() && dt.Get() > 0.0 && std::isfinite(dt.Get())) {
346 m_stream->SetMaximumChassisAngularVelocity(wpi::units::radians_per_second_t{dt.Get()});
347 }
348 PublishStreamValue(dt, m_stream->GetMaximumChassisAngularVelocity().value());
349 break;
350 default:
351 // Read-only fields are published by UpdateTelemetry.
352 break;
353 }
354 }
355 for (auto& [field, bt] : m_boolFields) {
356 if (!bt.IsEnabled()) continue;
357 switch (field) {
359 if (bt.IsTunable()) {
360 m_stream->SetTranslationCubeEnabled(bt.Get());
361 }
362 PublishStreamValue(bt, m_stream->IsTranslationCubeEnabled());
363 break;
365 if (bt.IsTunable()) {
366 m_stream->SetOmegaCubeEnabled(bt.Get());
367 }
368 PublishStreamValue(bt, m_stream->IsOmegaCubeEnabled());
369 break;
371 if (bt.IsTunable()) {
372 m_stream->SetAllianceRelativeEnabled(bt.Get());
373 }
374 PublishStreamValue(bt, m_stream->IsAllianceRelativeEnabled());
375 break;
377 if (bt.IsTunable()) {
378 m_stream->SetRobotRelativeEnabled(bt.Get());
379 }
380 PublishStreamValue(bt, m_stream->IsRobotRelativeEnabled());
381 break;
382 default:
383 // Read-only fields are published by UpdateTelemetry.
384 break;
385 }
386 }
387 }
388
395 std::optional<std::reference_wrapper<wpi::cmd::Command>> GetLiveTuningCommand() {
396 if (!m_liveTuningCommand) {
397 return std::nullopt;
398 }
399 return std::ref(*m_liveTuningCommand->get());
400 }
401
402 private:
403 void AddField(DoubleTelemetryField field, std::string key, double defaultValue, bool tunable,
404 std::string unit) {
405 m_doubleFields.emplace(field, telemetry::DoubleTelemetry<DoubleTelemetryField>{
406 std::move(key), defaultValue, field, tunable, std::move(unit)});
407 }
408
409 void AddField(BooleanTelemetryField field, std::string key, bool tunable) {
410 m_boolFields.emplace(field, telemetry::BooleanTelemetry<BooleanTelemetryField>{
411 std::move(key), false, field, tunable});
412 }
413
419 template <typename Telemetry, typename T>
420 static void PublishStreamValue(Telemetry& telemetry, T streamValue) {
421 if (telemetry.Get() != streamValue) {
422 telemetry.ForceSet(streamValue);
423 telemetry.IsTunable();
424 }
425 }
426
427 SwerveInputStream<NumModules>* m_stream;
428 std::string m_liveTuningPath;
429 std::unordered_map<DoubleTelemetryField, telemetry::DoubleTelemetry<DoubleTelemetryField>>
430 m_doubleFields;
431 std::unordered_map<BooleanTelemetryField, telemetry::BooleanTelemetry<BooleanTelemetryField>>
432 m_boolFields;
433 std::unordered_map<StringTelemetryField, telemetry::StringTelemetry<StringTelemetryField>>
434 m_stringFields;
436 std::optional<wpi::cmd::CommandPtr> m_liveTuningCommand;
437};
438
439} // namespace yams::mechanisms::swerve::utility
motorcontrollers::SmartMotorControllerConfig::TelemetryVerbosity TelemetryVerbosity
Definition SwerveDriveConfig.hpp:40
SwerveDriveConfig::TelemetryVerbosity TelemetryVerbosity
Definition SwerveInputStreamTelemetry.hpp:52
~SwerveInputStreamTelemetry()
Definition SwerveInputStreamTelemetry.hpp:228
std::optional< std::reference_wrapper< wpi::cmd::Command > > GetLiveTuningCommand()
Definition SwerveInputStreamTelemetry.hpp:395
void ApplyTuningValues()
Definition SwerveInputStreamTelemetry.hpp:316
SwerveInputStreamTelemetry & operator=(const SwerveInputStreamTelemetry &)=delete
void UpdateTelemetry()
Definition SwerveInputStreamTelemetry.hpp:248
DoubleTelemetryField
Definition SwerveInputStreamTelemetry.hpp:55
SwerveInputStreamTelemetry(SwerveInputStream< NumModules > &stream, std::string_view name, TelemetryVerbosity verbosity)
Definition SwerveInputStreamTelemetry.hpp:122
SwerveInputStreamTelemetry(const SwerveInputStreamTelemetry &)=delete
BooleanTelemetryField
Definition SwerveInputStreamTelemetry.hpp:89
StringTelemetryField
Definition SwerveInputStreamTelemetry.hpp:109
Definition SwerveInputStream.hpp:40
Definition SmartMotorControllerTelemetry.hpp:370
Definition SmartMotorControllerTelemetry.hpp:145
Definition StringTelemetry.hpp:26
Definition SwerveInputStream.hpp:34
void EnsureTuningTunableBackend()
Definition NetworkTablesBackends.hpp:57