working on porting swerve

src/main/cpp/Robot.cpp

@@ -8,13 +8,18 @@
 #include <frc/DriverStation.h>
 #include <frc2/command/CommandScheduler.h>
 
+#include "constants/SwerveConstants.h"
+
 Robot::Robot() {
   frc::DataLogManager::Start();
   frc::DriverStation::StartDataLog(frc::DataLogManager::GetLog());
+  AddPeriodic([this] { swerve.UpdateOdom(); },
+              1 / consts::swerve::ODOM_UPDATE_RATE, 2_ms);
 }
 
 void Robot::RobotPeriodic() {
   frc2::CommandScheduler::GetInstance().Run();
+  swerve.UpdateSimulation();
 }
 
 void Robot::DisabledInit() {}

src/main/cpp/str/swerve/SwerveDrive.cpp

@@ -4,4 +4,118 @@
 
 #include "str/swerve/SwerveDrive.h"
 
+#include <frc/DataLogManager.h>
+
+#include "constants/SwerveConstants.h"
+#include "ctre/phoenix/StatusCodes.h"
+#include "frc/Alert.h"
+
 using namespace str::swerve;
+
+SwerveDrive::SwerveDrive()
+    : imuConfigAlert{imuConfigAlertStr, frc::Alert::AlertType::kError},
+      imuOptimizeAlert{imuOptimizeAlertStr, frc::Alert::AlertType::kError} {
+  ConfigureImu();
+  SetupSignals();
+}
+
+void SwerveDrive::UpdateOdom() {
+  ctre::phoenix::StatusCode status =
+      ctre::phoenix6::BaseStatusSignal::WaitForAll(
+          1.0 / consts::swerve::ODOM_UPDATE_RATE, allSignals);
+
+  if (!status.IsOK()) {
+    frc::DataLogManager::Log(fmt::format(
+        "Error updating swerve odom! Error was: {}", status.GetName()));
+  }
+
+  int i = 0;
+  for (auto& mod : modules) {
+    modulePositions[i] = mod.GetPosition();
+    moduleStates[i] = mod.GetState();
+    i++;
+  }
+
+  yawLatencyComped =
+      ctre::phoenix6::BaseStatusSignal::GetLatencyCompensatedValue(
+          imu.GetYaw(), imu.GetAngularVelocityZWorld());
+  poseEstimator.Update(frc::Rotation2d{yawLatencyComped}, modulePositions);
+  odom.Update(frc::Rotation2d{yawLatencyComped}, modulePositions);
+
+  units::second_t now = frc::Timer::GetFPGATimestamp();
+  odomUpdateRate = 1.0 / (now - lastOdomUpdateTime);
+  lastOdomUpdateTime = now;
+}
+
+void SwerveDrive::UpdateSimulation() {
+  std::array<frc::SwerveModuleState, 4> simState;
+  int i = 0;
+  for (auto& swerveModule : modules) {
+    simState[i] = swerveModule.UpdateSimulatedModule(
+        frc::RobotController::GetBatteryVoltage());
+    i++;
+  }
+
+  simStatesPub.Set(simState);
+
+  units::radians_per_second_t omega =
+      consts::swerve::KINEMATICS.ToChassisSpeeds(simState).omega;
+  units::radian_t angleChange = omega * (1 / 50_Hz);
+
+  lastSimAngle = lastSimAngle + frc::Rotation2d{angleChange};
+  imuSimState.SetRawYaw(lastSimAngle.Degrees());
+  imuSimState.SetAngularVelocityZ(omega);
+}
+
+void SwerveDrive::SetupSignals() {
+  // If you are changing this you're prob cooked ngl
+  for (size_t i = 0; i < modules.size(); i++) {
+    const auto& modSigs = modules[i].GetSignals();
+    allSignals[(i * 8) + 0] = modSigs[0];
+    allSignals[(i * 8) + 1] = modSigs[1];
+    allSignals[(i * 8) + 2] = modSigs[2];
+    allSignals[(i * 8) + 3] = modSigs[3];
+    allSignals[(i * 8) + 4] = modSigs[4];
+    allSignals[(i * 8) + 5] = modSigs[5];
+    allSignals[(i * 8) + 6] = modSigs[6];
+    allSignals[(i * 8) + 7] = modSigs[7];
+  }
+
+  allSignals[allSignals.size() - 2] = &imu.GetYaw();
+  allSignals[allSignals.size() - 1] = &imu.GetAngularVelocityZWorld();
+
+  for (const auto& sig : allSignals) {
+    sig->SetUpdateFrequency(consts::swerve::ODOM_UPDATE_RATE);
+  }
+
+  for (auto& mod : modules) {
+    mod.OptimizeBusSignals();
+  }
+
+  ctre::phoenix::StatusCode optimizeImuResult = imu.OptimizeBusUtilization();
+
+  frc::DataLogManager::Log(
+      fmt::format("Optimized bus signals for imu. Result was: {}",
+                  optimizeImuResult.GetName()));
+
+  if (!optimizeImuResult.IsOK()) {
+    imuOptimizeAlert.Set(true);
+  }
+}
+
+void SwerveDrive::ConfigureImu() {
+  ctre::phoenix6::configs::Pigeon2Configuration imuConfig;
+  imuConfig.MountPose.MountPoseRoll = consts::swerve::IMU_MOUNT_ROLL;
+  imuConfig.MountPose.MountPosePitch = consts::swerve::IMU_MOUNT_PITCH;
+  imuConfig.MountPose.MountPoseYaw = consts::swerve::IMU_MOUNT_YAW;
+
+  ctre::phoenix::StatusCode imuConfigStatus =
+      imu.GetConfigurator().Apply(imuConfig);
+
+  frc::DataLogManager::Log(
+      fmt::format("Imu Configured. Result was: {}", imuConfigStatus.GetName()));
+
+  if (!imuConfigStatus.IsOK()) {
+    imuConfigAlert.Set(true);
+  }
+}

src/main/cpp/str/swerve/SwerveModule.cpp

@@ -7,6 +7,7 @@
 #include <utility>
 
 #include "ctre/phoenix/StatusCodes.h"
+#include "ctre/phoenix6/StatusSignal.hpp"
 #include "ctre/phoenix6/core/CoreCANcoder.hpp"
 #include "ctre/phoenix6/signals/SpnEnums.hpp"
 #include "frc/Alert.h"
@@ -30,6 +31,7 @@ SwerveModule::SwerveModule(const ModuleConstants& consts,
       configureDriveAlert(driveAlertMsg, frc::Alert::AlertType::kError),
       optimizeSteerMotorAlert(optimizeSteerMsg, frc::Alert::AlertType::kError),
       optimizeDriveMotorAlert(optimizeDriveMsg, frc::Alert::AlertType::kError),
+      physicalChar{physical},
       steerGains{std::move(steer)},
       driveGains{std::move(drive)},
       steerEncoder{consts.encoderId, "*"},
@@ -46,6 +48,59 @@ SwerveModule::SwerveModule(const ModuleConstants& consts,
   ConfigureControlSignals();
 }
 
+void SwerveModule::OptimizeBusSignals() {
+  ctre::phoenix::StatusCode optimizeDriveResult =
+      driveMotor.OptimizeBusUtilization();
+  frc::DataLogManager::Log(
+      fmt::format("Optimized bus signals for {} drive motor. Result was: {}",
+                  moduleNamePrefix, optimizeDriveResult.GetName()));
+  ctre::phoenix::StatusCode optimizeSteerResult =
+      steerMotor.OptimizeBusUtilization();
+  frc::DataLogManager::Log(
+      fmt::format("Optimized bus signals for {} steer motor. Result was {}",
+                  moduleNamePrefix, optimizeSteerResult.GetName()));
+  optimizeDriveMotorAlert.Set(!optimizeDriveResult.IsOK());
+  optimizeSteerMotorAlert.Set(!optimizeSteerResult.IsOK());
+}
+
+std::array<ctre::phoenix6::BaseStatusSignal*, 8> SwerveModule::GetSignals() {
+  return {&drivePositionSig, &driveVelocitySig,      &steerPositionSig,
+          &steerVelocitySig, &driveTorqueCurrentSig, &steerTorqueCurrentSig,
+          &driveVoltageSig,  &steerVoltageSig};
+}
+
+frc::SwerveModulePosition SwerveModule::GetPosition() {
+  units::radian_t latencyCompSteerPos =
+      ctre::phoenix6::BaseStatusSignal::GetLatencyCompensatedValue(
+          steerPositionSig, steerVelocitySig);
+  units::radian_t latencyCompDrivePos =
+      ctre::phoenix6::BaseStatusSignal::GetLatencyCompensatedValue(
+          drivePositionSig, driveVelocitySig);
+
+  latencyCompDrivePos -= latencyCompSteerPos * physicalChar.couplingRatio;
+
+  frc::SwerveModulePosition position{
+      ConvertWheelRotationsToWheelDistance(
+          ConvertDriveMotorRotationsToWheelRotations(latencyCompDrivePos)),
+      frc::Rotation2d{latencyCompSteerPos}};
+
+  return position;
+}
+
+frc::SwerveModuleState SwerveModule::GetState() {
+  frc::SwerveModuleState currentState{
+      ConvertWheelVelToLinearVel(
+          ConvertDriveMotorVelToWheelVel(driveVelocitySig.GetValue())),
+      frc::Rotation2d{steerPositionSig.GetValue()}};
+
+  return currentState;
+}
+
+frc::SwerveModuleState SwerveModule::UpdateSimulatedModule(
+    units::volt_t batteryVoltage) {
+  return moduleSim.Update(batteryVoltage);
+}
+
 void SwerveModule::ConfigureSteerEncoder(units::turn_t encoderOffset) {
   ctre::phoenix6::configs::CANcoderConfiguration encoderConfig{};
 
@@ -167,17 +222,32 @@ void SwerveModule::ConfigureControlSignals() {
   driveVelocitySetter.OverrideCoastDurNeutral = true;
 }
 
-void SwerveModule::OptimizeBusSignals() {
-  ctre::phoenix::StatusCode optimizeDriveResult =
-      driveMotor.OptimizeBusUtilization();
-  frc::DataLogManager::Log(
-      fmt::format("Optimized bus signals for {} drive motor. Result was: {}",
-                  moduleNamePrefix, optimizeDriveResult.GetName()));
-  ctre::phoenix::StatusCode optimizeSteerResult =
-      steerMotor.OptimizeBusUtilization();
-  frc::DataLogManager::Log(
-      fmt::format("Optimized bus signals for {} steer motor. Result was {}",
-                  moduleNamePrefix, optimizeSteerResult.GetName()));
-  optimizeDriveMotorAlert.Set(!optimizeDriveResult.IsOK());
-  optimizeSteerMotorAlert.Set(!optimizeSteerResult.IsOK());
+units::radian_t SwerveModule::ConvertDriveMotorRotationsToWheelRotations(
+    units::radian_t motorRotations) const {
+  return motorRotations / physicalChar.driveGearing;
+}
+
+units::radians_per_second_t SwerveModule::ConvertDriveMotorVelToWheelVel(
+    units::radians_per_second_t motorVel) const {
+  return motorVel / physicalChar.driveGearing;
+}
+
+units::meter_t SwerveModule::ConvertWheelRotationsToWheelDistance(
+    units::radian_t wheelRotations) const {
+  return (wheelRotations / 1_rad) * physicalChar.wheelRadius;
+}
+
+units::meters_per_second_t SwerveModule::ConvertWheelVelToLinearVel(
+    units::radians_per_second_t wheelVel) const {
+  return (wheelVel / 1_rad) * physicalChar.wheelRadius;
+}
+
+units::radians_per_second_t SwerveModule::ConvertLinearVelToWheelVel(
+    units::meters_per_second_t linVel) const {
+  return (linVel / physicalChar.wheelRadius) * 1_rad;
+}
+
+units::radians_per_second_t SwerveModule::ConvertWheelVelToMotorVel(
+    units::radians_per_second_t wheelVel) const {
+  return wheelVel * physicalChar.driveGearing;
 }

src/main/cpp/str/swerve/SwerveModuleSim.cpp

@@ -43,8 +43,7 @@ SwerveModuleSim::SwerveModuleSim(
   steerSim.SetState(0_rad, 0_rad_per_s);
 }
 
-frc::SwerveModuleState SwerveModuleSim::Update(units::second_t deltaTime,
-                                               units::volt_t supplyVoltage) {
+frc::SwerveModuleState SwerveModuleSim::Update(units::volt_t supplyVoltage) {
   driveSimState.Orientation =
       driveInverted
           ? ctre::phoenix6::sim::ChassisReference::Clockwise_Positive
@@ -63,8 +62,8 @@ frc::SwerveModuleState SwerveModuleSim::Update(units::second_t deltaTime,
   steerSim.SetInputVoltage(AddFrictionVoltage(steerSimState.GetMotorVoltage(),
                                               steerFrictionVoltage));
 
-  driveSim.Update(deltaTime);
-  steerSim.Update(deltaTime);
+  driveSim.Update(1 / 50_Hz);
+  steerSim.Update(1 / 50_Hz);
 
   driveSimState.SetRawRotorPosition(driveSim.GetAngularPosition() *
                                     driveGearing);

src/main/include/constants/Constants.h

@@ -9,6 +9,6 @@
 #include "frc/apriltag/AprilTagFields.h"
 
 namespace consts::yearspecific {
-inline const frc::AprilTagFieldLayout tagLayout =
+inline const frc::AprilTagFieldLayout TAG_LAYOUT =
     frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo);
 }  // namespace consts::yearspecific

src/main/include/constants/SwerveConstants.h

@@ -4,20 +4,33 @@
 
 #pragma once
 
+#include <units/frequency.h>
+
+#include "frc/geometry/Translation2d.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/system/plant/DCMotor.h"
 #include "str/swerve/SwerveModuleHelpers.h"
+#include "units/angle.h"
 
 namespace consts::swerve {
-inline const str::swerve::ModuleConstants flModule{"FL",         2,     3,   4,
-                                                   -0.272949_tr, false, true};
-inline const str::swerve::ModuleConstants frModule{"FR",        5,    6,   7,
-                                                   0.356201_tr, true, true};
-inline const str::swerve::ModuleConstants blModule{"BL",        8,     9,   10,
-                                                   0.195068_tr, false, true};
-inline const str::swerve::ModuleConstants brModule{"BR",         11,   12,  13,
-                                                   -0.492676_tr, true, true};
-
-inline const str::swerve::ModulePhysicalCharacteristics physicalCharacteristics{
+
+inline constexpr units::hertz_t ODOM_UPDATE_RATE = 250_Hz;
+
+inline constexpr int IMU_ID = 14;
+inline constexpr units::degree_t IMU_MOUNT_ROLL = 0_deg;
+inline constexpr units::degree_t IMU_MOUNT_PITCH = 0_deg;
+inline constexpr units::degree_t IMU_MOUNT_YAW = 0_deg;
+
+inline const str::swerve::ModuleConstants FL_MODULE{"FL",         2,     3,   4,
+                                                    -0.272949_tr, false, true};
+inline const str::swerve::ModuleConstants FR_MODULE{"FR",        5,    6,   7,
+                                                    0.356201_tr, true, true};
+inline const str::swerve::ModuleConstants BL_MODULE{"BL",        8,     9,   10,
+                                                    0.195068_tr, false, true};
+inline const str::swerve::ModuleConstants BR_MODULE{"BR",         11,   12,  13,
+                                                    -0.492676_tr, true, true};
+
+inline const str::swerve::ModulePhysicalCharacteristics PHY_CHAR{
     (50.0 / 14.0) * (60.0 / 10.0),
     (50.0 / 16.0) * (16.0 / 28.0) * (45.0 / 15.0),
     40_A,
@@ -29,12 +42,11 @@ inline const str::swerve::ModulePhysicalCharacteristics physicalCharacteristics{
     (50.0 / 16.0),
     1.9154_in};
 
-inline const str::swerve::SteerGains steerGains{
-    physicalCharacteristics.steerMotor.freeSpeed /
-        physicalCharacteristics.steerGearing,
+inline const str::swerve::SteerGains STEER_GAINS{
+    PHY_CHAR.steerMotor.freeSpeed / PHY_CHAR.steerGearing,
     str::gains::radial::turn_volt_ka_unit_t{.1},
-    str::gains::radial::turn_volt_kv_unit_t{
-        .12 * physicalCharacteristics.steerGearing.value()},
+    str::gains::radial::turn_volt_kv_unit_t{.12 *
+                                            PHY_CHAR.steerGearing.value()},
     str::gains::radial::turn_amp_ka_unit_t{.82395},
     str::gains::radial::turn_amp_kv_unit_t{2.40},
     4.7145_A,
@@ -43,12 +55,25 @@ inline const str::swerve::SteerGains steerGains{
     str::gains::radial::turn_amp_kd_unit_t{39.663},
 };
 
-inline const str::swerve::DriveGains driveGains{
+inline const str::swerve::DriveGains DRIVE_GAINS{
     str::gains::radial::turn_amp_ka_unit_t{0},
     str::gains::radial::turn_amp_kv_unit_t{0},
     9_A,
     str::gains::radial::turn_amp_kp_unit_t{9},
     str::gains::radial::turn_amp_ki_unit_t{0},
     str::gains::radial::turn_amp_kd_unit_t{0},
 };
+
+inline constexpr units::meter_t WHEELBASE_WIDTH = 21.75_in;
+inline constexpr units::meter_t WHEELBASE_LENGTH = 15.75_in;
+
+inline constexpr std::array<frc::Translation2d, 4> MODULE_LOCATIONS{
+    frc::Translation2d{WHEELBASE_LENGTH / 2, WHEELBASE_WIDTH / 2},
+    frc::Translation2d{WHEELBASE_LENGTH / 2, -WHEELBASE_WIDTH / 2},
+    frc::Translation2d{-WHEELBASE_LENGTH / 2, WHEELBASE_WIDTH / 2},
+    frc::Translation2d{-WHEELBASE_LENGTH / 2, -WHEELBASE_WIDTH / 2}};
+
+inline frc::SwerveDriveKinematics<4> KINEMATICS{
+    MODULE_LOCATIONS[0], MODULE_LOCATIONS[1], MODULE_LOCATIONS[2],
+    MODULE_LOCATIONS[3]};
 }  // namespace consts::swerve