Merge branch 'drivetrain_impl' of https://github.com/Team846/monkey_m…

…enace into drivetrain_impl

README.md

@@ -174,12 +174,14 @@ To undo the going back:
 
 - `git switch -` <-- goes back to latest commit
 
+
+
+
 ## CppCheck Warnings
 
 ```
 src\frc846\cpp\frc846\robot\swerve\drivetrain.cc:94:44: warning: Variable 'ol_target' is assigned a value that is never used. [unreadVariable]
 src\frc846\cpp\frc846\robot\swerve\drivetrain.cc:108:64: warning: Variable 'accel_target' is assigned a value that is never used. [unreadVariable]
 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:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
 ```

src/frc846/cpp/frc846/control/MotorMonkey.cc

@@ -9,6 +9,8 @@
 
 #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"
 
 // TODO: Add dynamic can/power management
@@ -54,6 +56,7 @@ frc846::base::Loggable MotorMonkey::loggable_{"MotorMonkey"};
 size_t MotorMonkey::slot_counter_{0};
 std::map<size_t, frc846::control::base::MotorMonkeyType>
     MotorMonkey::slot_id_to_type_{};
+std::map<size_t, bool> MotorMonkey::slot_id_to_sim_{};
 
 frc846::control::hardware::IntermediateController*
     MotorMonkey::controller_registry[CONTROLLER_REGISTRY_SIZE]{};
@@ -67,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<simulation::MCSimulator*>(controller_registry[i]);
+        sim->SetBatteryVoltage(battery_voltage);
+        sim->SetLoad(load_registry[i]);
+      }
+    }
   }
 }
 
@@ -89,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{
@@ -146,11 +164,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(

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<double>(&control_message)) {
     duty_cycle = *dc;
   } else if (auto* vel =
                  std::get_if<units::radians_per_second_t>(&control_message)) {
     duty_cycle =
-        gains.calculate(vel->to<double>(), 0.0, 0.0, load.to<double>());
+        gains.calculate(vel->to<double>(), 0.0, 0.0, load_.to<double>());
   } else if (auto* pos = std::get_if<units::radian_t>(&control_message)) {
     duty_cycle = gains.calculate(
-        pos->to<double>(), 0.0, velocity_.to<double>(), load.to<double>());
+        pos->to<double>(), 0.0, velocity_.to<double>(), load_.to<double>());
   }
-
+  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<std::chrono::microseconds>(
@@ -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<frc846::control::config::StatusFrame> 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<double>(control_message) &&
+         std::get<double>(control_message) == duty_cycle;
+}
+bool MCSimulator::IsDuplicateControlMessage(
+    units::radians_per_second_t velocity) {
+  return std::holds_alternative<units::radians_per_second_t>(control_message) &&
+         std::get<units::radians_per_second_t>(control_message) == velocity;
+}
+bool MCSimulator::IsDuplicateControlMessage(units::radian_t position) {
+  return std::holds_alternative<units::radian_t>(control_message) &&
+         std::get<units::radian_t>(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

src/frc846/include/frc846/control/MotorMonkey.h

@@ -108,6 +108,7 @@ class MotorMonkey {
   static size_t slot_counter_;
   static std::map<size_t, frc846::control::base::MotorMonkeyType>
       slot_id_to_type_;
+  static std::map<size_t, bool> slot_id_to_sim_;
 
   static frc846::control::hardware::IntermediateController*
       controller_registry[CONTROLLER_REGISTRY_SIZE];

src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h

@@ -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,21 +32,42 @@ 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;
   void WritePosition(units::radian_t position) override;
 
   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<frc846::control::config::StatusFrame> 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<double, units::radians_per_second_t, units::radian_t>
       control_message = 0.0;
+
+  units::newton_meter_t load_{0};
+  units::volt_t battery_voltage_{0};
+
+  bool inverted = false;
 };
 
 }  // namespace frc846::control::simulation

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