ISingleAxisSubsystem fixes

lib/cpp/subzero/autonomous/AutoFactory.cpp

@@ -11,7 +11,7 @@ bool AutoFactory<T>::AutoFileExists(const std::string fileName) {
 
   std::error_code error_code;
   std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
-      wpi::MemoryBuffer::GetFile(filePath, error_code);
+      wpi::MemoryBuffer::GetFile(filePath);
 
   if (fileBuffer == nullptr || error_code) {
     return false;

lib/cpp/subzero/motor/PidMotorController.cpp

@@ -5,47 +5,51 @@
 using namespace subzero;
 
 template <typename TMotor, typename TController, typename TRelativeEncoder,
-          typename TAbsoluteEncoder>
-PidMotorController<TMotor, TController, TRelativeEncoder, TAbsoluteEncoder>::
-    PidMotorController(std::string name, TMotor &motor,
-                       TRelativeEncoder &encoder, TController &controller,
-                       PidSettings pidSettings, TAbsoluteEncoder *absEncoder,
-                       units::revolutions_per_minute_t maxRpm)
+          typename TAbsoluteEncoder, typename TPidConfig>
+PidMotorController<
+    TMotor, TController, TRelativeEncoder, TAbsoluteEncoder,
+    TPidConfig>::PidMotorController(std::string name, TMotor &motor,
+                                    TRelativeEncoder &encoder,
+                                    TController &controller,
+                                    PidSettings pidSettings,
+                                    TAbsoluteEncoder *absEncoder,
+                                    units::revolutions_per_minute_t maxRpm)
     : IPidMotorController(name), m_motor{motor}, m_controller{controller},
       m_encoder{encoder}, m_absEncoder{absEncoder}, m_settings{pidSettings},
       m_pidController{
           frc::PIDController{pidSettings.p, pidSettings.i, pidSettings.d}},
-      m_maxRpm{maxRpm} {
+      m_maxRpm{maxRpm}, m_isInitialized{false} {
+
   // Doing it here so the PID controllers themselves get updated
   UpdatePidSettings(pidSettings);
 }
 
 template <typename TMotor, typename TController, typename TRelativeEncoder,
-          typename TAbsoluteEncoder>
-void PidMotorController<TMotor, TController, TRelativeEncoder,
-                        TAbsoluteEncoder>::Set(units::volt_t volts) {
+          typename TAbsoluteEncoder, typename TPidConfig>
+void PidMotorController<TMotor, TController, TRelativeEncoder, TAbsoluteEncoder,
+                        TPidConfig>::Set(units::volt_t volts) {
   frc::SmartDashboard::PutNumber(m_name + " Commanded volts", volts.value());
   m_motor.SetVoltage(volts);
 }
 
 template <typename TMotor, typename TController, typename TRelativeEncoder,
-          typename TAbsoluteEncoder>
-void PidMotorController<TMotor, TController, TRelativeEncoder,
-                        TAbsoluteEncoder>::Set(double percentage) {
+          typename TAbsoluteEncoder, typename TPidConfig>
+void PidMotorController<TMotor, TController, TRelativeEncoder, TAbsoluteEncoder,
+                        TPidConfig>::Set(double percentage) {
   m_motor.Set(percentage);
 }
 
 template <typename TMotor, typename TController, typename TRelativeEncoder,
-          typename TAbsoluteEncoder>
-void PidMotorController<TMotor, TController, TRelativeEncoder,
-                        TAbsoluteEncoder>::SetPidTolerance(double tolerance) {
+          typename TAbsoluteEncoder, typename TPidConfig>
+void PidMotorController<TMotor, TController, TRelativeEncoder, TAbsoluteEncoder,
+                        TPidConfig>::SetPidTolerance(double tolerance) {
   m_pidController.SetTolerance(tolerance);
 }
 
 template <typename TMotor, typename TController, typename TRelativeEncoder,
-          typename TAbsoluteEncoder>
-void PidMotorController<TMotor, TController, TRelativeEncoder,
-                        TAbsoluteEncoder>::Update() {
+          typename TAbsoluteEncoder, typename TPidConfig>
+void PidMotorController<TMotor, TController, TRelativeEncoder, TAbsoluteEncoder,
+                        TPidConfig>::Update() {
   if (m_absolutePositionEnabled) {
     // ConsoleWriter.logVerbose(
     //     m_name,
@@ -67,20 +71,20 @@ void PidMotorController<TMotor, TController, TRelativeEncoder,
 }
 
 template <typename TMotor, typename TController, typename TRelativeEncoder,
-          typename TAbsoluteEncoder>
+          typename TAbsoluteEncoder, typename TPidConfig>
 void PidMotorController<
-    TMotor, TController, TRelativeEncoder,
-    TAbsoluteEncoder>::RunWithVelocity(units::revolutions_per_minute_t rpm) {
+    TMotor, TController, TRelativeEncoder, TAbsoluteEncoder,
+    TPidConfig>::RunWithVelocity(units::revolutions_per_minute_t rpm) {
   m_absolutePositionEnabled = false;
   frc::SmartDashboard::PutNumber(m_name + "commanded rpm", rpm.value());
   m_controller.SetReference(rpm.value(),
                             rev::spark::SparkLowLevel::ControlType::kVelocity);
 }
 
 template <typename TMotor, typename TController, typename TRelativeEncoder,
-          typename TAbsoluteEncoder>
-void PidMotorController<TMotor, TController, TRelativeEncoder,
-                        TAbsoluteEncoder>::RunWithVelocity(double percentage) {
+          typename TAbsoluteEncoder, typename TPidConfig>
+void PidMotorController<TMotor, TController, TRelativeEncoder, TAbsoluteEncoder,
+                        TPidConfig>::RunWithVelocity(double percentage) {
   if (abs(percentage) > 1.0) {
     ConsoleWriter.logError("PidMotorController",
                            "Incorrect percentages for motor %s: Value=%.4f ",
@@ -92,9 +96,9 @@ void PidMotorController<TMotor, TController, TRelativeEncoder,
 }
 
 template <typename TMotor, typename TController, typename TRelativeEncoder,
-          typename TAbsoluteEncoder>
-void PidMotorController<TMotor, TController, TRelativeEncoder,
-                        TAbsoluteEncoder>::RunToPosition(double position) {
+          typename TAbsoluteEncoder, typename TPidConfig>
+void PidMotorController<TMotor, TController, TRelativeEncoder, TAbsoluteEncoder,
+                        TPidConfig>::RunToPosition(double position) {
   ConsoleWriter.logVerbose(m_name + " Target position", position);
   Stop();
   m_pidController.Reset();
@@ -103,10 +107,10 @@ void PidMotorController<TMotor, TController, TRelativeEncoder,
 }
 
 template <typename TMotor, typename TController, typename TRelativeEncoder,
-          typename TAbsoluteEncoder>
+          typename TAbsoluteEncoder, typename TPidConfig>
 std::optional<double>
-PidMotorController<TMotor, TController, TRelativeEncoder,
-                   TAbsoluteEncoder>::GetAbsoluteEncoderPosition() {
+PidMotorController<TMotor, TController, TRelativeEncoder, TAbsoluteEncoder,
+                   TPidConfig>::GetAbsoluteEncoderPosition() {
   if (m_absEncoder) {
     return m_absEncoder->GetPosition();
   }
@@ -115,46 +119,69 @@ PidMotorController<TMotor, TController, TRelativeEncoder,
 }
 
 template <typename TMotor, typename TController, typename TRelativeEncoder,
-          typename TAbsoluteEncoder>
-void PidMotorController<TMotor, TController, TRelativeEncoder,
-                        TAbsoluteEncoder>::Stop() {
+          typename TAbsoluteEncoder, typename TPidConfig>
+void PidMotorController<TMotor, TController, TRelativeEncoder, TAbsoluteEncoder,
+                        TPidConfig>::Stop() {
   m_absolutePositionEnabled = false;
   m_motor.Set(0);
 }
 
 template <typename TMotor, typename TController, typename TRelativeEncoder,
-          typename TAbsoluteEncoder>
-void PidMotorController<TMotor, TController, TRelativeEncoder,
-                        TAbsoluteEncoder>::UpdatePidSettings(PidSettings
-                                                                 settings) {
-  if (settings.p != m_settings.p) {
+          typename TAbsoluteEncoder, typename TPidConfig>
+void PidMotorController<TMotor, TController, TRelativeEncoder, TAbsoluteEncoder,
+                        TPidConfig>::UpdatePidSettings(PidSettings settings) {
+
+  bool changed = false;
+
+  if (settings.p != m_settings.p || !m_isInitialized) {
     ConsoleWriter.logInfo("PidMotorController", "Setting P to %.6f for %s",
                           settings.p, m_name.c_str());
-    m_controller.SetP(settings.p);
+
+    m_config.closedLoop.P(settings.p);
+    changed = true;
   }
 
-  if (settings.i != m_settings.i) {
+  if (settings.i != m_settings.i || !m_isInitialized) {
     ConsoleWriter.logInfo("PidMotorController", "Setting I to %.6f for %s",
                           settings.i, m_name.c_str());
-    m_controller.SetI(settings.i);
+    m_config.closedLoop.I(settings.i);
+    changed = true;
   }
 
-  if (settings.d != m_settings.d) {
+  if (settings.d != m_settings.d || !m_isInitialized) {
     ConsoleWriter.logInfo("PidMotorController", "Setting D to %.6f for %s",
                           settings.d, m_name.c_str());
-    m_controller.SetD(settings.d);
+    m_config.closedLoop.D(settings.d);
+    changed = true;
   }
 
-  if (settings.iZone != m_settings.iZone) {
+  if (settings.iZone != m_settings.iZone || !m_isInitialized) {
     ConsoleWriter.logInfo("PidMotorController", "Setting IZone to %.6f for %s",
                           settings.iZone, m_name.c_str());
-    m_controller.SetIZone(settings.iZone);
+    m_config.closedLoop.IZone(settings.iZone);
+    changed = true;
   }
 
-  if (settings.ff != m_settings.ff) {
+  if (settings.ff != m_settings.ff || !m_isInitialized) {
     ConsoleWriter.logInfo("PidMotorController", "Setting FF to %.6f for %s",
                           settings.ff, m_name.c_str());
-    m_controller.SetFF(settings.ff);
+    m_config.closedLoop.VelocityFF(settings.ff);
+    changed = true;
+  }
+
+  if (settings.isIdleModeBrake != m_settings.isIdleModeBrake ||
+      !m_isInitialized) {
+    m_config.SetIdleMode(m_settings.isIdleModeBrake
+                             ? rev::spark::SparkBaseConfig::IdleMode::kBrake
+                             : rev::spark::SparkBaseConfig::IdleMode::kCoast);
+    changed = true;
+  }
+
+  if (changed || !m_isInitialized) {
+    m_motor.Configure(m_config,
+                      rev::spark::SparkBase::ResetMode::kNoResetSafeParameters,
+                      rev::spark::SparkBase::PersistMode::kPersistParameters);
+    m_isInitialized = true;
   }
 
   m_settings = settings;

lib/include/subzero/autonomous/AutoFactory.h

@@ -34,31 +34,9 @@ template <typename T> class AutoFactory {
 private:
   const std::map<T, std::string> &m_autos;
 
-  bool AutoFileExists(const std::string fileName) {
-    const std::string filePath = frc::filesystem::GetDeployDirectory() +
-                                 "/pathplanner/autos/" + fileName + ".auto";
+  bool AutoFileExists(const std::string fileName);
 
-    // Use wpi::MemoryBuffer::GetFile and handle the wpi::expected result
-    auto fileBufferResult = wpi::MemoryBuffer::GetFile(filePath);
-
-    if (!fileBufferResult) {
-      // fileBufferResult is an error; the file does not exist
-      return false;
-    }
-
-    // fileBufferResult contains a valid std::unique_ptr<wpi::MemoryBuffer>
-    return true;
-  }
-
-  frc2::CommandPtr PathPlannerPathFromName(const std::string autoName) {
-    if (!AutoFileExists(autoName)) {
-      ConsoleWriter.logError("Auto Factory",
-                             "AUTO '%s' DOES NOT EXIST HELP US EVAN",
-                             autoName.c_str());
-      return EmptyCommand().ToPtr();
-    }
-    return pathplanner::PathPlannerAuto(autoName).ToPtr();
-  }
+  frc2::CommandPtr PathPlannerPathFromName(const std::string autoName);
 
 public:
   /**
@@ -67,18 +45,7 @@ template <typename T> class AutoFactory {
    * @param type
    * @return frc2::CommandPtr The schedulable auto command
    */
-  frc2::CommandPtr GetAuto(T type) {
-    if (!m_autos.contains(type)) {
-      ConsoleWriter.logWarning(
-          "Auto Factory",
-          "Auto type %d does not exist, defaulting to empty "
-          "auto",
-          static_cast<int>(type));
-      return EmptyCommand().ToPtr();
-    }
-
-    return PathPlannerPathFromName(m_autos.at(type));
-  }
+  frc2::CommandPtr GetAuto(T type);
 };
 
 } // namespace subzero

lib/include/subzero/motor/IPidMotorController.h

@@ -7,8 +7,11 @@
 #include <string>
 
 namespace subzero {
+
+// TODO: Add slot support inside of PidSettings struct
 struct PidSettings {
   double p, i, d, iZone, ff;
+  bool isIdleModeBrake;
 };
 
 class IPidMotorController {