Merge motor control changes into drivetrain_impl

README.md

@@ -176,8 +176,8 @@ To undo the going back:
 
 ## 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]
 ```

build.gradle

@@ -182,12 +182,7 @@ task runCppcheck(type: Exec) {
             }
         }
 
-        def cppcheckSection = """
-## CppCheck Warnings
-```
-${reportContent.trim()}
-```
-"""
+        def cppcheckSection = """## CppCheck Warnings\n```\n${reportContent.trim()}\n```\n"""
         def readmeFile = file('README.md')
         def readmeContent = readmeFile.text
 
@@ -205,4 +200,4 @@ runCppcheck.onlyIf { !project.hasProperty('fromCI') }
 if (!project.hasProperty('fromCI')) {
     check.dependsOn runCppcheck
     runCppcheck.dependsOn spotlessApply
-}
+}

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

@@ -15,6 +15,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

@@ -74,6 +74,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

@@ -8,6 +8,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

@@ -86,6 +86,9 @@ class HigherMotorController {
   // Custom soft limits maintained by HigherMotorController
   void SetSoftLimits(config::SoftLimits soft_limits);
 
+  // verify's if the hardware is connected
+  bool VerifyConnected();
+
 private:
   frc846::control::base::MotorMonkeyType mmtype_;
   frc846::control::config::MotorConstructionParameters constr_params_;

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

@@ -90,6 +90,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

@@ -158,6 +158,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/846_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;