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>
49template <
size_t NumModules = 4>
125 m_liveTuningPath{
"Tuning/SwerveInputStream/" + std::string{name} +
"/Live Tuning"} {
126 constexpr double kNaN = std::numeric_limits<double>::quiet_NaN();
131 "meters_per_second");
133 "radians_per_second");
135 kNaN,
false,
"radians");
142 "meters_per_second");
144 "radians_per_second");
158 case TelemetryVerbosity::HIGH:
169 case TelemetryVerbosity::MEDIUM:
182 case TelemetryVerbosity::LOW:
185 case TelemetryVerbosity::NONE:
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);
200 for (
auto& [field, bt] : m_boolFields) {
201 if (bt.IsEnabled()) {
202 bt.SetupNetworkTables(dataTable, tuningTable);
206 for (
auto& [field, st] : m_stringFields) {
207 if (st.IsEnabled()) {
208 st.SetupNetworkTables(dataTable, tuningTable);
212 if (verbosity == TelemetryVerbosity::HIGH) {
216 m_liveTuningCommand.emplace(
219 wpi::tunables::Publish(m_liveTuningPath, *m_liveTuningCommand->get());
229 if (m_liveTuningCommand) {
230 m_liveTuningCommand->Cancel();
231 wpi::tunables::Remove(m_liveTuningPath);
233 for (
auto& [field, dt] : m_doubleFields) {
236 for (
auto& [field, bt] : m_boolFields) {
239 for (
auto& [field, st] : m_stringFields) {
249 for (
auto& [field, st] : m_stringFields) {
250 if (!st.IsEnabled())
continue;
253 st.Set(m_stream->GetCurrentModeName());
257 for (
auto& [field, dt] : m_doubleFields) {
258 if (!dt.IsEnabled())
continue;
261 dt.Set(m_stream->GetAxisDeadband());
264 dt.Set(m_stream->GetTranslationAxisScale());
267 dt.Set(m_stream->GetOmegaAxisScale());
270 dt.Set(m_stream->GetMaximumChassisLinearVelocity().value());
273 dt.Set(m_stream->GetMaximumChassisAngularVelocity().value());
276 auto measurement = m_stream->GetRotationControllerMeasurement();
277 dt.Set(measurement ? measurement->value() : std::numeric_limits<double>::quiet_NaN());
281 auto setpoint = m_stream->GetRotationControllerSetpoint();
282 dt.Set(setpoint ? setpoint->value() : std::numeric_limits<double>::quiet_NaN());
290 for (
auto& [field, bt] : m_boolFields) {
291 if (!bt.IsEnabled())
continue;
294 bt.Set(m_stream->IsTranslationCubeEnabled());
297 bt.Set(m_stream->IsOmegaCubeEnabled());
300 bt.Set(m_stream->IsAllianceRelativeEnabled());
303 bt.Set(m_stream->IsRobotRelativeEnabled());
317 for (
auto& [field, dt] : m_doubleFields) {
318 if (!dt.IsEnabled())
continue;
321 if (dt.IsTunable() && dt.Get() >= 0.0 && dt.Get() < 1.0) {
322 m_stream->SetAxisDeadband(dt.Get());
324 PublishStreamValue(dt, m_stream->GetAxisDeadband());
327 if (dt.IsTunable() && dt.Get() > 0.0 && dt.Get() <= 1.0) {
328 m_stream->SetTranslationAxisScale(dt.Get());
330 PublishStreamValue(dt, m_stream->GetTranslationAxisScale());
333 if (dt.IsTunable() && dt.Get() > 0.0 && dt.Get() <= 1.0) {
334 m_stream->SetOmegaAxisScale(dt.Get());
336 PublishStreamValue(dt, m_stream->GetOmegaAxisScale());
339 if (dt.IsTunable() && dt.Get() > 0.0 && std::isfinite(dt.Get())) {
340 m_stream->SetMaximumChassisLinearVelocity(wpi::units::meters_per_second_t{dt.Get()});
342 PublishStreamValue(dt, m_stream->GetMaximumChassisLinearVelocity().value());
345 if (dt.IsTunable() && dt.Get() > 0.0 && std::isfinite(dt.Get())) {
346 m_stream->SetMaximumChassisAngularVelocity(wpi::units::radians_per_second_t{dt.Get()});
348 PublishStreamValue(dt, m_stream->GetMaximumChassisAngularVelocity().value());
355 for (
auto& [field, bt] : m_boolFields) {
356 if (!bt.IsEnabled())
continue;
359 if (bt.IsTunable()) {
360 m_stream->SetTranslationCubeEnabled(bt.Get());
362 PublishStreamValue(bt, m_stream->IsTranslationCubeEnabled());
365 if (bt.IsTunable()) {
366 m_stream->SetOmegaCubeEnabled(bt.Get());
368 PublishStreamValue(bt, m_stream->IsOmegaCubeEnabled());
371 if (bt.IsTunable()) {
372 m_stream->SetAllianceRelativeEnabled(bt.Get());
374 PublishStreamValue(bt, m_stream->IsAllianceRelativeEnabled());
377 if (bt.IsTunable()) {
378 m_stream->SetRobotRelativeEnabled(bt.Get());
380 PublishStreamValue(bt, m_stream->IsRobotRelativeEnabled());
396 if (!m_liveTuningCommand) {
399 return std::ref(*m_liveTuningCommand->get());
406 std::move(key), defaultValue, field, tunable, std::move(unit)});
411 std::move(key),
false, field, tunable});
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();
427 SwerveInputStream<NumModules>* m_stream;
428 std::string m_liveTuningPath;
429 std::unordered_map<DoubleTelemetryField, telemetry::DoubleTelemetry<DoubleTelemetryField>>
431 std::unordered_map<BooleanTelemetryField, telemetry::BooleanTelemetry<BooleanTelemetryField>>
433 std::unordered_map<StringTelemetryField, telemetry::StringTelemetry<StringTelemetryField>>
436 std::optional<wpi::cmd::CommandPtr> m_liveTuningCommand;
motorcontrollers::SmartMotorControllerConfig::TelemetryVerbosity TelemetryVerbosity
Definition SwerveDriveConfig.hpp:40
Definition SmartMotorControllerTelemetry.hpp:370
Definition SmartMotorControllerTelemetry.hpp:145
Definition StringTelemetry.hpp:26
Definition SwerveInputStream.hpp:34
void EnsureTuningTunableBackend()
Definition NetworkTablesBackends.hpp:57