Partial implementation of swerve_module

README.md

@@ -174,6 +174,11 @@ 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/control/config/SoftLimitsHelper.cc → src/frc846/cpp/frc846/control/config/soft_limits.cc

@@ -1,4 +1,4 @@
-#include "frc846/control/config/SoftLimitsHelper.h"
+#include "frc846/control/config/soft_limits.h"
 
 namespace frc846::control::config {
 

src/frc846/cpp/frc846/robot/swerve/swerve_module.cc

@@ -0,0 +1,54 @@
+#include "frc846/robot/swerve/swerve_module.h"
+
+namespace frc846::robot::swerve {
+
+SwerveModule::SwerveModule(Loggable& parent, std::string loc,
+    frc846::control::config::MotorConstructionParameters drive_params,
+    frc846::control::config::MotorConstructionParameters steer_params,
+    frc846::control::base::MotorMonkeyType motor_types, int cancoder_id,
+    steer_conv_unit steer_reduction, drive_conv_unit drive_reduction,
+    std::string cancoder_bus)
+    : frc846::robot::GenericSubsystem<SwerveModuleReadings, SwerveModuleTarget>(
+          parent, loc),
+      drive_{motor_types, drive_params},
+      steer_{motor_types, steer_params},
+      cancoder_{cancoder_id, cancoder_bus} {
+  drive_helper_.SetConversion(drive_reduction);
+  steer_helper_.SetConversion(steer_reduction);
+
+  drive_helper_.bind(&drive_);
+  steer_helper_.bind(&steer_);
+}
+
+void SwerveModule::Setup() {
+  drive_.Setup();
+  steer_.Setup();
+
+  // TODO: finish. Refer to howler_monkey.
+}
+
+SwerveModuleTarget SwerveModule::ZeroTarget() const {
+  return SwerveModuleOLControlTarget{0.0, 0_deg};
+}
+
+bool SwerveModule::VerifyHardware() {
+  bool ok = true;
+  // TODO: Add a verify hardware to motor controllers.
+  return ok;
+}
+
+SwerveModuleReadings SwerveModule::ReadFromHardware() {
+  SwerveModuleReadings readings;
+  readings.vel = drive_helper_.GetVelocity();
+  readings.drive_pos = drive_helper_.GetPosition();
+  readings.steer_pos = steer_helper_.GetPosition();
+  return readings;
+
+  // TODO: recheck
+}
+
+void SwerveModule::WriteToHardware(SwerveModuleTarget target) {
+  // TODO: finish. Refer to howler_monkey for open-loop.
+}
+
+}  // namespace frc846::robot::swerve

src/frc846/include/frc846/control/HMCHelper.h

@@ -94,7 +94,7 @@ template <typename T> class HMCHelper {
   void SetSoftLimits(bool using_limits, pos_unit forward_limit = pos_unit(0),
       pos_unit reverse_limit = pos_unit(0),
       pos_unit forward_reduce = pos_unit(0),
-      pos_unit reverse_reduce = pos_unit(0), double reduce_max_dc = double) {
+      pos_unit reverse_reduce = pos_unit(0), double reduce_max_dc = 0.0) {
     hmc_->SetSoftLimits(config::SoftLimitsHelper<T>::CreateSoftLimits(conv_,
         using_limits, forward_limit, reverse_limit, forward_reduce,
         reverse_reduce, reduce_max_dc));

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

@@ -7,7 +7,7 @@
 #include "frc846/control/base/motor_specs.h"
 #include "frc846/control/config/construction_params.h"
 
-#include "frc846/control/config/SoftLimitsHelper.h"
+#include "frc846/control/config/soft_limits.h"
 
 #include <optional>
 

src/frc846/include/frc846/control/config/construction_params.h

@@ -46,7 +46,7 @@ struct MotorConstructionParameters {
 
   frc846::wpilib::unit_kg_m_sq rotational_inertia;
 
-  std::string bus = "rio";
+  std::string bus = "";
 
   units::millisecond_t max_wait_time = 20_ms;
 };

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

@@ -15,7 +15,7 @@ TalonFX hardware.
 */
 class TalonFX_interm : public IntermediateController {
 public:
-  TalonFX_interm(int can_id, std::string bus = "rio",
+  TalonFX_interm(int can_id, std::string bus = "",
       units::millisecond_t max_wait_time = 20_ms);
   /*
   Tick()

src/frc846/include/frc846/robot/GenericSubsystem.h

@@ -57,6 +57,17 @@ class GenericSubsystem : public frc2::SubsystemBase, public SubsystemBase {
     init_ = true;
   }
 
+  /*
+  InitByParent()
+
+  Initializer function to be called by a parent subsystem only. Will not
+  register with WPILib.
+  */
+  void InitByParent() {
+    Log("Initializing subsystem (by parent)");
+    init_ = true;
+  }
+
 private:
   bool init_;
 

src/frc846/include/frc846/robot/swerve/swerve_module.h

@@ -0,0 +1,76 @@
+#pragma once
+
+#include "frc846/robot/GenericSubsystem.h"
+#include "frc846/control/HigherMotorController.h"
+#include "frc846/control/HMCHelper.h"
+
+#include <units/angle.h>
+#include <units/torque.h>
+#include <units/velocity.h>
+#include <units/base.h>
+
+#include <variant>
+
+#include <ctre/phoenix6/CANCoder.hpp>
+
+namespace frc846::robot::swerve {
+
+struct SwerveModuleReadings {
+  units::feet_per_second_t vel;
+  units::foot_t drive_pos;
+  units::degree_t steer_pos;
+};
+
+struct SwerveModuleTorqueControlTarget {
+  units::newton_meter_t drive;
+  units::newton_meter_t steer;
+};
+
+struct SwerveModuleOLControlTarget {
+  double drive;
+  units::degree_t steer;
+};
+
+using SwerveModuleTarget =
+    std::variant<SwerveModuleTorqueControlTarget, SwerveModuleOLControlTarget>;
+
+class SwerveModule
+    : public frc846::robot::GenericSubsystem<SwerveModuleReadings,
+          SwerveModuleTarget> {
+  using steer_conv_unit = units::dimensionless::dimensionless_t;
+  using drive_conv_unit = units::unit_t<
+      units::compound_unit<units::foot, units::inverse<units::turn>>>;
+
+public:
+  SwerveModule(Loggable& parent, std::string loc,
+      frc846::control::config::MotorConstructionParameters drive_params,
+      frc846::control::config::MotorConstructionParameters steer_params,
+      frc846::control::base::MotorMonkeyType motor_types, int cancoder_id,
+      steer_conv_unit steer_reduction, drive_conv_unit drive_reduction,
+      std::string cancoder_bus = "");
+
+  void Setup() override;
+
+  SwerveModuleTarget ZeroTarget() const override;
+
+  bool VerifyHardware() override;
+
+  void SetMaxSpeed(units::feet_per_second_t max_speed);
+
+private:
+  SwerveModuleReadings ReadFromHardware() override;
+
+  void WriteToHardware(SwerveModuleTarget target) override;
+
+  units::feet_per_second_t max_speed_ = 10.0_fps;
+
+  frc846::control::HigherMotorController drive_;
+  frc846::control::HigherMotorController steer_;
+
+  frc846::control::HMCHelper<units::foot> drive_helper_;
+  frc846::control::HMCHelper<units::degree> steer_helper_;
+
+  ctre::phoenix6::hardware::CANcoder cancoder_;
+};
+
+}  // namespace frc846::robot::swerve