9#include <unordered_map>
12#include <wpi/datalog/DataLog.hpp>
13#include <wpi/nt/BooleanTopic.hpp>
14#include <wpi/nt/DoubleTopic.hpp>
15#include <wpi/nt/NetworkTable.hpp>
16#include <wpi/system/DataLogManager.hpp>
17#include <wpi/util/json.hpp>
125 std::optional<std::vector<BooleanTelemetryField>>
boolFields;
156 DoubleTelemetry(std::string key,
double defaultVal, F field,
bool tunable, std::string unit)
158 m_key{std::move(key)},
160 m_unit{std::move(unit)},
161 m_defaultValue{defaultVal},
162 m_cachedValue{defaultVal} {}
178 std::shared_ptr<wpi::nt::NetworkTable> tuningTable) {
179 m_dataTable = dataTable;
180 m_tuningTable = tuningTable;
181 if (!m_enabled)
return;
183 if (tuningTable && m_tunable) {
184 auto topic = tuningTable->GetDoubleTopic(m_key);
185 if (m_unit !=
"none") {
186 topic.SetProperties(wpi::util::json{{
"units", m_unit}});
188 m_subPublisher = topic.Publish();
189 m_subPublisher->SetDefault(m_defaultValue);
190 m_subscriber = topic.Subscribe(m_defaultValue);
191 }
else if (dataTable) {
192 auto topic = dataTable->GetDoubleTopic(m_key);
193 if (m_unit !=
"none") {
194 topic.SetProperties(wpi::util::json{{
"units", m_unit}});
196 m_publisher = topic.Publish();
197 m_publisher->SetDefault(m_defaultValue);
216 if (m_tunable)
return;
217 std::string path = prefix;
218 if (!path.empty() && path.back() !=
'/') path +=
'/';
219 path += m_unit +
'/' + m_key;
220 m_dataLogEntry = wpi::log::DoubleLogEntry{wpi::DataLogManager::GetLog(), path};
231 if (m_unit ==
"tunable_position") {
232 m_unit = linear ?
"meter" :
"degrees";
233 }
else if (m_unit ==
"position") {
234 m_unit = linear ?
"meter" :
"rotations";
235 }
else if (m_unit ==
"tunable_velocity") {
236 m_unit = linear ?
"meter_per_second" :
"rotations_per_minute";
237 }
else if (m_unit ==
"velocity") {
238 m_unit = linear ?
"meter_per_second" :
"rotation_per_second";
239 }
else if (m_unit ==
"tunable_acceleration") {
240 m_unit = linear ?
"meter_per_second_per_second" :
"rotations_per_minute_per_second";
241 }
else if (m_unit ==
"acceleration") {
242 m_unit = linear ?
"meter_per_second_per_second" :
"rotation_per_second_per_second";
254 if (m_subPublisher) {
255 m_subPublisher->Set(value);
256 }
else if (m_publisher) {
257 m_publisher->Set(value);
268 if (!m_enabled)
return false;
269 if (m_dataLogEntry) {
270 m_dataLogEntry->Append(value);
273 double tuningValue = m_subscriber->Get(m_defaultValue);
274 if (tuningValue != value)
return false;
277 m_publisher->Set(value);
288 if (!m_enabled)
return m_defaultValue;
290 return m_subscriber->Get(m_defaultValue);
292 throw std::runtime_error(
"Tuning table not configured for " + m_key +
"!");
301 if (m_subscriber && m_tunable && m_enabled) {
302 double current = m_subscriber->Get(m_defaultValue);
303 if (current != m_cachedValue) {
304 m_cachedValue = current;
320 void Display(
bool state) { m_enabled = state; }
328 m_subscriber.reset();
329 m_subPublisher.reset();
332 m_dataTable->GetEntry(m_key).Unpublish();
335 m_tuningTable->GetEntry(m_key).Unpublish();
343 bool m_enabled{
false};
345 double m_defaultValue;
346 double m_cachedValue;
348 std::optional<wpi::nt::DoublePublisher> m_publisher;
349 std::optional<wpi::nt::DoubleSubscriber> m_subscriber;
350 std::optional<wpi::nt::DoublePublisher> m_subPublisher;
351 std::optional<wpi::log::DoubleLogEntry> m_dataLogEntry;
353 std::shared_ptr<wpi::nt::NetworkTable> m_tuningTable;
354 std::shared_ptr<wpi::nt::NetworkTable> m_dataTable;
382 m_key{std::move(key)},
384 m_defaultValue{defaultVal},
385 m_cachedValue{defaultVal} {}
401 std::shared_ptr<wpi::nt::NetworkTable> tuningTable) {
402 m_dataTable = dataTable;
403 m_tuningTable = tuningTable;
404 if (!m_enabled)
return;
406 if (tuningTable && m_tunable) {
407 auto topic = tuningTable->GetBooleanTopic(m_key);
408 m_pubSub = topic.Publish();
409 m_pubSub->SetDefault(m_defaultValue);
410 m_subscriber = topic.Subscribe(m_defaultValue);
411 }
else if (dataTable) {
412 auto topic = dataTable->GetBooleanTopic(m_key);
413 m_publisher = topic.Publish();
414 m_publisher->SetDefault(m_defaultValue);
433 if (m_tunable)
return;
434 std::string path = prefix;
435 if (!path.empty() && path.back() !=
'/') path +=
'/';
437 m_dataLogEntry = wpi::log::BooleanLogEntry{wpi::DataLogManager::GetLog(), path};
447 if (!m_enabled)
return false;
448 if (m_dataLogEntry) {
449 m_dataLogEntry->Append(value);
452 bool tuningValue = m_subscriber->Get(m_defaultValue);
453 if (tuningValue != value)
return false;
456 m_publisher->Set(value);
467 if (!m_enabled)
return m_defaultValue;
469 return m_subscriber->Get(m_defaultValue);
471 throw std::runtime_error(
"Tuning table not configured for " + m_key +
"!");
484 m_pubSub->Set(value);
485 }
else if (m_publisher) {
486 m_publisher->Set(value);
496 if (m_subscriber && m_tunable && m_enabled) {
497 bool current = m_subscriber->Get(m_defaultValue);
498 if (current != m_cachedValue) {
499 m_cachedValue = current;
515 void Display(
bool state) { m_enabled = state; }
523 m_subscriber.reset();
527 m_dataTable->GetEntry(m_key).Unpublish();
530 m_tuningTable->GetEntry(m_key).Unpublish();
538 bool m_enabled{
false};
542 std::optional<wpi::nt::BooleanPublisher> m_publisher;
543 std::optional<wpi::nt::BooleanSubscriber> m_subscriber;
544 std::optional<wpi::nt::BooleanPublisher> m_pubSub;
545 std::optional<wpi::log::BooleanLogEntry> m_dataLogEntry;
547 std::shared_ptr<wpi::nt::NetworkTable> m_tuningTable;
548 std::shared_ptr<wpi::nt::NetworkTable> m_dataTable;
575 std::shared_ptr<wpi::nt::NetworkTable> publishTable,
576 std::shared_ptr<wpi::nt::NetworkTable> tuningTable,
580 bool nt4Enabled, std::optional<std::string> dataLogName);
620 std::shared_ptr<wpi::nt::NetworkTable> m_dataTable;
621 std::shared_ptr<wpi::nt::NetworkTable> m_tuningTable;
623 std::unordered_map<DoubleTelemetryField, DoubleTelemetry<DoubleTelemetryField>>* m_doubleFields{
625 std::unordered_map<BooleanTelemetryField, BooleanTelemetry<BooleanTelemetryField>>* m_boolFields{
Definition ADRCConfig.hpp:82
Definition SmartMotorControllerConfig.hpp:66
bool GetLinearClosedLoopControllerUse() const
Definition SmartMotorController.hpp:49
Definition SmartMotorControllerTelemetry.hpp:370
void SetDefaultValue(bool value)
Definition SmartMotorControllerTelemetry.hpp:392
void ForceSet(bool value)
Definition SmartMotorControllerTelemetry.hpp:482
bool Get() const
Definition SmartMotorControllerTelemetry.hpp:466
bool Set(bool value)
Definition SmartMotorControllerTelemetry.hpp:446
void SetupDataLog(const std::string &prefix)
Definition SmartMotorControllerTelemetry.hpp:432
void Close()
Definition SmartMotorControllerTelemetry.hpp:522
bool IsEnabled() const
Definition SmartMotorControllerTelemetry.hpp:519
F GetField() const
Definition SmartMotorControllerTelemetry.hpp:517
void Enable()
Definition SmartMotorControllerTelemetry.hpp:507
void SetupNetworkTables(std::shared_ptr< wpi::nt::NetworkTable > dataTable, std::shared_ptr< wpi::nt::NetworkTable > tuningTable)
Definition SmartMotorControllerTelemetry.hpp:400
void Disable()
Definition SmartMotorControllerTelemetry.hpp:509
void Display(bool state)
Definition SmartMotorControllerTelemetry.hpp:515
bool IsTunable()
Definition SmartMotorControllerTelemetry.hpp:495
void SetupNetworkTable(std::shared_ptr< wpi::nt::NetworkTable > dataTable)
Definition SmartMotorControllerTelemetry.hpp:423
BooleanTelemetry(std::string key, bool defaultVal, F field, bool tunable)
Definition SmartMotorControllerTelemetry.hpp:380
Definition SmartMotorControllerTelemetry.hpp:145
void SetupDataLog(const std::string &prefix)
Definition SmartMotorControllerTelemetry.hpp:215
double Get() const
Definition SmartMotorControllerTelemetry.hpp:287
bool IsTunable()
Definition SmartMotorControllerTelemetry.hpp:300
F GetField() const
Definition SmartMotorControllerTelemetry.hpp:322
void TransformUnit(const motorcontrollers::SmartMotorControllerConfig &cfg)
Definition SmartMotorControllerTelemetry.hpp:229
void Enable()
Definition SmartMotorControllerTelemetry.hpp:312
void SetDefaultValue(double value)
Definition SmartMotorControllerTelemetry.hpp:169
void SetupNetworkTables(std::shared_ptr< wpi::nt::NetworkTable > dataTable, std::shared_ptr< wpi::nt::NetworkTable > tuningTable)
Definition SmartMotorControllerTelemetry.hpp:177
void Disable()
Definition SmartMotorControllerTelemetry.hpp:314
bool Set(double value)
Definition SmartMotorControllerTelemetry.hpp:267
void Display(bool state)
Definition SmartMotorControllerTelemetry.hpp:320
DoubleTelemetry(std::string key, double defaultVal, F field, bool tunable, std::string unit)
Definition SmartMotorControllerTelemetry.hpp:156
void SetupNetworkTable(std::shared_ptr< wpi::nt::NetworkTable > dataTable)
Definition SmartMotorControllerTelemetry.hpp:206
void ForceSet(double value)
Definition SmartMotorControllerTelemetry.hpp:253
bool IsEnabled() const
Definition SmartMotorControllerTelemetry.hpp:324
void Close()
Definition SmartMotorControllerTelemetry.hpp:327
Definition SmartMotorControllerTelemetry.hpp:560
void Publish(motorcontrollers::SmartMotorController &smc)
void SetupTelemetry(motorcontrollers::SmartMotorController &smc, std::shared_ptr< wpi::nt::NetworkTable > publishTable, std::shared_ptr< wpi::nt::NetworkTable > tuningTable, std::unordered_map< DoubleTelemetryField, DoubleTelemetry< DoubleTelemetryField > > &doubleFields, std::unordered_map< BooleanTelemetryField, BooleanTelemetry< BooleanTelemetryField > > &boolFields, bool nt4Enabled, std::optional< std::string > dataLogName)
void StopTuning(motorcontrollers::SmartMotorController &smc)
void ApplyTuningValues(motorcontrollers::SmartMotorController &smc)
bool TuningEnabled() const
Definition ADRCConfig.hpp:20
Definition SwerveDrive.hpp:872
DoubleTelemetryField
Definition SmartMotorControllerTelemetry.hpp:33
@ TrapezoidalProfileMaxAcceleration
Definition SmartMotorControllerTelemetry.hpp:38
@ kG
Gravity feedforward gain kG (tunable).
Definition SmartMotorControllerTelemetry.hpp:46
@ kI
Integral feedback gain kI (tunable).
Definition SmartMotorControllerTelemetry.hpp:48
@ MechanismUpperLimit
Configured upper angular soft limit (degrees, tunable).
Definition SmartMotorControllerTelemetry.hpp:90
@ MechanismAcceleration
Mechanism acceleration in rotations per second squared.
Definition SmartMotorControllerTelemetry.hpp:88
@ ADRCBandwidth
ADRC closed-loop bandwidth, rad/s. ADRC only.
Definition SmartMotorControllerTelemetry.hpp:59
@ ExternalEncoderVelocity
External encoder's mechanism velocity in rotations per second, only with an external encoder.
Definition SmartMotorControllerTelemetry.hpp:95
@ kA
Acceleration feedforward gain kA (tunable).
Definition SmartMotorControllerTelemetry.hpp:45
@ TunableClosedLoopControllerSlot
Live-tunable closed-loop gain slot selector.
Definition SmartMotorControllerTelemetry.hpp:41
@ kP
Proportional feedback gain kP (tunable).
Definition SmartMotorControllerTelemetry.hpp:47
@ SupplyCurrent
Supply (input) current draw (A).
Definition SmartMotorControllerTelemetry.hpp:77
@ StatorCurrent
Stator (output) current draw (A).
Definition SmartMotorControllerTelemetry.hpp:75
@ LQRRelms
LQR control effort (voltage) tolerance, V (tunable).
Definition SmartMotorControllerTelemetry.hpp:52
@ OutputVoltage
Voltage currently applied to the motor (V).
Definition SmartMotorControllerTelemetry.hpp:74
@ ADRCObserverBandwidth
ADRC observer bandwidth, rad/s. ADRC only.
Definition SmartMotorControllerTelemetry.hpp:60
@ VelocityMeasurementDelay
How far behind the measured velocity is, when known (s).
Definition SmartMotorControllerTelemetry.hpp:71
@ ExponentialProfileKV
Exponential profile kV constant (tunable).
Definition SmartMotorControllerTelemetry.hpp:34
@ SetpointForce
Feedforward force applied on the velocity setpoint (N).
Definition SmartMotorControllerTelemetry.hpp:69
@ SupplyCurrentLimit
Configured supply current limit (A, tunable).
Definition SmartMotorControllerTelemetry.hpp:78
@ MeasurementVelocity
Linear measurement velocity (m/s).
Definition SmartMotorControllerTelemetry.hpp:82
@ kV
Velocity feedforward gain kV (tunable).
Definition SmartMotorControllerTelemetry.hpp:44
@ kD
Derivative feedback gain kD (tunable).
Definition SmartMotorControllerTelemetry.hpp:49
@ SetpointPosition
Read-only position setpoint rotations or meters.
Definition SmartMotorControllerTelemetry.hpp:66
@ TunableVoltage
Definition SmartMotorControllerTelemetry.hpp:72
@ MeasurementUpperLimit
Configured upper linear soft limit (m, tunable).
Definition SmartMotorControllerTelemetry.hpp:85
@ ClosedLoopPeriod
Period the closed-loop controller runs at (s).
Definition SmartMotorControllerTelemetry.hpp:70
@ ExternalEncoderPosition
External encoder's mechanism position in rotations, only with an external encoder.
Definition SmartMotorControllerTelemetry.hpp:94
@ LQREncoderVelocity
LQR encoder velocity std dev, RPM. Flywheel LQR only (tunable).
Definition SmartMotorControllerTelemetry.hpp:56
@ TunableSetpointPosition
Live-tunable position setpoint degrees or meters.
Definition SmartMotorControllerTelemetry.hpp:65
@ ADRCB0
ADRC input gain (tunable). ADRC only.
Definition SmartMotorControllerTelemetry.hpp:62
@ MechanismPosition
Mechanism position in rotations.
Definition SmartMotorControllerTelemetry.hpp:86
@ RotorVelocity
Raw rotor velocity in rotations per second.
Definition SmartMotorControllerTelemetry.hpp:92
@ ADRCBandwidthScale
ADRC bandwidth scale (tunable). ADRC only.
Definition SmartMotorControllerTelemetry.hpp:64
@ ADRCSystemOrder
ADRC plant order. ADRC only.
Definition SmartMotorControllerTelemetry.hpp:61
@ MeasurementPosition
Linear measurement position (m).
Definition SmartMotorControllerTelemetry.hpp:80
@ ADRCSettlingTime
ADRC settling time, s (tunable). ADRC only.
Definition SmartMotorControllerTelemetry.hpp:63
@ MeasurementAcceleration
Linear measurement acceleration (m/s²).
Definition SmartMotorControllerTelemetry.hpp:83
@ OpenloopRampRate
Open-loop duty-cycle ramp rate (s, tunable).
Definition SmartMotorControllerTelemetry.hpp:97
@ TrapezoidalProfileMaxVelocity
Trapezoidal profile max velocity RPM or m/s (tunable).
Definition SmartMotorControllerTelemetry.hpp:37
@ MeasurementCircumference
The mechanism's circumference (m per mechanism rotation), if it has one.
Definition SmartMotorControllerTelemetry.hpp:81
@ ExponentialProfileKA
Exponential profile kA constant (tunable).
Definition SmartMotorControllerTelemetry.hpp:35
@ TunableSetpointVelocity
Live-tunable velocity setpoint RPM or m/s.
Definition SmartMotorControllerTelemetry.hpp:67
@ MeasurementLowerLimit
Configured lower linear soft limit (m, tunable).
Definition SmartMotorControllerTelemetry.hpp:84
@ SetpointVelocity
Read-only velocity setpoint RPS or m/s.
Definition SmartMotorControllerTelemetry.hpp:68
@ ClosedloopRampRate
Closed-loop duty-cycle ramp rate (s, tunable).
Definition SmartMotorControllerTelemetry.hpp:96
@ kS
Static friction feedforward gain kS (tunable).
Definition SmartMotorControllerTelemetry.hpp:43
@ StatorCurrentLimit
Configured stator current limit (A, tunable).
Definition SmartMotorControllerTelemetry.hpp:76
@ ExponentialProfileMaxInput
Exponential profile maximum input voltage (tunable).
Definition SmartMotorControllerTelemetry.hpp:36
@ ADRCDisturbance
ADRC estimated total disturbance. ADRC only.
Definition SmartMotorControllerTelemetry.hpp:58
@ ActiveClosedLoopControllerSlot
Currently active closed-loop gain slot (read-only).
Definition SmartMotorControllerTelemetry.hpp:42
@ RotorGearRatio
Rotor rotations per mechanism rotation: 60 for a 60:1 arm.
Definition SmartMotorControllerTelemetry.hpp:93
@ LQREncoderPosition
LQR encoder position std dev, degrees or meters. Arm or elevator LQR only (tunable).
Definition SmartMotorControllerTelemetry.hpp:55
@ MotorTemperature
Motor controller temperature (°F).
Definition SmartMotorControllerTelemetry.hpp:79
@ LQRModelVelocity
LQR model velocity std dev, RPM or m/s (tunable).
Definition SmartMotorControllerTelemetry.hpp:54
@ MechanismLowerLimit
Configured lower angular soft limit (degrees, tunable).
Definition SmartMotorControllerTelemetry.hpp:89
@ LQRMaxVoltage
LQR maximum output voltage, V (tunable).
Definition SmartMotorControllerTelemetry.hpp:57
@ LQRModelPosition
LQR model position std dev, degrees or meters. Arm or elevator LQR only (tunable).
Definition SmartMotorControllerTelemetry.hpp:53
@ RotorPosition
Raw rotor position in rotations.
Definition SmartMotorControllerTelemetry.hpp:91
@ LQRQelmsVelocity
LQR velocity error tolerance, RPM or m/s (tunable).
Definition SmartMotorControllerTelemetry.hpp:51
@ LQRQelmsPosition
LQR position error tolerance, degrees or meters. Arm or elevator LQR only (tunable).
Definition SmartMotorControllerTelemetry.hpp:50
@ TrapezoidalProfileMaxJerk
Trapezoidal profile max jerk RPM/s² (tunable).
Definition SmartMotorControllerTelemetry.hpp:40
@ MechanismVelocity
Mechanism velocity in rotations per second.
Definition SmartMotorControllerTelemetry.hpp:87
BooleanTelemetryField
Definition SmartMotorControllerTelemetry.hpp:101
@ EncoderInversion
true when the encoder count direction is inverted (tunable).
Definition SmartMotorControllerTelemetry.hpp:111
@ TemperatureLimit
true when the motor temperature has reached the cutoff threshold.
Definition SmartMotorControllerTelemetry.hpp:104
@ SimpleMotorFeedForward
true when a simple motor feedforward model is configured.
Definition SmartMotorControllerTelemetry.hpp:108
@ MotorInversion
true when the motor output direction is inverted (tunable).
Definition SmartMotorControllerTelemetry.hpp:110
@ ElevatorFeedForward
true when an elevator feedforward model is configured.
Definition SmartMotorControllerTelemetry.hpp:106
@ MotionProfile
true when a motion profile (trapezoidal or exponential) is active.
Definition SmartMotorControllerTelemetry.hpp:109
@ TunableVoltageEnabled
Definition SmartMotorControllerTelemetry.hpp:112
@ VelocityControl
true when a velocity setpoint is active.
Definition SmartMotorControllerTelemetry.hpp:105
@ ArmFeedForward
true when an arm feedforward model is configured.
Definition SmartMotorControllerTelemetry.hpp:107
Definition SmartMotorControllerTelemetry.hpp:124
std::optional< std::vector< DoubleTelemetryField > > doubleFields
Unsupported double fields.
Definition SmartMotorControllerTelemetry.hpp:126
std::optional< std::vector< BooleanTelemetryField > > boolFields
Unsupported boolean fields.
Definition SmartMotorControllerTelemetry.hpp:125