YAMS
Yet Another Mechanism System
Loading...
Searching...
No Matches
SmartMotorController.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 <functional>
7#include <memory>
8#include <optional>
9#include <string>
10#include <vector>
11#include <wpi/math/controller/ArmFeedforward.hpp>
12#include <wpi/math/controller/ElevatorFeedforward.hpp>
13#include <wpi/math/controller/PIDController.hpp>
14#include <wpi/math/controller/SimpleMotorFeedforward.hpp>
15#include <wpi/math/system/DCMotor.hpp>
16#include <wpi/math/trajectory/ExponentialProfile.hpp>
17#include <wpi/math/trajectory/TrapezoidProfile.hpp>
18#include <wpi/nt/NetworkTable.hpp>
19#include <wpi/system/Notifier.hpp>
20#include <wpi/units/acceleration.hpp>
21#include <wpi/units/angle.hpp>
22#include <wpi/units/angular_acceleration.hpp>
23#include <wpi/units/angular_jerk.hpp>
24#include <wpi/units/angular_velocity.hpp>
25#include <wpi/units/current.hpp>
26#include <wpi/units/force.hpp>
27#include <wpi/units/length.hpp>
28#include <wpi/units/temperature.hpp>
29#include <wpi/units/time.hpp>
30#include <wpi/units/velocity.hpp>
31#include <wpi/units/voltage.hpp>
32
33#include "SimSupplier.hpp"
39
40namespace yams::motorcontrollers {
41
50 public:
54
57
58 protected:
61
62 public:
63 // ---- Configuration ------------------------------------------------------
64
71 virtual bool ApplyConfig(const SmartMotorControllerConfig& config) = 0;
72
73 // ---- Simulation ---------------------------------------------------------
74
76 virtual void SetupSimulation() = 0;
77
84 virtual void SimIterate();
85
93 virtual void SetSimSupplier(std::shared_ptr<SimSupplier> supplier);
94
101
102 // ---- Encoder sync -------------------------------------------------------
103
105 virtual void SeedRelativeEncoder() = 0;
106
108 virtual void SynchronizeRelativeEncoder() = 0;
109
110 // ---- Open-loop outputs --------------------------------------------------
111
117 virtual void SetDutyCycle(double dutyCycle) = 0;
118
124 virtual double GetDutyCycle() = 0;
125
131 virtual void SetVoltage(wpi::units::volt_t voltage) = 0;
132
138 virtual wpi::units::volt_t GetVoltage() = 0;
139
140 // ---- Closed-loop setpoints ----------------------------------------------
141
147 virtual void SetPosition(wpi::units::turn_t angle) = 0;
148
154 virtual void SetPosition(wpi::units::meter_t distance) = 0;
155
161 virtual void SetVelocity(wpi::units::turns_per_second_t velocity) = 0;
162
168 virtual void SetVelocity(wpi::units::meters_per_second_t velocity) = 0;
169
180 virtual void SetVelocity(wpi::units::turns_per_second_t velocity,
181 wpi::units::newton_t feedforwardForce);
182
192 void SetVelocity(wpi::units::meters_per_second_t velocity, wpi::units::newton_t feedforwardForce);
193
194 // ---- Encoder writes -----------------------------------------------------
195
201 virtual void SetEncoderPosition(wpi::units::turn_t angle) = 0;
202
208 virtual void SetEncoderPosition(wpi::units::meter_t distance) = 0;
209
215 virtual void SetEncoderVelocity(wpi::units::turns_per_second_t velocity) = 0;
216
222 virtual void SetEncoderVelocity(wpi::units::meters_per_second_t velocity) = 0;
223
224 // ---- Encoder reads ------------------------------------------------------
225
233 virtual wpi::units::turn_t GetMechanismPosition() = 0;
234
242 virtual wpi::units::turns_per_second_t GetMechanismVelocity() = 0;
243
250 virtual wpi::units::turn_t GetRelativeMechanismPosition() = 0;
251
258 virtual wpi::units::turns_per_second_t GetRelativeMechanismVelocity() = 0;
259
265 virtual wpi::units::turns_per_second_squared_t GetMechanismAcceleration() = 0;
266
272 virtual wpi::units::turn_t GetRotorPosition() = 0;
273
279 virtual wpi::units::turns_per_second_t GetRotorVelocity() = 0;
280
286 virtual wpi::units::meter_t GetMeasurementPosition() = 0;
287
293 virtual wpi::units::meters_per_second_t GetMeasurementVelocity() = 0;
294
300 virtual wpi::units::meters_per_second_squared_t GetMeasurementAcceleration() = 0;
301
308 virtual std::optional<wpi::units::turn_t> GetExternalEncoderMechanismPosition() = 0;
309
317 virtual std::optional<wpi::units::turns_per_second_t> GetExternalEncoderMechanismVelocity() = 0;
318
319 // ---- Motor status -------------------------------------------------------
320
326 virtual std::optional<wpi::units::ampere_t> GetSupplyCurrent() = 0;
327
333 virtual wpi::units::ampere_t GetStatorCurrent() = 0;
334
340 virtual wpi::units::celsius_t GetTemperature() = 0;
341
347 virtual wpi::math::DCMotor GetDCMotor() = 0;
348
357 wpi::math::DCMotor GetMechanismDCMotor();
358
367 void SetMechanismSimSupplier(std::shared_ptr<SimSupplier> supplier);
368
378 double GetMechanismDutyCycle(double dutyCycle);
379
380 // ---- Configuration setters (live tuning) --------------------------------
381
387 virtual void SetZeroPower(MotorMode mode) = 0;
388
394 virtual void SetMotorInverted(bool inverted) = 0;
395
401 virtual void SetEncoderInverted(bool inverted) = 0;
402
408 virtual void SetKp(double kP) = 0;
409
415 virtual void SetKi(double kI) = 0;
416
422 virtual void SetKd(double kD) = 0;
423
431 virtual void SetFeedback(double kP, double kI, double kD) = 0;
432
438 virtual void SetKs(double kS) = 0;
439
445 virtual void SetKv(double kV) = 0;
446
452 virtual void SetKa(double kA) = 0;
453
459 virtual void SetKg(double kG) = 0;
460
469 virtual void SetFeedforward(double kS, double kV, double kA, double kG) = 0;
470
476 virtual void SetStatorCurrentLimit(wpi::units::ampere_t currentLimit) = 0;
477
483 virtual void SetSupplyCurrentLimit(wpi::units::ampere_t currentLimit) = 0;
484
490 virtual void SetClosedLoopRampRate(wpi::units::second_t rampRate) = 0;
491
497 virtual void SetOpenLoopRampRate(wpi::units::second_t rampRate) = 0;
498
504 virtual void SetMechanismUpperLimit(wpi::units::turn_t upperLimit) = 0;
505
511 virtual void SetMechanismLowerLimit(wpi::units::turn_t lowerLimit) = 0;
512
519 virtual void SetMechanismLimits(wpi::units::turn_t lower, wpi::units::turn_t upper) = 0;
520
526 virtual void SetMechanismLimitsEnabled(bool enabled) = 0;
527
533 virtual void SetMeasurementUpperLimit(wpi::units::meter_t upperLimit) = 0;
534
540 virtual void SetMeasurementLowerLimit(wpi::units::meter_t lowerLimit) = 0;
541
547 virtual void SetMotionProfileMaxVelocity(wpi::units::turns_per_second_t maxVelocity) = 0;
548
554 virtual void SetMotionProfileMaxVelocity(wpi::units::meters_per_second_t maxVelocity) = 0;
555
561 virtual void SetMotionProfileMaxAcceleration(wpi::units::turns_per_second_squared_t maxAcc) = 0;
562
568 virtual void SetMotionProfileMaxAcceleration(wpi::units::meters_per_second_squared_t maxAcc) = 0;
569
576 wpi::units::angular_jerk::turns_per_second_cubed_t maxJerk) = 0;
577
585 virtual void SetExponentialProfile(std::optional<double> kV, std::optional<double> kA,
586 std::optional<wpi::units::volt_t> maxInput) = 0;
587
594
604
613 virtual void SetMechanismCircumference(wpi::units::meter_t circumference);
614
615 // ---- Closed-loop controller thread --------------------------------------
616
625 wpi::units::second_t GetClosedLoopControllerPeriod() const;
626
635 virtual std::optional<wpi::units::second_t> GetVelocityMeasurementDelay() const;
636
643
649
658
665
673
676
677 // ---- Telemetry ----------------------------------------------------------
678
685 void SetupTelemetry(std::shared_ptr<wpi::nt::NetworkTable> dataTable,
686 std::shared_ptr<wpi::nt::NetworkTable> tuningTable);
687
690
697
706
712 bool TuningEnabled() const;
713
720
726
733 const std::vector<SmartMotorController*>& GetLooselyCoupledFollowers() const {
734 return m_looseFollowers;
735 }
736
743 void AddCloseHook(std::function<void()> hook);
744
755
765
772
773 // ---- Misc ---------------------------------------------------------------
774
780 std::optional<wpi::units::turn_t> GetMechanismPositionSetpoint() const;
781
787 std::optional<wpi::units::turns_per_second_t> GetMechanismSetpointVelocity() const;
788
796 std::optional<wpi::units::meter_t> GetMeasurementPositionSetpoint() const;
797
805 std::optional<wpi::units::meters_per_second_t> GetMeasurementSetpointVelocity() const;
806
812 std::optional<wpi::units::newton_t> GetSetpointFeedforwardForce() const;
813
820
826 virtual void* GetMotorController() = 0;
827
833 virtual void* GetMotorControllerConfig() = 0;
834
840 std::string GetName() const;
841
849 bool IsMotor(const wpi::math::DCMotor& a, const wpi::math::DCMotor& b) const;
850
853
859 void Close();
860
861 protected:
867 ClosedLoopControllerSlot m_slot{ClosedLoopControllerSlot::SLOT_0};
868 std::shared_ptr<SimSupplier> m_simSupplier;
869
871 std::vector<SmartMotorController*> m_looseFollowers;
872
879
885
891
901 void Follow(SmartMotorController& leader, bool inverted);
902
904 double CalculateSimDutyCycle(double dutyCycle);
905
906
907
916
921 void ForwardPositionToFollowers(wpi::units::turn_t pos);
922
927 void ForwardPositionToFollowers(wpi::units::meter_t dist);
928
933 void ForwardVelocityToFollowers(wpi::units::turns_per_second_t vel);
934
939 void ForwardVelocityToFollowers(wpi::units::meters_per_second_t vel);
940
941 std::optional<wpi::math::PIDController> m_pid;
942 std::optional<math::LQRController> m_lqr;
944 std::optional<math::ADRCController> m_adrc;
945
954
964
965 // Angular motion profile state
966 std::optional<wpi::math::TrapezoidProfile<wpi::units::turns>::State> m_trapState;
967 std::optional<wpi::math::ExponentialProfile<wpi::units::turns, wpi::units::volts>::State>
969
970 // Linear motion profile state
971 std::optional<wpi::math::TrapezoidProfile<wpi::units::meters>::State> m_linearTrapState;
972 std::optional<wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State>
974
979 std::optional<wpi::units::turn_t> m_lastClosedLoopMechanismPosition;
984 static constexpr double kClosedLoopRampFullScaleVolts = 12.0;
987 std::optional<double> m_lastClosedLoopVoltage;
988
989 std::optional<wpi::units::turn_t> m_setpointPosition;
990 std::optional<wpi::units::turns_per_second_t> m_setpointVelocity;
992 std::optional<wpi::units::newton_t> m_setpointFeedforwardForce;
993
995 std::vector<std::function<void()>> m_closeHooks;
996
1003 const void* BatterySimKey() const { return this; }
1004
1005 std::unique_ptr<wpi::Notifier> m_closedLoopControllerThread;
1007
1008 std::shared_ptr<wpi::nt::NetworkTable> m_parentTable;
1009 std::shared_ptr<wpi::nt::NetworkTable> m_telemetryTable;
1010 std::shared_ptr<wpi::nt::NetworkTable> m_tuningTable;
1011
1016 std::shared_ptr<telemetry::SmartMotorControllerTelemetryConfig> m_specifiedTelemetryConfig;
1018 bool m_closed{false};
1019
1020 private:
1022 void ResetProfileStates();
1023
1025 void ResetClosedLoopControllerState();
1026};
1027
1028} // namespace yams::motorcontrollers
Definition MechanismGearing.hpp:19
Definition ADRCController.hpp:61
Definition LQRController.hpp:36
Definition SimSupplier.hpp:22
Definition SmartMotorControllerConfig.hpp:66
ControlMode
Definition SmartMotorControllerConfig.hpp:69
ClosedLoopControllerSlot
Definition SmartMotorControllerConfig.hpp:75
MotorMode
Definition SmartMotorControllerConfig.hpp:71
ClosedLoopControllerSlot m_slot
Definition SmartMotorController.hpp:867
std::optional< wpi::units::turn_t > m_lastClosedLoopMechanismPosition
Definition SmartMotorController.hpp:979
virtual wpi::units::celsius_t GetTemperature()=0
virtual void SetMeasurementUpperLimit(wpi::units::meter_t upperLimit)=0
double GetMechanismDutyCycle(double dutyCycle)
virtual wpi::units::meters_per_second_t GetMeasurementVelocity()=0
virtual void SetMeasurementLowerLimit(wpi::units::meter_t lowerLimit)=0
std::optional< wpi::math::PIDController > m_pid
Definition SmartMotorController.hpp:941
std::optional< wpi::units::turns_per_second_t > m_setpointVelocity
Definition SmartMotorController.hpp:990
double CalculateSimDutyCycle(double dutyCycle)
virtual void SetClosedLoopSlot(ClosedLoopControllerSlot slot)=0
std::shared_ptr< wpi::nt::NetworkTable > m_tuningTable
Definition SmartMotorController.hpp:1010
virtual void SetMechanismLimits(wpi::units::turn_t lower, wpi::units::turn_t upper)=0
void SetupTelemetry(std::shared_ptr< wpi::nt::NetworkTable > dataTable, std::shared_ptr< wpi::nt::NetworkTable > tuningTable)
SmartMotorControllerConfig::MotorMode MotorMode
Definition SmartMotorController.hpp:52
std::optional< wpi::math::ExponentialProfile< wpi::units::turns, wpi::units::volts >::State > m_expoState
Definition SmartMotorController.hpp:968
virtual void SetSimSupplier(std::shared_ptr< SimSupplier > supplier)
SmartMotorControllerConfig::ControlMode ControlMode
Definition SmartMotorController.hpp:53
virtual wpi::units::turn_t GetRotorPosition()=0
virtual void SetOpenLoopRampRate(wpi::units::second_t rampRate)=0
SmartMotorControllerConfig * m_config
Definition SmartMotorController.hpp:866
virtual void SetVoltage(wpi::units::volt_t voltage)=0
std::shared_ptr< wpi::nt::NetworkTable > m_telemetryTable
Definition SmartMotorController.hpp:1009
virtual wpi::units::turns_per_second_t GetRotorVelocity()=0
void ConfigureSoftwarePID(const SmartMotorControllerConfig &config)
bool IsMotor(const wpi::math::DCMotor &a, const wpi::math::DCMotor &b) const
virtual SmartMotorControllerConfig & GetConfig()=0
bool m_simulatesMechanism
Definition SmartMotorController.hpp:884
std::shared_ptr< wpi::nt::NetworkTable > m_parentTable
Definition SmartMotorController.hpp:1008
SmartMotorController & WithTelemetry(telemetry::SmartMotorControllerTelemetryConfig config)
virtual void SetMechanismGearing(const gearing::MechanismGearing &gearing)
virtual void SetSupplyCurrentLimit(wpi::units::ampere_t currentLimit)=0
bool m_telemetryConfigExplicit
Definition SmartMotorController.hpp:1014
void ForwardVelocityToFollowers(wpi::units::turns_per_second_t vel)
virtual wpi::units::turns_per_second_squared_t GetMechanismAcceleration()=0
virtual wpi::units::ampere_t GetStatorCurrent()=0
void AddCloseHook(std::function< void()> hook)
virtual void SetMotionProfileMaxAcceleration(wpi::units::turns_per_second_squared_t maxAcc)=0
virtual telemetry::UnsupportedTelemetryFields GetUnsupportedTelemetryFields()
SmartMotorControllerConfig::ClosedLoopControllerSlot ClosedLoopControllerSlot
Definition SmartMotorController.hpp:51
std::optional< wpi::math::TrapezoidProfile< wpi::units::turns >::State > m_trapState
Definition SmartMotorController.hpp:966
virtual void SetClosedLoopRampRate(wpi::units::second_t rampRate)=0
std::optional< wpi::units::newton_t > m_setpointFeedforwardForce
Definition SmartMotorController.hpp:992
virtual void SetEncoderVelocity(wpi::units::meters_per_second_t velocity)=0
bool m_closed
Definition SmartMotorController.hpp:1018
bool m_liveTuningRegistered
Definition SmartMotorController.hpp:1017
void SetMechanismSimSupplier(std::shared_ptr< SimSupplier > supplier)
virtual wpi::units::meters_per_second_squared_t GetMeasurementAcceleration()=0
virtual wpi::units::volt_t GetVoltage()=0
telemetry::SmartMotorControllerTelemetry m_telemetry
Definition SmartMotorController.hpp:1012
virtual void SetMotionProfileMaxVelocity(wpi::units::meters_per_second_t maxVelocity)=0
void SetVelocity(wpi::units::meters_per_second_t velocity, wpi::units::newton_t feedforwardForce)
virtual void SetEncoderPosition(wpi::units::meter_t distance)=0
virtual void SetMechanismLowerLimit(wpi::units::turn_t lowerLimit)=0
virtual std::optional< wpi::units::turn_t > GetExternalEncoderMechanismPosition()=0
std::optional< wpi::math::TrapezoidProfile< wpi::units::meters >::State > m_linearTrapState
Definition SmartMotorController.hpp:971
virtual std::optional< wpi::units::second_t > GetVelocityMeasurementDelay() const
virtual void SetMechanismLimitsEnabled(bool enabled)=0
virtual std::optional< wpi::units::ampere_t > GetSupplyCurrent()=0
virtual void SetZeroPower(MotorMode mode)=0
std::optional< double > m_lastClosedLoopVoltage
Definition SmartMotorController.hpp:987
virtual wpi::units::turns_per_second_t GetRelativeMechanismVelocity()=0
virtual void SetMotionProfileMaxJerk(wpi::units::angular_jerk::turns_per_second_cubed_t maxJerk)=0
virtual void SetEncoderVelocity(wpi::units::turns_per_second_t velocity)=0
void ForwardVelocityToFollowers(wpi::units::meters_per_second_t vel)
virtual void SetMotionProfileMaxAcceleration(wpi::units::meters_per_second_squared_t maxAcc)=0
virtual void SetVelocity(wpi::units::turns_per_second_t velocity, wpi::units::newton_t feedforwardForce)
virtual void SetMechanismUpperLimit(wpi::units::turn_t upperLimit)=0
ClosedLoopControllerSlot GetClosedLoopControllerSlot() const
bool m_closedLoopControllerRunning
Definition SmartMotorController.hpp:1006
std::optional< wpi::units::newton_t > GetSetpointFeedforwardForce() const
const std::vector< SmartMotorController * > & GetLooselyCoupledFollowers() const
Definition SmartMotorController.hpp:733
virtual void SetVelocity(wpi::units::turns_per_second_t velocity)=0
virtual wpi::units::meter_t GetMeasurementPosition()=0
math::ADRCController * GetADRCController()
std::optional< wpi::units::meters_per_second_t > GetMeasurementSetpointVelocity() const
virtual wpi::units::turns_per_second_t GetMechanismVelocity()=0
virtual void SetMotionProfileMaxVelocity(wpi::units::turns_per_second_t maxVelocity)=0
virtual void SetPosition(wpi::units::turn_t angle)=0
std::optional< wpi::units::turn_t > GetMechanismPositionSetpoint() const
virtual void SetMechanismCircumference(wpi::units::meter_t circumference)
std::vector< std::function< void()> > m_closeHooks
Definition SmartMotorController.hpp:995
virtual void SetExponentialProfile(std::optional< double > kV, std::optional< double > kA, std::optional< wpi::units::volt_t > maxInput)=0
void ForwardPositionToFollowers(wpi::units::meter_t dist)
std::optional< wpi::units::turn_t > m_setpointPosition
Definition SmartMotorController.hpp:989
std::vector< SmartMotorController * > m_looseFollowers
Definition SmartMotorController.hpp:871
virtual void SetEncoderPosition(wpi::units::turn_t angle)=0
virtual void SetDutyCycle(double dutyCycle)=0
virtual void SetStatorCurrentLimit(wpi::units::ampere_t currentLimit)=0
virtual wpi::units::turn_t GetMechanismPosition()=0
std::optional< math::ADRCController > m_adrc
Definition SmartMotorController.hpp:944
virtual wpi::units::turn_t GetRelativeMechanismPosition()=0
std::optional< wpi::math::ExponentialProfile< wpi::units::meters, wpi::units::volts >::State > m_linearExpoState
Definition SmartMotorController.hpp:973
virtual bool ApplyConfig(const SmartMotorControllerConfig &config)=0
void ForwardPositionToFollowers(wpi::units::turn_t pos)
virtual wpi::math::DCMotor GetDCMotor()=0
std::optional< wpi::units::turns_per_second_t > GetMechanismSetpointVelocity() const
virtual void SetMotorInverted(bool inverted)=0
wpi::units::second_t GetClosedLoopControllerPeriod() const
virtual void SetEncoderInverted(bool inverted)=0
const void * BatterySimKey() const
Definition SmartMotorController.hpp:1003
std::shared_ptr< telemetry::SmartMotorControllerTelemetryConfig > m_specifiedTelemetryConfig
Definition SmartMotorController.hpp:1016
std::optional< math::LQRController > m_lqr
Definition SmartMotorController.hpp:942
virtual void SetVelocity(wpi::units::meters_per_second_t velocity)=0
virtual std::optional< wpi::units::turns_per_second_t > GetExternalEncoderMechanismVelocity()=0
void Follow(SmartMotorController &leader, bool inverted)
std::shared_ptr< SimSupplier > m_simSupplier
Definition SmartMotorController.hpp:868
std::unique_ptr< wpi::Notifier > m_closedLoopControllerThread
Definition SmartMotorController.hpp:1005
static constexpr double kClosedLoopRampFullScaleVolts
Definition SmartMotorController.hpp:984
void ApplyADRCConfig(const SmartMotorControllerConfig &config)
virtual void SetPosition(wpi::units::meter_t distance)=0
bool m_iteratedByLeader
Definition SmartMotorController.hpp:878
std::optional< wpi::units::meter_t > GetMeasurementPositionSetpoint() const
virtual void SetFeedforward(double kS, double kV, double kA, double kG)=0
virtual void SetFeedback(double kP, double kI, double kD)=0
telemetry::SmartMotorControllerTelemetryConfig m_telemetryConfig
Definition SmartMotorController.hpp:1013
Definition SmartMotorControllerTelemetryConfig.hpp:63
Definition SmartMotorControllerTelemetry.hpp:560
Definition GearBox.hpp:11
Definition ADRCConfig.hpp:20
Definition SmartMotorControllerTelemetry.hpp:124