Merge branch 'master' into drivetrain_impl

build.gradle

@@ -206,4 +206,4 @@ runCppcheck.onlyIf { !project.hasProperty('fromCI') }
 if (!project.hasProperty('fromCI')) {
     check.dependsOn runCppcheck
     runCppcheck.dependsOn spotlessApply
-}
+}

src/frc846/cpp/frc846/control/HigherMotorController.cc

@@ -19,6 +19,10 @@ void HigherMotorController::Setup() {
       mmtype_, constr_params_);
 }
 
+bool HigherMotorController::VerifyConnected() {
+  return frc846::control::MotorMonkey::VerifyConnected();
+}
+
 void HigherMotorController::SetGains(frc846::control::base::MotorGains gains) {
   gains_ = gains;
   frc846::control::MotorMonkey::SetGains(slot_id_, gains_);

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

@@ -73,6 +73,15 @@ void MotorMonkey::Tick() {
   }
 }
 
+bool MotorMonkey::VerifyConnected() {
+  for (size_t i = 0; i < CONTROLLER_REGISTRY_SIZE; i++) {
+    if (controller_registry[i] != nullptr) {
+      if (!controller_registry[i]->VerifyConnected()) { return false; }
+    }
+  }
+  return true;
+}
+
 size_t MotorMonkey::ConstructController(
     frc846::control::base::MotorMonkeyType type,
     frc846::control::config::MotorConstructionParameters params) {

src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc

@@ -9,6 +9,11 @@ namespace frc846::control::hardware {
   set_last_error(code);                \
   if (last_error_ != ControllerErrorCodes::kAllOK) return
 
+bool SparkMXFX_interm::VerifyConnected() {
+  esc_->GetFirmwareVersion();
+  return esc_->GetFirmwareVersion() != 0;
+}
+
 SparkMXFX_interm::SparkMXFX_interm(int can_id,
     units::millisecond_t max_wait_time, bool is_controller_spark_flex) {
   esc_ = is_controller_spark_flex

src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc

@@ -2,6 +2,8 @@
 
 namespace frc846::control::hardware {
 
+bool TalonFX_interm::VerifyConnected() { return talon_.IsAlive(); }
+
 TalonFX_interm::TalonFX_interm(
     int can_id, std::string bus, units::millisecond_t max_wait_time)
     : talon_(can_id, bus), max_wait_time_(max_wait_time) {}

src/frc846/cpp/frc846/control/simulation/MCSimulator.cc → src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc

@@ -1,4 +1,4 @@
-#include "frc846/control/simulation/MCSimulator.h"
+#include "frc846/control/hardware/simulation/MCSimulator.h"
 
 #include "frc846/control/calculators/CurrentTorqueCalculator.h"
 #include "frc846/control/calculators/VelocityPositionEstimator.h"

src/frc846/include/frc846/control/HigherMotorController.h

@@ -90,6 +90,9 @@ class HigherMotorController {
   // Enables specific status frames and disables all others
   void EnableStatusFrames(std::vector<config::StatusFrame> frames);
 
+  // Verifies if the speed controller is connected and accessible
+  bool VerifyConnected();
+
 private:
   frc846::control::base::MotorMonkeyType mmtype_;
   frc846::control::config::MotorConstructionParameters constr_params_;

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

@@ -100,6 +100,8 @@ class MotorMonkey {
   static std::string parseError(
       frc846::control::hardware::ControllerErrorCodes err);
 
+  static bool VerifyConnected();
+
 private:
   static frc846::base::Loggable loggable_;
 

src/frc846/include/frc846/control/hardware/IntermediateController.h

@@ -64,6 +64,8 @@ class IntermediateController {
   virtual units::ampere_t GetCurrent() = 0;
 
   virtual ControllerErrorCodes GetLastErrorCode() = 0;
+
+  virtual bool VerifyConnected() = 0;  // changed
 };
 
 }  // namespace frc846::control::hardware

src/frc846/include/frc846/control/hardware/SparkMXFX_interm.h

@@ -159,6 +159,15 @@ class SparkMXFX_interm : public IntermediateController {
   */
   void ZeroEncoder(units::radian_t position) override;
 
+  /*
+  VerifyHardware()
+
+  Checks the TalonFX hardware and verifies it is functioning correctly.
+
+  @return boolean indicating whether the hardware is verified successfully.
+  */
+  bool VerifyConnected() override;  // added
+
   /*
   GetVelocity()
 

src/frc846/include/frc846/control/hardware/TalonFX_interm.h

@@ -156,6 +156,15 @@ class TalonFX_interm : public IntermediateController {
   */
   void ZeroEncoder(units::radian_t position) override;
 
+  /*
+  VerifyHardware()
+
+  Checks the TalonFX hardware and verifies it is functioning correctly.
+
+  @return boolean indicating whether the hardware is verified successfully.
+  */
+  bool VerifyConnected() override;  // added
+
   /*
   GetVelocity()
 

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

@@ -8,6 +8,7 @@
 
 #include <variant>
 
+#include "frc846/control/hardware/IntermediateController.h"
 #include "frc846/control/base/motor_gains.h"
 #include "frc846/control/base/motor_specs.h"
 #include "frc846/wpilib/units.h"
@@ -20,7 +21,7 @@ MCSimulator
 A class that simulates a motor controller. Uses motor dynamics to simulate
 position and velocity accurately.
 */
-class MCSimulator {
+class MCSimulator : frc846::control::hardware::IntermediateController {
 public:
   MCSimulator(frc846::control::base::MotorSpecs specs,
       frc846::wpilib::unit_ohm circuit_resistance,
@@ -33,19 +34,19 @@ class MCSimulator {
   */
   void Tick(units::volt_t battery_voltage, units::newton_meter_t load);
 
-  void WriteDC(double duty_cycle);
-  void WriteVelocity(units::radians_per_second_t velocity);
-  void WritePosition(units::radian_t position);
+  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();
-  units::radians_per_second_t GetVelocity();
+  units::radian_t GetPosition() override;
+  units::radians_per_second_t GetVelocity() override;
 
   /*
   ZeroEncoder()
 
   Sets the motor's position to specified value.
   */
-  void ZeroEncoder(units::radian_t position = 0_rad);
+  void ZeroEncoder(units::radian_t position = 0_rad) override;
 
   /*
   DisablePositionPacket()
@@ -60,7 +61,7 @@ class MCSimulator {
   */
   void DisableVelocityPacket();
 
-  void SetGains(frc846::control::base::MotorGains gains);
+  void SetGains(frc846::control::base::MotorGains gains) override;
 
 private:
   frc846::control::base::MotorSpecs specs;