From f14e0956f9282342da974f8ad7663e63de8ab75d Mon Sep 17 00:00:00 2001 From: VyaasBaskar Date: Wed, 1 Jan 2025 15:47:36 -0800 Subject: [PATCH] Added motor physics simulation to MotorMonkey --- README.md | 11 ++-- src/frc846/cpp/frc846/control/MotorMonkey.cc | 36 +++++++---- .../hardware/simulation/MCSimulator.cc | 62 ++++++++++++++++--- .../include/frc846/control/MotorMonkey.h | 3 +- .../control/hardware/simulation/MCSimulator.h | 45 ++++++++++++-- .../control/hardware/simulation/SIMLEVEL.h | 10 +++ 6 files changed, 139 insertions(+), 28 deletions(-) create mode 100644 src/frc846/include/frc846/control/hardware/simulation/SIMLEVEL.h diff --git a/README.md b/README.md index 8a0aad6..5726a77 100644 --- a/README.md +++ b/README.md @@ -174,10 +174,13 @@ To undo the going back: - `git switch -` <-- goes back to latest commit + + + ## CppCheck Warnings ``` -src/frc846/cpp/frc846/math/collection.cc:11:0: warning: The function 'HorizontalDeadband' is never used. [unusedFunction] -src/frc846/cpp/frc846/math/collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction] -src/frc846/cpp/frc846/math/collection.cc:39:0: warning: The function 'CoterminalDifference' is never used. [unusedFunction] -src/frc846/cpp/frc846/math/collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction] +src\frc846\cpp\frc846\math\collection.cc:11:0: warning: The function 'HorizontalDeadband' is never used. [unusedFunction] +src\frc846\cpp\frc846\math\collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction] +src\frc846\cpp\frc846\math\collection.cc:39:0: warning: The function 'CoterminalDifference' is never used. [unusedFunction] +src\frc846\cpp\frc846\math\collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction] ``` \ No newline at end of file diff --git a/src/frc846/cpp/frc846/control/MotorMonkey.cc b/src/frc846/cpp/frc846/control/MotorMonkey.cc index de2c3fc..ac24f73 100644 --- a/src/frc846/cpp/frc846/control/MotorMonkey.cc +++ b/src/frc846/cpp/frc846/control/MotorMonkey.cc @@ -5,13 +5,14 @@ #include #include -#include "frc846/control/hardware/TalonFX_interm.h" -#include "frc846/control/hardware/SparkMXFX_interm.h" +#include +#include "frc846/control/hardware/SparkMXFX_interm.h" +#include "frc846/control/hardware/TalonFX_interm.h" +#include "frc846/control/hardware/simulation/MCSimulator.h" +#include "frc846/control/hardware/simulation/SIMLEVEL.h" #include "frc846/math/collection.h" -#include - // TODO: Add dynamic can/power management namespace frc846::control { @@ -55,6 +56,7 @@ frc846::base::Loggable MotorMonkey::loggable_{"MotorMonkey"}; size_t MotorMonkey::slot_counter_{0}; std::map MotorMonkey::slot_id_to_type_{}; +std::map MotorMonkey::slot_id_to_sim_{}; frc846::control::hardware::IntermediateController* MotorMonkey::controller_registry[CONTROLLER_REGISTRY_SIZE]{}; @@ -68,9 +70,18 @@ units::volt_t MotorMonkey::battery_voltage{0_V}; void MotorMonkey::Tick() { battery_voltage = frc::RobotController::GetBatteryVoltage(); + // TODO: Improve battery voltage estimation for simulation for (size_t i = 0; i < CONTROLLER_REGISTRY_SIZE; i++) { - if (controller_registry[i] != nullptr) { controller_registry[i]->Tick(); } + if (controller_registry[i] != nullptr) { + controller_registry[i]->Tick(); + if (slot_id_to_sim_[i]) { + simulation::MCSimulator* sim = + dynamic_cast(controller_registry[i]); + sim->SetBatteryVoltage(battery_voltage); + sim->SetLoad(load_registry[i]); + } + } } } @@ -90,11 +101,17 @@ size_t MotorMonkey::ConstructController( size_t slot_id = slot_counter_; slot_id_to_type_[slot_id] = type; + slot_id_to_sim_[slot_id] = false; frc846::control::hardware::IntermediateController* this_controller = nullptr; - if (frc::RobotBase::IsSimulation()) { - // TODO: implement sim controllers + if (frc::RobotBase::IsSimulation() && + MOTOR_SIM_LEVEL == MOTOR_SIM_LEVEL_SIM_HARDWARE) { + slot_id_to_sim_[slot_id] = true; + this_controller = controller_registry[slot_id] = + new frc846::control::simulation::MCSimulator{ + frc846::control::base::MotorSpecificationPresets::get(type), + params.circuit_resistance, params.rotational_inertia}; } else if (frc846::control::base::MotorMonkeyTypeHelper::is_talon_fx(type)) { this_controller = controller_registry[slot_id] = new frc846::control::hardware::TalonFX_interm{ @@ -138,11 +155,6 @@ void MotorMonkey::SetLoad(size_t slot_id, units::newton_meter_t load) { CHECK_SLOT_ID(); load_registry[slot_id] = load; - - if (controller_registry[slot_id] != nullptr) { - // TODO: If is MCSim, cast then set load - // controller_registry[slot_id]->SetLoad(load); - } } void MotorMonkey::SetGains( diff --git a/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc b/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc index 310d485..ebf342c 100644 --- a/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc +++ b/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc @@ -15,23 +15,27 @@ MCSimulator::MCSimulator(frc846::control::base::MotorSpecs specs, std::chrono::system_clock::now().time_since_epoch()); } -void MCSimulator::Tick( - units::volt_t battery_voltage, units::newton_meter_t load) { +void MCSimulator::Tick() { double duty_cycle = 0.0; if (auto* dc = std::get_if(&control_message)) { duty_cycle = *dc; } else if (auto* vel = std::get_if(&control_message)) { duty_cycle = - gains.calculate(vel->to(), 0.0, 0.0, load.to()); + gains.calculate(vel->to(), 0.0, 0.0, load_.to()); } else if (auto* pos = std::get_if(&control_message)) { duty_cycle = gains.calculate( - pos->to(), 0.0, velocity_.to(), load.to()); + pos->to(), 0.0, velocity_.to(), load_.to()); } - + pred_current_ = frc846::control::calculators::CurrentTorqueCalculator:: + predict_current_draw( + duty_cycle, velocity_, battery_voltage_, circuit_resistance_, specs); units::newton_meter_t torque_output = - frc846::control::calculators::CurrentTorqueCalculator::predict_torque( - duty_cycle, velocity_, battery_voltage, circuit_resistance_, specs); + frc846::control::calculators::CurrentTorqueCalculator::current_to_torque( + pred_current_, specs); + torque_output -= load_; + + if (inverted) torque_output = -torque_output; std::chrono::microseconds current_time = std::chrono::duration_cast( @@ -51,6 +55,11 @@ void MCSimulator::Tick( velocity_ = new_velocity; } +void MCSimulator::SetInverted(bool inverted) { + this->inverted = inverted; + position_ = -position_; +} + void MCSimulator::WriteDC(double duty_cycle) { control_message = duty_cycle; } void MCSimulator::WriteVelocity(units::radians_per_second_t velocity) { control_message = velocity; @@ -59,6 +68,7 @@ void MCSimulator::WritePosition(units::radian_t position) { control_message = position; } +units::ampere_t MCSimulator::GetCurrent() { return pred_current_; } units::radian_t MCSimulator::GetPosition() { return position_; } units::radians_per_second_t MCSimulator::GetVelocity() { return velocity_; } @@ -69,8 +79,46 @@ void MCSimulator::ZeroEncoder(units::radian_t position) { void MCSimulator::DisablePositionPacket() { position_packet_enabled = false; } void MCSimulator::DisableVelocityPacket() { velocity_packet_enabled = false; } +void MCSimulator::EnableStatusFrames( + std::vector frames) { + bool disable_pos = true; + bool disable_vel = true; + for (auto frame : frames) { + switch (frame) { + case frc846::control::config::StatusFrame::kPositionFrame: + disable_pos = false; + break; + case frc846::control::config::StatusFrame::kVelocityFrame: + disable_vel = false; + break; + default: break; + } + } + if (disable_pos) DisablePositionPacket(); + if (disable_vel) DisableVelocityPacket(); +} + +bool MCSimulator::IsDuplicateControlMessage(double duty_cycle) { + return std::holds_alternative(control_message) && + std::get(control_message) == duty_cycle; +} +bool MCSimulator::IsDuplicateControlMessage( + units::radians_per_second_t velocity) { + return std::holds_alternative(control_message) && + std::get(control_message) == velocity; +} +bool MCSimulator::IsDuplicateControlMessage(units::radian_t position) { + return std::holds_alternative(control_message) && + std::get(control_message) == position; +} + void MCSimulator::SetGains(frc846::control::base::MotorGains gains) { this->gains = gains; } +void MCSimulator::SetLoad(units::newton_meter_t load) { this->load_ = load; } +void MCSimulator::SetBatteryVoltage(units::volt_t voltage) { + this->battery_voltage_ = voltage; +} + } // namespace frc846::control::simulation \ No newline at end of file diff --git a/src/frc846/include/frc846/control/MotorMonkey.h b/src/frc846/include/frc846/control/MotorMonkey.h index 5970ac4..14ef3eb 100644 --- a/src/frc846/include/frc846/control/MotorMonkey.h +++ b/src/frc846/include/frc846/control/MotorMonkey.h @@ -5,12 +5,12 @@ #include +#include "frc846/base/Loggable.h" #include "frc846/control/base/motor_control_base.h" #include "frc846/control/base/motor_gains.h" #include "frc846/control/base/motor_specs.h" #include "frc846/control/config/construction_params.h" #include "frc846/control/hardware/TalonFX_interm.h" -#include "frc846/base/Loggable.h" #define CONTROLLER_REGISTRY_SIZE 64 @@ -98,6 +98,7 @@ class MotorMonkey { static size_t slot_counter_; static std::map slot_id_to_type_; + static std::map slot_id_to_sim_; static frc846::control::hardware::IntermediateController* controller_registry[CONTROLLER_REGISTRY_SIZE]; diff --git a/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h b/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h index ab42e49..108b7cb 100644 --- a/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h +++ b/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h @@ -8,9 +8,9 @@ #include -#include "frc846/control/hardware/IntermediateController.h" #include "frc846/control/base/motor_gains.h" #include "frc846/control/base/motor_specs.h" +#include "frc846/control/hardware/IntermediateController.h" #include "frc846/wpilib/846_units.h" namespace frc846::control::simulation { @@ -21,7 +21,7 @@ MCSimulator A class that simulates a motor controller. Uses motor dynamics to simulate position and velocity accurately. */ -class MCSimulator : frc846::control::hardware::IntermediateController { +class MCSimulator : public frc846::control::hardware::IntermediateController { public: MCSimulator(frc846::control::base::MotorSpecs specs, frc846::wpilib::unit_ohm circuit_resistance, @@ -32,7 +32,27 @@ class MCSimulator : frc846::control::hardware::IntermediateController { Uses the control message to update the motor state. */ - void Tick(units::volt_t battery_voltage, units::newton_meter_t load); + void Tick() override; + + bool VerifyConnected() override { return true; } + + frc846::control::hardware::ControllerErrorCodes GetLastErrorCode() override { + return frc846::control::hardware::ControllerErrorCodes::kAllOK; + } + + void SetInverted(bool inverted) override; + void SetNeutralMode(bool brake_mode) override { (void)brake_mode; }; + void SetCurrentLimit(units::ampere_t current_limit) { (void)current_limit; }; + + void SetSoftLimits( + units::radian_t forward_limit, units::radian_t reverse_limit) override { + (void)forward_limit; + (void)reverse_limit; + }; + + void SetVoltageCompensation(units::volt_t voltage_compensation) { + (void)voltage_compensation; + }; void WriteDC(double duty_cycle) override; void WriteVelocity(units::radians_per_second_t velocity) override; @@ -40,13 +60,14 @@ class MCSimulator : frc846::control::hardware::IntermediateController { units::radian_t GetPosition() override; units::radians_per_second_t GetVelocity() override; + units::ampere_t GetCurrent() override; /* ZeroEncoder() Sets the motor's position to specified value. */ - void ZeroEncoder(units::radian_t position = 0_rad) override; + void ZeroEncoder(units::radian_t position) override; /* DisablePositionPacket() @@ -61,8 +82,18 @@ class MCSimulator : frc846::control::hardware::IntermediateController { */ void DisableVelocityPacket(); + void EnableStatusFrames( + std::vector frames) override; + + bool IsDuplicateControlMessage(double duty_cycle) override; + bool IsDuplicateControlMessage(units::radians_per_second_t velocity) override; + bool IsDuplicateControlMessage(units::radian_t position) override; + void SetGains(frc846::control::base::MotorGains gains) override; + void SetLoad(units::newton_meter_t load); + void SetBatteryVoltage(units::volt_t voltage); + private: frc846::control::base::MotorSpecs specs; frc846::control::base::MotorGains gains; @@ -75,11 +106,17 @@ class MCSimulator : frc846::control::hardware::IntermediateController { std::chrono::microseconds last_tick_; + units::ampere_t pred_current_{0}; units::radian_t position_ = 0_rad; units::radians_per_second_t velocity_ = 0_rad_per_s; std::variant control_message = 0.0; + + units::newton_meter_t load_{0}; + units::volt_t battery_voltage_{0}; + + bool inverted = false; }; } // namespace frc846::control::simulation \ No newline at end of file diff --git a/src/frc846/include/frc846/control/hardware/simulation/SIMLEVEL.h b/src/frc846/include/frc846/control/hardware/simulation/SIMLEVEL.h new file mode 100644 index 0000000..c42ef26 --- /dev/null +++ b/src/frc846/include/frc846/control/hardware/simulation/SIMLEVEL.h @@ -0,0 +1,10 @@ +#pragma once + +namespace frc846::control::simulation { + +#define MOTOR_SIM_LEVEL_SIM_HARDWARE 1 +#define MOTOR_SIM_LEVEL_SIM_PHYSICS 2 + +#define MOTOR_SIM_LEVEL MOTOR_SIM_LEVEL_SIM_HARDWARE + +} // namespace frc846::control::simulation \ No newline at end of file