Finished DrivetrainConstructor, added preferences

Team846 · Jan 2, 2025 · 832fd3e · 832fd3e
1 parent da6b702
commit 832fd3e
Show file tree

Hide file tree

Showing 4 changed files with 106 additions and 30 deletions.
diff --git a/README.md b/README.md
@@ -179,7 +179,6 @@ To undo the going back:
 ```
 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]

diff --git a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc
@@ -1,52 +1,124 @@
 #include "subsystems/hardware/DrivetrainConstructor.h"
 
+#include "frc846/control/calculators/CircuitResistanceCalculator.h"
+#include "ports.h"
+
+DrivetrainConstructor::DrivetrainConstructor()
+    : Loggable{"DrivetrainConstructor"} {
+  RegisterPreference("drive_motor_current_limit", 120_A);
+  RegisterPreference("steer_motor_current_limit", 120_A);
+  RegisterPreference("drive_motor_smart_current_limit", 80_A);
+  RegisterPreference("steer_motor_smart_current_limit", 80_A);
+
+  RegisterPreference("drive_motor_voltage_compensation", 16_V);
+  RegisterPreference("steer_motor_voltage_compensation", 12_V);
+}
+
 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
+  /*
+  START SETTABLES
+  TODO: Set these values during season
+  */
+
   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
+  units::inch_t wheel_diameter = 4_in;
+
+  units::inch_t wire_length_FL = 14_in;
+  units::inch_t wire_length_FR = 14_in;
+  units::inch_t wire_length_BL = 14_in;
+  units::inch_t wire_length_BR = 14_in;
 
-  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};
+  unsigned int num_connectors_FR = 3;
+  unsigned int num_connectors_FL = 3;
+  unsigned int num_connectors_BL = 3;
+  unsigned int num_connectors_BR = 3;
+
+  frc846::robot::swerve::drive_conv_unit drive_reduction =
+      (M_PI * wheel_diameter) / 6.12_tr;
+  frc846::robot::swerve::steer_conv_unit steer_reduction = 7_tr / 150_tr;
 
   configs.wheelbase_forward_dim = 26_in;
   configs.wheelbase_horizontal_dim = 26_in;
 
+  units::pound_t wheel_approx_weight = 2_lb;
+  units::inch_t wheel_weight_radius = 1_in;
+
+  units::pound_t robot_weight = 120_lb;
+
+  // (Mass wheel) * (wheel_r)^2 * (steer reduction)^2
+  frc846::wpilib::unit_kg_m_sq relative_steer_inertia{
+      wheel_approx_weight * (wheel_weight_radius * wheel_weight_radius) *
+      (steer_reduction * steer_reduction).to<double>()};
+
+  // (Mass robot) * [(wheel_d)/2]^2 * (drive reduction)^2
+  frc846::wpilib::unit_kg_m_sq relative_drive_inertia{
+      robot_weight * (wheel_diameter * wheel_diameter) *
+      (drive_reduction * drive_reduction).to<double>()};
+
   /* END SETTABLES */
 
+  frc846::wpilib::unit_ohm wire_resistance_FR{
+      frc846::control::calculators::CircuitResistanceCalculator::calculate(
+          wire_length_FR, frc846::control::calculators::fourteen_gauge,
+          num_connectors_FR)};
+  frc846::wpilib::unit_ohm wire_resistance_FL{
+      frc846::control::calculators::CircuitResistanceCalculator::calculate(
+          wire_length_FL, frc846::control::calculators::fourteen_gauge,
+          num_connectors_FL)};
+
+  frc846::wpilib::unit_ohm wire_resistance_BL{
+      frc846::control::calculators::CircuitResistanceCalculator::calculate(
+          wire_length_BL, frc846::control::calculators::fourteen_gauge,
+          num_connectors_BL)};
+  frc846::wpilib::unit_ohm wire_resistance_BR{
+      frc846::control::calculators::CircuitResistanceCalculator::calculate(
+          wire_length_BR, frc846::control::calculators::fourteen_gauge,
+          num_connectors_BR)};
+
+  frc846::robot::swerve::SwerveModuleUniqueConfig FR_config{"FR",
+      ports::drivetrain_::kFRDrive_CANID, ports::drivetrain_::kFRSteer_CANID,
+      ports::drivetrain_::kFRCANCoder_CANID, wire_resistance_FR};
+  frc846::robot::swerve::SwerveModuleUniqueConfig FL_config{"FL",
+      ports::drivetrain_::kFLDrive_CANID, ports::drivetrain_::kFLSteer_CANID,
+      ports::drivetrain_::kFLCANCoder_CANID, wire_resistance_FL};
+  frc846::robot::swerve::SwerveModuleUniqueConfig BL_config{"BL",
+      ports::drivetrain_::kBLDrive_CANID, ports::drivetrain_::kBLSteer_CANID,
+      ports::drivetrain_::kBLCANCoder_CANID, wire_resistance_BL};
+  frc846::robot::swerve::SwerveModuleUniqueConfig BR_config{"BR",
+      ports::drivetrain_::kBRDrive_CANID, ports::drivetrain_::kBRSteer_CANID,
+      ports::drivetrain_::kBRCANCoder_CANID, wire_resistance_BR};
+
   frc846::control::config::MotorConstructionParameters drive_params{
-      .can_id = 999,
+      .can_id = 999,  // overriden by unique config
       .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},
+      .motor_current_limit = GetPreferenceValue_unit_type<units::ampere_t>(
+          "drive_motor_current_limit"),
+      .smart_current_limit = GetPreferenceValue_unit_type<units::ampere_t>(
+          "drive_motor_smart_current_limit"),
+      .voltage_compensation = GetPreferenceValue_unit_type<units::volt_t>(
+          "drive_motor_voltage_compensation"),
+      .circuit_resistance = 999_Ohm,  // overriden by unique config
+      .rotational_inertia = relative_drive_inertia,
       .bus = "",
   };
   frc846::control::config::MotorConstructionParameters steer_params{
-      .can_id = 999,
+      .can_id = 999,  // overriden by unique config
       .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},
+      .motor_current_limit = GetPreferenceValue_unit_type<units::ampere_t>(
+          "steer_motor_current_limit"),
+      .smart_current_limit = GetPreferenceValue_unit_type<units::ampere_t>(
+          "steer_motor_smart_current_limit"),
+      .voltage_compensation = GetPreferenceValue_unit_type<units::volt_t>(
+          "steer_motor_voltage_compensation"),
+      .circuit_resistance = 999_Ohm,  // overriden by unique config
+      .rotational_inertia = relative_steer_inertia,
       .bus = "",
   };
 

diff --git a/src/y2025/include/subsystems/hardware/DrivetrainConstructor.h b/src/y2025/include/subsystems/hardware/DrivetrainConstructor.h
@@ -1,14 +1,17 @@
 #pragma once
 
+#include "frc846/base/Loggable.h"
 #include "frc846/robot/swerve/drivetrain.h"
 
 /*
 DrivetrainConstructor
 
-A class providing static methods to aid construction of a DrivetrainSubsystem
+A class providing methods to aid construction of a DrivetrainSubsystem
 object.
 */
-class DrivetrainConstructor {
+class DrivetrainConstructor : public frc846::base::Loggable {
 public:
-  static frc846::robot::swerve::DrivetrainConfigs getDrivetrainConfigs();
+  DrivetrainConstructor();
+
+  frc846::robot::swerve::DrivetrainConfigs getDrivetrainConfigs();
 };
diff --git a/src/y2025/include/subsystems/robot_container.h b/src/y2025/include/subsystems/robot_container.h
@@ -9,8 +9,10 @@ class RobotContainer : public frc846::robot::GenericRobotContainer {
 public:
   ControlInputSubsystem control_input_{};
   LEDsSubsystem leds_{};
+
+  DrivetrainConstructor drivetrain_constructor_{};
   frc846::robot::swerve::DrivetrainSubsystem drivetrain_{
-      DrivetrainConstructor::getDrivetrainConfigs()};
+      drivetrain_constructor_.getDrivetrainConfigs()};
 
   RobotContainer() {
     RegisterPreference("init_drivetrain", true);