Finished drivetrain control, drive_command; Tested with motor physics…

… simulation

README.md

@@ -180,9 +180,7 @@ To undo the going back:
 ## CppCheck Warnings
 
 ```
-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\frc846\cpp\frc846\math\collection.cc:11:0: warning: The function 'HorizontalDeadband' is never used. [unusedFunction]
+src\frc846\cpp\frc846\robot\swerve\drivetrain.cc:152:64: warning: Variable 'accel_target' is assigned a value that is never used. [unreadVariable]
 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]
 ```

networktables.json

@@ -134,5 +134,53 @@
     "properties": {
       "persistent": true
     }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/SwerveDrivetrain/max_speed (fps)",
+    "type": "double",
+    "value": 15.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/SwerveDrivetrain/max_omega (deg_per_s)",
+    "type": "double",
+    "value": 180.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/translation_deadband",
+    "type": "double",
+    "value": 0.07,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/translation_exponent",
+    "type": "int",
+    "value": 2,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/rotation_deadband",
+    "type": "double",
+    "value": 0.07,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/rotation_exponent",
+    "type": "int",
+    "value": 2,
+    "properties": {
+      "persistent": true
+    }
   }
 ]

networktables.json.bck

@@ -54,5 +54,85 @@
     "properties": {
       "persistent": true
     }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/FL/SwerveDrivetrain/FL/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/BR/SwerveDrivetrain/BR/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/BL/SwerveDrivetrain/BL/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kP",
+    "type": "double",
+    "value": 0.3,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kI",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kD",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kF",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/robot_container/init_drivetrain",
+    "type": "boolean",
+    "value": true,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/robot_container/init_leds",
+    "type": "boolean",
+    "value": true,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/trigger_threshold",
+    "type": "double",
+    "value": 0.3,
+    "properties": {
+      "persistent": true
+    }
   }
 ]

simgui.json

@@ -4,10 +4,21 @@
       "/FMSInfo": "FMSInfo",
       "/SmartDashboard/get_prune_list": "Command",
       "/SmartDashboard/prune_prefs": "Command",
-      "/SmartDashboard/verify_hardware": "Command"
+      "/SmartDashboard/set_cancoder_offsets": "Command",
+      "/SmartDashboard/verify_hardware": "Command",
+      "/SmartDashboard/zero_bearing": "Command"
     }
   },
   "NetworkTables Info": {
+    "Connections": {
+      "open": true
+    },
+    "Server": {
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
     "visible": true
   }
 }

src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc

@@ -0,0 +1,57 @@
+#include "frc846/robot/swerve/control/swerve_ol_calculator.h"
+
+namespace frc846::robot::swerve::control {
+
+SwerveOpenLoopCalculatorOutput SwerveOpenLoopCalculator::calculate(
+    SwerveOpenLoopCalculatorInputs inputs) {
+  std::pair<double, double> kModuleLocationSigns[4] = {
+      {+0.5, +0.5},  // FR
+      {-0.5, +0.5},  // FL
+      {-0.5, -0.5},  // BL
+      {+0.5, -0.5},  // BR
+  };
+
+  std::array<frc846::math::VectorND<units::feet_per_second, 2>, 4>
+      module_targets;
+
+  units::inch_t radius = units::math::hypot(
+      constants_.wheelbase_horizontal_dim, constants_.wheelbase_forward_dim);
+
+  for (int i = 0; i < 4; i++) {
+    frc846::math::VectorND<units::inch, 2> location{
+        kModuleLocationSigns[i].first * constants_.wheelbase_horizontal_dim,
+        kModuleLocationSigns[i].second * constants_.wheelbase_forward_dim};
+
+    units::degree_t rot_direction = location.angle(false);
+
+    frc846::math::VectorND<units::feet_per_second, 2> rotation{
+        inputs.rotation_target * units::math::cos(rot_direction) * radius /
+            1_rad,
+        inputs.rotation_target * units::math::sin(rot_direction) * radius /
+            1_rad};
+
+    module_targets[i] = inputs.translation_target + rotation;
+  }
+
+  units::feet_per_second_t max_mag = 0_fps;
+  for (int i = 0; i < 4; i++) {
+    if (module_targets[i].magnitude() > max_mag) {
+      max_mag = module_targets[i].magnitude();
+    }
+  }
+
+  if (max_mag > inputs.max_speed) {
+    for (int i = 0; i < 4; i++) {
+      module_targets[i] *= inputs.max_speed / max_mag;
+    }
+  }
+
+  SwerveOpenLoopCalculatorOutput output{};
+  for (int i = 0; i < 4; i++) {
+    output.drive_outputs[i] = module_targets[i].magnitude();
+    output.steer_outputs[i] = module_targets[i].angle(true);
+  }
+  return output;
+}
+
+}  // namespace frc846::robot::swerve::control

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

@@ -1,5 +1,6 @@
 #include "frc846/robot/swerve/drivetrain.h"
 
+#include "frc846/robot/swerve/control/swerve_ol_calculator.h"
 #include "frc846/robot/swerve/swerve_module.h"
 
 namespace frc846::robot::swerve {
@@ -18,6 +19,15 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)
   RegisterPreference("steer_gains/kI", 0.0);
   RegisterPreference("steer_gains/kD", 0.0);
   RegisterPreference("steer_gains/kF", 0.0);
+
+  RegisterPreference("max_speed", 15_fps);
+  RegisterPreference("max_omega", units::degrees_per_second_t{180});
+
+  odometry_.setConstants({});
+  ol_calculator_.setConstants({
+      .wheelbase_horizontal_dim = configs.wheelbase_horizontal_dim,
+      .wheelbase_forward_dim = configs.wheelbase_forward_dim,
+  });
 }
 
 void DrivetrainSubsystem::Setup() {
@@ -35,7 +45,7 @@ void DrivetrainSubsystem::Setup() {
 }
 
 DrivetrainTarget DrivetrainSubsystem::ZeroTarget() const {
-  return DrivetrainOLControlTarget{{0_fps, 0_fps}};
+  return DrivetrainOLControlTarget{{0_fps, 0_fps}, 0_deg_per_s};
 }
 
 bool DrivetrainSubsystem::VerifyHardware() {
@@ -47,6 +57,36 @@ bool DrivetrainSubsystem::VerifyHardware() {
   return ok;
 }
 
+void DrivetrainSubsystem::ZeroBearing() {
+  if (!is_initialized()) return;
+
+  constexpr int kMaxAttempts = 5;
+  constexpr int kSleepTimeMs = 500;
+  for (int attempts = 1; attempts <= kMaxAttempts; ++attempts) {
+    Log("Gyro zero attempt {}/{}", attempts, kMaxAttempts);
+    if (navX_.IsConnected() && !navX_.IsCalibrating()) {
+      navX_.ZeroYaw();
+      Log("Zeroed bearing");
+
+      for (SwerveModuleSubsystem* module : modules_) {
+        module->ZeroWithCANcoder();
+      }
+      return;
+    }
+
+    Warn("Attempt to zero failed, sleeping {} ms...", kSleepTimeMs);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(kSleepTimeMs));
+  }
+  Error("Unable to zero after {} attempts", kMaxAttempts);
+}
+
+void DrivetrainSubsystem::SetCANCoderOffsets() {
+  for (SwerveModuleSubsystem* module : modules_) {
+    module->SetCANCoderOffset();
+  }
+}
+
 DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
   units::degree_t bearing = navX_.GetAngle() * 1_deg;
 
@@ -90,26 +130,37 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
 }
 
 void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) {
-  // TODO: finish
   if (DrivetrainOLControlTarget* ol_target =
           std::get_if<DrivetrainOLControlTarget>(&target)) {
+    Graph("target/ol_velocity_x", ol_target->velocity[0]);
+    Graph("target/ol_velocity_y", ol_target->velocity[1]);
+    Graph("target/ol_angular_velocity", ol_target->angular_velocity);
+
     units::degree_t bearing = navX_.GetAngle() * 1_deg;
 
-    /*
-    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.
-    */
+    auto ol_calc_outputs = ol_calculator_.calculate({ol_target->velocity,
+        ol_target->angular_velocity,
+        GetPreferenceValue_unit_type<units::feet_per_second_t>("max_speed")});
 
     for (int i = 0; i < 4; i++) {
-      // modules_[i]->SetTarget(module_target);
-      // modules_[i]->UpdateHardware();
+      modules_[i]->SetSteerGains({GetPreferenceValue_double("steer_gains/kP"),
+          GetPreferenceValue_double("steer_gains/kI"),
+          GetPreferenceValue_double("steer_gains/kD"),
+          GetPreferenceValue_double("steer_gains/kF")});
+
+      SwerveModuleOLControlTarget module_target{
+          .drive = ol_calc_outputs.drive_outputs[i],
+          .steer = ol_calc_outputs.steer_outputs[i] - bearing};
+      modules_[i]->SetTarget(module_target);
     }
   } else if (DrivetrainAccelerationControlTarget* accel_target =
                  std::get_if<DrivetrainAccelerationControlTarget>(&target)) {
     throw std::runtime_error("Acceleration control not yet implemented");
-    // TODO: finish later
+    // TODO: implement acceleration control for drivetrain
   }
+
+  for (int i = 0; i < 4; i++)
+    modules_[i]->UpdateHardware();
 }
 
 }  // namespace frc846::robot::swerve

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

@@ -26,6 +26,11 @@ SwerveModuleSubsystem::SwerveModuleSubsystem(Loggable& parent,
   cancoder_.GetAbsolutePosition().SetUpdateFrequency(20_Hz);
 
   RegisterPreference("cancoder_offset_", 0.0_deg);
+
+  max_speed_ = frc846::control::base::MotorSpecificationPresets::get(
+                   common_config.motor_types)
+                   .free_speed *
+               common_config.drive_reduction;
 }
 
 std::pair<frc846::control::config::MotorConstructionParameters,
@@ -64,7 +69,7 @@ void SwerveModuleSubsystem::Setup() {
 }
 
 SwerveModuleTarget SwerveModuleSubsystem::ZeroTarget() const {
-  return SwerveModuleOLControlTarget{0.0, 0_deg};
+  return SwerveModuleOLControlTarget{0.0_fps, 0_deg};
 }
 
 bool SwerveModuleSubsystem::VerifyHardware() {
@@ -75,7 +80,7 @@ bool SwerveModuleSubsystem::VerifyHardware() {
 }
 
 void SwerveModuleSubsystem::SetCANCoderOffset() {
-  SetCANCoderOffset(GetReadings().steer_pos);
+  SetCANCoderOffset(cancoder_.GetAbsolutePosition().GetValue());
 }
 void SwerveModuleSubsystem::SetCANCoderOffset(units::degree_t offset) {
   SetPreferenceValue("cancoder_offset_", offset);
@@ -135,12 +140,12 @@ void SwerveModuleSubsystem::WriteToHardware(SwerveModuleTarget target) {
 
     Graph("target/cosine_comp", cosine_comp.to<double>());
 
-    drive_helper_.WriteDC(
-        cosine_comp *
-        ol_target->drive);  // Ignore invert_drive, cosine comp should handle it
+    double drive_duty_cycle = ol_target->drive / max_speed_;
+
+    drive_helper_.WriteDC(cosine_comp * drive_duty_cycle);
 
-    if (std::abs(ol_target->drive) > 0.002) {
-      steer_helper_.WritePosition(ol_target->steer);
+    if (std::abs(drive_duty_cycle) > 0.002) {
+      steer_helper_.WritePositionOnController(ol_target->steer);
     }
   } else if (SwerveModuleTorqueControlTarget* torque_target =
                  std::get_if<SwerveModuleTorqueControlTarget>(&target)) {