Added DrivetrainConstructor, fixed build errors

README.md

@@ -177,7 +177,9 @@ To undo the going back:
 ## CppCheck Warnings
 
 ```
-src\frc846\cpp\frc846\robot\swerve\drivetrain.cc:18:39: warning: Variable 'locations' is assigned a value that is never used. [unreadVariable]
+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\y2025\cpp\subsystems\hardware\DrivetrainConstructor.cc:14:14: warning: Same expression on both sides of '/'. [duplicateExpression]
 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/hardware/SparkMXFX_interm.cc

@@ -10,6 +10,7 @@ namespace frc846::control::hardware {
   if (last_error_ != ControllerErrorCodes::kAllOK) return
 
 bool SparkMXFX_interm::VerifyConnected() {
+  if (esc_ == nullptr) return false;
   esc_->GetFirmwareVersion();
   return esc_->GetFirmwareVersion() != 0;
 }

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

@@ -5,24 +5,13 @@
 namespace frc846::robot::swerve {
 
 DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)
-    : GenericSubsystem{"SwerveDrivetrain"}, configs_{configs}, modules_{} {
-  // if (configs_.navX_connection_mode == NavX_connection_type::kSPI) {
-  //   navX_ = new AHRS{frc::SPI::Port::kMXP};
-  // } else if (configs_.navX_connection_mode == NavX_connection_type::kSerial)
-  // {
-  //   navX_ = new AHRS{frc::SerialPort::Port::kMXP};
-  // } else {
-  //   throw std::runtime_error("Invalid navX connection mode");
-  // }
-
-  // TODO: finish
-
-  std::array<std::string, 4> locations{"FR", "FL", "BL", "BR"};
-  std::array<int, 4> can_ids {}
-
+    : GenericSubsystem{"SwerveDrivetrain"},
+      configs_{configs},
+      modules_{},
+      navX_{frc::SerialPort::kMXP} {
   for (int i = 0; i < 4; i++) {
-    modules_[i] =
-        new SwerveModuleSubsystem{*this, "Module" + std::to_string(i)};
+    modules_[i] = new SwerveModuleSubsystem{*this,
+        configs_.module_unique_configs[i], configs_.module_common_config};
   }
 
   RegisterPreference("steer_gains/kP", 0.3);
@@ -43,7 +32,6 @@ void DrivetrainSubsystem::Setup() {
     module->SetSteerGains(steer_gains);
     module->ZeroWithCANcoder();
   }
-  // TODO: finish
 }
 
 DrivetrainTarget DrivetrainSubsystem::ZeroTarget() const {
@@ -60,7 +48,9 @@ bool DrivetrainSubsystem::VerifyHardware() {
 }
 
 DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
-  units::degree_t bearing = navX_->GetAngle() * 1_deg;
+  units::degree_t bearing = navX_.GetAngle() * 1_deg;
+
+  Graph("readings/bearing", bearing);
 
   frc846::math::VectorND<units::inch, 4> drive_positions{
       0_in, 0_in, 0_in, 0_in};
@@ -81,52 +71,44 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
 
   velocity /= 4.0;
 
+  Graph("readings/velocity_x", velocity[0]);
+  Graph("readings/velocity_y", velocity[1]);
+
   frc846::robot::swerve::odometry::SwervePose new_pose{
       .position =
           odometry_.calculate({bearing, steer_positions, drive_positions})
               .position,
       .bearing = bearing,
       .velocity = velocity};
 
+  // TODO: consider bearing simulation
+
+  Graph("readings/position_x", new_pose.position[0]);
+  Graph("readings/position_y", new_pose.position[1]);
+
   return {new_pose};
 }
 
 void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) {
   // TODO: finish
   if (DrivetrainOLControlTarget* ol_target =
           std::get_if<DrivetrainOLControlTarget>(&target)) {
-    units::degree_t bearing = navX_->GetAngle() * 1_deg;
-
-    std::array<units::feet_per_second_t, 4> module_speeds;
-    std::array<units::degree_t, 4> module_angles;
-    for (int i = 0; i < 4; i++) {
-      frc846::math::VectorND<units::feet_per_second, 2> module_vel =
-          ol_target->velocity;
-
-      module_vel = module_vel.rotate(-bearing);
+    units::degree_t bearing = navX_.GetAngle() * 1_deg;
 
-      module_speeds[i] = module_vel.magnitude();
-      module_angles[i] = module_vel.angle();
-    }
+    /*
+    TODO: For each module, find the target direction and speed based on velocity
+    and turn speed. Then, construct a SwerveModuleOLControlTarget and write it
+    to the module.
+    */
 
     for (int i = 0; i < 4; i++) {
-      SwerveModuleOLControlTarget module_target{
-          module_speeds[i].to<double>(), module_angles[i]};
-
-      modules_[i]->UpdateHardware(module_target);
+      // modules_[i]->SetTarget(module_target);
+      // modules_[i]->UpdateHardware();
     }
   } else if (DrivetrainAccelerationControlTarget* accel_target =
                  std::get_if<DrivetrainAccelerationControlTarget>(&target)) {
-    DrivetrainReadings readings = GetReadings();
-    units::degree_t bearing = readings.pose.bearing;
-
-    frc846::math::VectorND<units::feet_per_second, 2> target_velocity =
-        readings.pose.velocity +
-        frc846::math::VectorND<units::feet_per_second, 2>{
-            accel_target->linear_acceleration};
-
-    DrivetrainOLControlTarget ol_target{target_velocity};
-    WriteToHardware(ol_target);
+    throw std::runtime_error("Acceleration control not yet implemented");
+    // TODO: finish later
   }
 }
 

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

@@ -11,11 +11,13 @@ SwerveModuleSubsystem::SwerveModuleSubsystem(Loggable& parent,
     SwerveModuleCommonConfig common_config)
     : frc846::robot::GenericSubsystem<SwerveModuleReadings, SwerveModuleTarget>(
           parent, unique_config.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_{common_config.motor_types,
+          getMotorParams(unique_config, common_config).first},
+      steer_{common_config.motor_types,
+          getMotorParams(unique_config, common_config).second},
+      cancoder_{unique_config.cancoder_id, common_config.bus} {
+  drive_helper_.SetConversion(common_config.drive_reduction);
+  steer_helper_.SetConversion(common_config.steer_reduction);
 
   drive_helper_.bind(&drive_);
   steer_helper_.bind(&steer_);
@@ -26,6 +28,27 @@ SwerveModuleSubsystem::SwerveModuleSubsystem(Loggable& parent,
   RegisterPreference("cancoder_offset_", 0.0_deg);
 }
 
+std::pair<frc846::control::config::MotorConstructionParameters,
+    frc846::control::config::MotorConstructionParameters>
+SwerveModuleSubsystem::getMotorParams(SwerveModuleUniqueConfig unique_config,
+    SwerveModuleCommonConfig common_config) {
+  frc846::control::config::MotorConstructionParameters drive_params =
+      common_config.drive_params;
+  frc846::control::config::MotorConstructionParameters steer_params =
+      common_config.steer_params;
+
+  drive_params.can_id = unique_config.drive_id;
+  steer_params.can_id = unique_config.steer_id;
+
+  drive_params.bus = common_config.bus;
+  steer_params.bus = common_config.bus;
+
+  drive_params.circuit_resistance = unique_config.circuit_resistance;
+  steer_params.circuit_resistance = unique_config.circuit_resistance;
+
+  return {drive_params, steer_params};
+}
+
 void SwerveModuleSubsystem::Setup() {
   drive_.EnableStatusFrames(
       {frc846::control::config::StatusFrame::kPositionFrame,
@@ -48,7 +71,8 @@ SwerveModuleTarget SwerveModuleSubsystem::ZeroTarget() const {
 
 bool SwerveModuleSubsystem::VerifyHardware() {
   bool ok = true;
-  // TODO: Add a verify hardware to motor controllers.
+  FRC846_VERIFY(drive_.VerifyConnected(), ok, "Could not verify drive motor");
+  FRC846_VERIFY(steer_.VerifyConnected(), ok, "Could not verify steer motor");
   return ok;
 }
 
@@ -86,11 +110,11 @@ SwerveModuleReadings SwerveModuleSubsystem::ReadFromHardware() {
   readings.drive_pos = drive_helper_.GetPosition();
   readings.steer_pos = steer_helper_.GetPosition();
 
-  Graph("drive_motor_vel", readings.vel);
-  Graph("drive_motor_pos", readings.drive_pos);
-  Graph("steer_motor_pos", readings.steer_pos);
+  Graph("readings/drive_motor_vel", readings.vel);
+  Graph("readings/drive_motor_pos", readings.drive_pos);
+  Graph("readings/steer_motor_pos", readings.steer_pos);
 
-  Graph("cancoder_pos",
+  Graph("readings/cancoder_pos",
       units::degree_t(cancoder_.GetAbsolutePosition().GetValue()));
 
   return readings;
@@ -99,19 +123,19 @@ SwerveModuleReadings SwerveModuleSubsystem::ReadFromHardware() {
 void SwerveModuleSubsystem::WriteToHardware(SwerveModuleTarget target) {
   if (SwerveModuleOLControlTarget* ol_target =
           std::get_if<SwerveModuleOLControlTarget>(&target)) {
-    Graph("ol_drive_target", ol_target->drive);
-    Graph("ol_steer_target", ol_target->steer);
+    Graph("target/ol_drive_target", ol_target->drive);
+    Graph("target/ol_steer_target", ol_target->steer);
 
     auto [steer_dir, invert_drive] =
         calculateSteerPosition(GetReadings().steer_pos, ol_target->steer);
 
-    Graph("steer_dir", steer_dir);
-    Graph("invert_drive", invert_drive);
+    Graph("target/steer_dir", steer_dir);
+    Graph("target/invert_drive", invert_drive);
 
     units::dimensionless::scalar_t cosine_comp =
         units::math::cos(steer_dir - GetReadings().steer_pos);
 
-    Graph("cosine_comp", cosine_comp.to<double>());
+    Graph("target/cosine_comp", cosine_comp.to<double>());
 
     drive_helper_.WriteDC(
         cosine_comp *
@@ -123,8 +147,8 @@ void SwerveModuleSubsystem::WriteToHardware(SwerveModuleTarget target) {
   } else if (SwerveModuleTorqueControlTarget* torque_target =
                  std::get_if<SwerveModuleTorqueControlTarget>(&target)) {
     // TODO: finish torque control for drivetrain
-    Graph("torque_drive_target", torque_target->drive);
-    Graph("torque_steer_target", torque_target->steer);
+    Graph("target/torque_drive_target", torque_target->drive);
+    Graph("target/torque_steer_target", torque_target->steer);
   } else {
     throw std::runtime_error("SwerveModuleTarget was not of a valid type");
   }

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

@@ -17,8 +17,6 @@ Contains all parameters necessary to construct a motor controller.
 @param inverted: Whether the motor controller is inverted.
 @param brake_mode: Whether the motor controller is in brake mode.
 @param motor_current_limit: The current limit maintained onboard the ESC.
-@param threshold_time: The time threshold for the current limit specified
-earlier.
 @param smart_current_limit: The smart current limit for the motor controller.
 @param voltage_compensation: The voltage compensation of the motor controller.
 @param circuit_resistance: The circuit resistance leading upto motor controller.

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

@@ -24,21 +24,11 @@ Contains all configs related to the specific drivetrain in use.
 struct DrivetrainConfigs {
   NavX_connection_type navX_connection_mode;
 
-  int FRD_id;
-  int FRS_id;
-  int FRC_id;
+  SwerveModuleCommonConfig module_common_config;
+  std::array<SwerveModuleUniqueConfig, 4> module_unique_configs;
 
-  int FLD_id;
-  int FLS_id;
-  int FLC_id;
-
-  int BLD_id;
-  int BLS_id;
-  int BLC_id;
-
-  int BRD_id;
-  int BRS_id;
-  int BRC_id;
+  units::inch_t wheelbase_horizontal_dim;
+  units::inch_t wheelbase_forward_dim;
 };
 
 struct DrivetrainReadings {
@@ -84,7 +74,7 @@ class DrivetrainSubsystem
   DrivetrainConfigs configs_;
   std::array<SwerveModuleSubsystem*, 4> modules_;
 
-  AHRS* navX_ = new AHRS{frc::SerialPort::Port::kMXP};
+  AHRS navX_;
 
   frc846::robot::swerve::odometry::SwerveOdometryCalculator odometry_;
 };

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

@@ -41,6 +41,8 @@ struct SwerveModuleUniqueConfig {
   int cancoder_id;
   int drive_id;
   int steer_id;
+
+  frc846::wpilib::unit_ohm circuit_resistance;
 };
 
 using steer_conv_unit = units::dimensionless::scalar_t;
@@ -99,6 +101,17 @@ class SwerveModuleSubsystem
   void SetSteerGains(frc846::control::base::MotorGains gains);
 
 private:
+  /*
+  getMotorParams()
+
+  Static helper function modifies the drive and steer motor parameters provided
+  in the common configuration using the unique configuration.
+  */
+  static std::pair<frc846::control::config::MotorConstructionParameters,
+      frc846::control::config::MotorConstructionParameters>
+  getMotorParams(SwerveModuleUniqueConfig unique_config,
+      SwerveModuleCommonConfig common_config);
+
   SwerveModuleReadings ReadFromHardware() override;
 
   void WriteToHardware(SwerveModuleTarget target) override;

src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc

@@ -0,0 +1,61 @@
+#include "subsystems/hardware/DrivetrainConstructor.h"
+
+frc846::robot::swerve::DrivetrainConfigs
+DrivetrainConstructor::getDrivetrainConfigs() {
+  frc846::robot::swerve::DrivetrainConfigs configs;
+
+  /* START SETTABLES */
+  // TODO: Set these values to the correct values for the robot in 2025
+  configs.navX_connection_mode =
+      frc846::robot::swerve::NavX_connection_type::kSerial;
+
+  frc846::robot::swerve::steer_conv_unit steer_reduction = 1.0;  // TODO: fix
+  frc846::robot::swerve::drive_conv_unit drive_reduction =
+      1.0_ft / 1.0_tr;  // TODO: fix
+
+  frc846::robot::swerve::SwerveModuleUniqueConfig FR_config{
+      "FR", 999, 999, 999, 999_Ohm};
+  frc846::robot::swerve::SwerveModuleUniqueConfig FL_config{
+      "FL", 999, 999, 999, 999_Ohm};
+  frc846::robot::swerve::SwerveModuleUniqueConfig BR_config{
+      "BR", 999, 999, 999, 999_Ohm};
+  frc846::robot::swerve::SwerveModuleUniqueConfig BL_config{
+      "BL", 999, 999, 999, 999_Ohm};
+
+  configs.wheelbase_forward_dim = 26_in;
+  configs.wheelbase_horizontal_dim = 26_in;
+
+  /* END SETTABLES */
+
+  frc846::control::config::MotorConstructionParameters drive_params{
+      .can_id = 999,
+      .inverted = false,
+      .brake_mode = true,
+      .motor_current_limit = 999_A,
+      .smart_current_limit = 999_A,
+      .voltage_compensation = 999_V,
+      .circuit_resistance = 999_Ohm,
+      .rotational_inertia = frc846::wpilib::unit_kg_m_sq{999},
+      .bus = "",
+  };
+  frc846::control::config::MotorConstructionParameters steer_params{
+      .can_id = 999,
+      .inverted = false,
+      .brake_mode = false,
+      .motor_current_limit = 999_A,
+      .smart_current_limit = 999_A,
+      .voltage_compensation = 999_V,
+      .circuit_resistance = 999_Ohm,
+      .rotational_inertia = frc846::wpilib::unit_kg_m_sq{999},
+      .bus = "",
+  };
+
+  configs.module_common_config =
+      frc846::robot::swerve::SwerveModuleCommonConfig{drive_params,
+          steer_params,
+          frc846::control::base::MotorMonkeyType::TALON_FX_KRAKENX60,
+          steer_reduction, drive_reduction, ""};
+  configs.module_unique_configs = {FR_config, FL_config, BR_config, BL_config};
+
+  return configs;
+}

src/y2025/include/subsystems/hardware/DrivetrainConstructor.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include "frc846/robot/swerve/drivetrain.h"
+
+/*
+DrivetrainConstructor
+
+A class providing static methods to aid construction of a DrivetrainSubsystem
+object.
+*/
+class DrivetrainConstructor {
+public:
+  static frc846::robot::swerve::DrivetrainConfigs getDrivetrainConfigs();
+};