Swerve gains struct

src/main/cpp/str/SwerveDrive.cpp

@@ -6,6 +6,7 @@
 
 #include "constants/Constants.h"
 #include "str/Math.h"
+#include <frc/DataLogManager.h>
 
 using namespace str;
 
@@ -34,7 +35,7 @@ SwerveDrive::SwerveDrive() {
           .value();
 
   if (!imu.GetConfigurator().Apply(imuConfig).IsOK()) {
-    fmt::print("Failed to configure IMU!\n");
+    frc::DataLogManager::Log("Failed to configure IMU!\n");
   }
 
   allSignals[allSignals.size() - 2] = &imu.GetYaw();
@@ -46,12 +47,12 @@ SwerveDrive::SwerveDrive() {
 
   for (auto &mod : modules) {
     if (!mod.OptimizeBusSignals()) {
-      fmt::print("Failed to optimize bus signals for {}\n", mod.GetName());
+      frc::DataLogManager::Log(fmt::format("Failed to optimize bus signals for {}\n", mod.GetName()));
     }
   }
 
   if (!imu.OptimizeBusUtilization().IsOK()) {
-    fmt::print("Failed to optimize bus signals for imu!\n");
+    frc::DataLogManager::Log("Failed to optimize bus signals for imu!\n");
   }
 }
 
@@ -145,7 +146,7 @@ void SwerveDrive::AddVisionMeasurement(const frc::Pose2d &visionMeasurement,
     poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp,
                                        newStdDevs);
   } else {
-    fmt::print(
+    frc::DataLogManager::Log(
         "WARNING: Vision pose was outside of field! Not adding to estimator!");
   }
 }
@@ -156,8 +157,8 @@ void SwerveDrive::UpdateSwerveOdom() {
           2.0 / (1 / consts::SWERVE_ODOM_LOOP_PERIOD), allSignals);
 
   // if(!status.IsOK()) {
-  //   fmt::print("Error updating swerve odom! Error was: {}\n",
-  //   status.GetName());
+  //   frc::DataLogManager::Log(fmt::format("Error updating swerve odom! Error was: {}\n",
+  //   status.GetName()));
   // }
 
   int i = 0;
@@ -181,6 +182,7 @@ void SwerveDrive::UpdateNTEntries() {
   lookaheadPub.Set(GetPredictedPose(1_s, 1_s));
   estimatorPub.Set(GetPose());
   isSlippingPub.Set(IsSlipping());
+  steerGainsEntry.Set(steerGainsMk4i);
 }
 
 void SwerveDrive::SimulationPeriodic() {

src/main/cpp/str/SwerveModule.cpp

@@ -5,6 +5,7 @@
 #include "str/SwerveModule.h"
 
 #include <frc/MathUtil.h>
+#include <frc/DataLogManager.h>
 
 using namespace str;
 
@@ -26,15 +27,15 @@ SwerveModule::SwerveModule(SwerveModuleConstants constants,
   if (!ConfigureSteerMotor(constants.invertSteer, physicalAttrib.steerGearing,
                            physicalAttrib.steerTorqueCurrentLimit,
                            physicalAttrib.steerTorqueCurrentLimit)) {
-    fmt::print("ERROR: Failed to configure steer motor!");
+    frc::DataLogManager::Log("ERROR: Failed to configure steer motor!");
   }
   if (!ConfigureDriveMotor(constants.invertDrive,
                            physicalAttrib.driveSupplySideLimit,
                            physicalAttrib.slipCurrent)) {
-    fmt::print("ERROR: Failed to configure drive motor!");
+    frc::DataLogManager::Log("ERROR: Failed to configure drive motor!");
   }
   if (!ConfigureSteerEncoder(constants.steerEncoderOffset)) {
-    fmt::print("ERROR: Failed to configure steer encoder!");
+    frc::DataLogManager::Log("ERROR: Failed to configure steer encoder!");
   }
   ConfigureControlSignals();
 }
@@ -96,9 +97,9 @@ frc::SwerveModulePosition SwerveModule::GetCurrentPosition(bool refresh) {
             steerPositionSig, steerVelocitySig, steerVoltageSig);
 
     if (!moduleSignalStatus.IsOK()) {
-      fmt::print("Error refreshing {} module signal in GetCurrentPosition()! "
+      frc::DataLogManager::Log(fmt::format("Error refreshing {} module signal in GetCurrentPosition()! "
                  "Error was: {}\n",
-                 moduleName, moduleSignalStatus.GetName());
+                 moduleName, moduleSignalStatus.GetName()));
     }
   }
 
@@ -136,9 +137,9 @@ units::radian_t SwerveModule::GetOutputShaftTurns() {
                                                    driveVelocitySig);
 
   if (!moduleSignalStatus.IsOK()) {
-    fmt::print("Error refreshing {} module signal in GetDriveMotorTurns()! "
+    frc::DataLogManager::Log(fmt::format("Error refreshing {} module signal in GetDriveMotorTurns()! "
                "Error was: {}\n",
-               moduleName, moduleSignalStatus.GetName());
+               moduleName, moduleSignalStatus.GetName()));
   }
 
   units::radian_t latencyCompDrivePos =
@@ -211,8 +212,8 @@ bool SwerveModule::ConfigureSteerMotor(bool invertSteer,
   ctre::phoenix::StatusCode configResult =
       steerMotor.GetConfigurator().Apply(steerConfig);
 
-  fmt::print("Configured steer motor on module {}. Result was: {}\n",
-             moduleName, configResult.GetName());
+  frc::DataLogManager::Log(fmt::format("Configured steer motor on module {}. Result was: {}\n",
+             moduleName, configResult.GetName()));
 
   return configResult.IsOK();
 }
@@ -252,8 +253,8 @@ bool SwerveModule::ConfigureDriveMotor(bool invertDrive,
   ctre::phoenix::StatusCode configResult =
       driveMotor.GetConfigurator().Apply(driveConfig);
 
-  fmt::print("Configured drive motor on module {}. Result was: {}\n",
-             moduleName, configResult.GetName());
+  frc::DataLogManager::Log(fmt::format("Configured drive motor on module {}. Result was: {}\n",
+             moduleName, configResult.GetName()));
 
   return configResult.IsOK();
 }
@@ -268,8 +269,8 @@ bool SwerveModule::ConfigureSteerEncoder(units::turn_t encoderOffset) {
   ctre::phoenix::StatusCode configResult =
       steerEncoder.GetConfigurator().Apply(encoderConfig);
 
-  fmt::print("Configured steer encoder on module {}. Result was: {}\n",
-             moduleName, configResult.GetName());
+  frc::DataLogManager::Log(fmt::format("Configured steer encoder on module {}. Result was: {}\n",
+             moduleName, configResult.GetName()));
 
   return configResult.IsOK();
 }
@@ -292,12 +293,12 @@ bool SwerveModule::OptimizeBusSignals() {
   ctre::phoenix::StatusCode optimizeDriveResult =
       driveMotor.OptimizeBusUtilization();
   if (optimizeDriveResult.IsOK()) {
-    fmt::print("Optimized bus signals for {} drive motor\n", moduleName);
+    frc::DataLogManager::Log(fmt::format("Optimized bus signals for {} drive motor\n", moduleName));
   }
   ctre::phoenix::StatusCode optimizeSteerResult =
       steerMotor.OptimizeBusUtilization();
   if (optimizeSteerResult.IsOK()) {
-    fmt::print("Optimized bus signals for {} steer motor\n", moduleName);
+    frc::DataLogManager::Log(fmt::format("Optimized bus signals for {} steer motor\n", moduleName));
   }
   return optimizeDriveResult.IsOK() && optimizeSteerResult.IsOK();
 }
@@ -308,6 +309,44 @@ units::ampere_t SwerveModule::GetSimulatedCurrentDraw() const {
   return moduleSim.GetDriveCurrentDraw() + moduleSim.GetSteerCurrentDraw();
 }
 
+void SwerveModule::SetSteerGains(str::SwerveModuleSteerGains newGains) {
+  steerGains = newGains;
+  ctre::phoenix6::configs::Slot0Configs steerSlotConfig{};
+  steerSlotConfig.kV = steerGains.kV.value();
+  steerSlotConfig.kA = steerGains.kA.value();
+  steerSlotConfig.kS = steerGains.kS.value();
+  steerSlotConfig.kP = steerGains.kP.value();
+  steerSlotConfig.kI = steerGains.kI.value();
+  steerSlotConfig.kD = steerGains.kD.value();
+  ctre::phoenix::StatusCode status =
+      steerMotor.GetConfigurator().Apply(steerSlotConfig);
+  if (!status.IsOK()) {
+    frc::DataLogManager::Log(
+        fmt::format("Swerve Steer Motor was unable to set new gains! "
+                    "Error: {}, More Info: {}",
+                    status.GetName(), status.GetDescription()));
+  }
+}
+
+void SwerveModule::SetDriveGains(str::SwerveModuleDriveGains newGains) {
+  driveGains = newGains;
+  ctre::phoenix6::configs::Slot0Configs driveSlotConfig{};
+  driveSlotConfig.kV = driveGains.kV.value();
+  driveSlotConfig.kA = driveGains.kA.value();
+  driveSlotConfig.kS = driveGains.kS.value();
+  driveSlotConfig.kP = driveGains.kP.value();
+  driveSlotConfig.kI = driveGains.kI.value();
+  driveSlotConfig.kD = driveGains.kD.value();
+  ctre::phoenix::StatusCode status =
+      driveMotor.GetConfigurator().Apply(driveSlotConfig);
+  if (!status.IsOK()) {
+    frc::DataLogManager::Log(
+        fmt::format("Swerve Drive Motor was unable to set new gains! "
+                    "Error: {}, More Info: {}",
+                    status.GetName(), status.GetDescription()));
+  }
+}
+
 void SwerveModule::SetSteerToTorque(units::volt_t voltsToSend) {
   steerMotor.SetControl(
       steerTorqueSetter.WithOutput(units::ampere_t{voltsToSend.value()}));

src/main/cpp/str/struct/SwerveHelperStruct.cpp

@@ -0,0 +1,42 @@
+#include "str/struct/SwerveHelperStruct.h"
+
+namespace {
+constexpr size_t start = 0;
+constexpr size_t one = start + 8;
+constexpr size_t two = one + 8;
+constexpr size_t three = two + 8;
+constexpr size_t four = three + 8;
+constexpr size_t five = four + 8;
+constexpr size_t six = five + 8;
+constexpr size_t seven = six + 8;
+constexpr size_t eight = seven + 8;
+}  // namespace
+
+using SteerStructType = wpi::Struct<str::SwerveModuleSteerGains>;
+
+str::SwerveModuleSteerGains SteerStructType::Unpack(std::span<const uint8_t> data) {
+  return str::SwerveModuleSteerGains{
+      units::turns_per_second_t{wpi::UnpackStruct<double, start>(data)},
+      str::gains::radial::turn_volt_ka_unit_t{wpi::UnpackStruct<double, one>(data)},
+      str::gains::radial::turn_volt_kv_unit_t{wpi::UnpackStruct<double, two>(data)},
+      str::gains::radial::turn_amp_ka_unit_t{wpi::UnpackStruct<double, three>(data)},
+      str::gains::radial::turn_amp_kv_unit_t{wpi::UnpackStruct<double, four>(data)},
+      units::ampere_t{wpi::UnpackStruct<double, five>(data)},
+      str::gains::radial::turn_amp_kp_unit_t{wpi::UnpackStruct<double, six>(data)},
+      str::gains::radial::turn_amp_ki_unit_t{wpi::UnpackStruct<double, seven>(data)},
+      str::gains::radial::turn_amp_kd_unit_t{wpi::UnpackStruct<double, eight>(data)},
+  };
+}
+
+void SteerStructType::Pack(std::span<uint8_t> data,
+                      const str::SwerveModuleSteerGains& value) {
+  wpi::PackStruct<start>(data, value.motionMagicCruiseVel.value());
+  wpi::PackStruct<one>(data, value.motionMagicExpoKa.value());
+  wpi::PackStruct<two>(data, value.motionMagicExpoKv.value());
+  wpi::PackStruct<three>(data, value.kA.value());
+  wpi::PackStruct<four>(data, value.kV.value());
+  wpi::PackStruct<five>(data, value.kS.value());
+  wpi::PackStruct<six>(data, value.kP.value());
+  wpi::PackStruct<seven>(data, value.kI.value());
+  wpi::PackStruct<eight>(data, value.kD.value());
+}

src/main/cpp/subsystems/IntakeSubsystem.cpp

@@ -6,6 +6,7 @@
 #include <frc/smartdashboard/SmartDashboard.h>
 #include "constants/Constants.h"
 #include <frc2/command/Commands.h>
+#include <frc/DataLogManager.h>
 
 IntakeSubsystem::IntakeSubsystem() {
     SetName("IntakeSubsystem");
@@ -88,8 +89,8 @@ bool IntakeSubsystem::ConfigureIntakeMotor(bool invert,
   ctre::phoenix::StatusCode intakeConfigResult =
       intakeMotor.GetConfigurator().Apply(intakeConfig);
 
-  fmt::print("Configured intake motor. Result was: {}\n",
-             intakeConfigResult.GetName());
+  frc::DataLogManager::Log(fmt::format("Configured intake motor. Result was: {}\n",
+             intakeConfigResult.GetName()));
 
   return intakeConfigResult.IsOK();
 }
@@ -107,7 +108,7 @@ bool IntakeSubsystem::ConfigureMotorSignals() {
   ctre::phoenix::StatusCode optimizeIntakeMotor =
       intakeMotor.OptimizeBusUtilization();
   if (optimizeIntakeMotor.IsOK()) {
-    fmt::print("Optimized bus signals for intake motor\n");
+    frc::DataLogManager::Log("Optimized bus signals for intake motor\n");
   }
 
   return optimizeIntakeMotor.IsOK();

src/main/cpp/subsystems/ShooterSubsystem.cpp

@@ -6,6 +6,7 @@
 #include "constants/Constants.h"
 #include <frc2/command/Commands.h>
 #include <frc/smartdashboard/SmartDashboard.h>
+#include <frc/DataLogManager.h>
 
 
 ShooterSubsystem::ShooterSubsystem() {
@@ -50,7 +51,7 @@ void ShooterSubsystem::Periodic() {
     });
 
     if(!shooterWaitResult.IsOK()) {
-        fmt::print("Error grabbing shooter signals! Details: {}\n", shooterWaitResult.GetName());
+        frc::DataLogManager::Log(fmt::format("Error grabbing shooter signals! Details: {}\n", shooterWaitResult.GetName()));
     }
 
     currentTopWheelPosition = ctre::phoenix6::BaseStatusSignal::GetLatencyCompensatedValue(topMotorPosSig, topMotorVelSig);
@@ -129,8 +130,8 @@ bool ShooterSubsystem::ConfigureShooterMotors(
   ctre::phoenix::StatusCode topConfigResult =
       topWheelMotor.GetConfigurator().Apply(shooterConfig);
 
-  fmt::print("Configured top shooter motor. Result was: {}\n",
-             topConfigResult.GetName());
+  frc::DataLogManager::Log(fmt::format("Configured top shooter motor. Result was: {}\n",
+             topConfigResult.GetName()));
 
   shooterConfig.MotorOutput.Inverted =
       invertBottom
@@ -140,8 +141,8 @@ bool ShooterSubsystem::ConfigureShooterMotors(
   ctre::phoenix::StatusCode bottomConfigResult =
       bottomWheelMotor.GetConfigurator().Apply(shooterConfig);
 
-  fmt::print("Configured bottom shooter motor. Result was: {}\n",
-             bottomConfigResult.GetName());
+  frc::DataLogManager::Log(fmt::format("Configured bottom shooter motor. Result was: {}\n",
+             bottomConfigResult.GetName()));
 
   return bottomConfigResult.IsOK() && topConfigResult.IsOK();
 }
@@ -164,13 +165,13 @@ bool ShooterSubsystem::ConfigureMotorSignals() {
   ctre::phoenix::StatusCode optimizeTopMotor =
       topWheelMotor.OptimizeBusUtilization();
   if (optimizeTopMotor.IsOK()) {
-    fmt::print("Optimized bus signals for top shooter motor\n");
+    frc::DataLogManager::Log("Optimized bus signals for top shooter motor\n");
   }
 
   ctre::phoenix::StatusCode optimizeBottomMotor =
       bottomWheelMotor.OptimizeBusUtilization();
   if (optimizeBottomMotor.IsOK()) {
-    fmt::print("Optimized bus signals for bottom shooter motor\n");
+    frc::DataLogManager::Log("Optimized bus signals for bottom shooter motor\n");
   }
 
   return optimizeTopMotor.IsOK() && optimizeBottomMotor.IsOK();

src/main/cpp/subsystems/SwerveSubsystem.cpp

@@ -11,6 +11,7 @@
 #include <pathplanner/lib/auto/AutoBuilder.h>
 #include <str/ChoreoSwerveCommandWithForce.h>
 #include <str/DriverstationUtils.h>
+#include <frc/DataLogManager.h>
 
 #include "constants/Constants.h"
 
@@ -149,7 +150,7 @@ void SwerveSubsystem::LoadChoreoTrajectories() {
     std::string fileName = entry.path().stem().string();
     if(fileName != "choreo") {
       pathMap[fileName] = choreolib::Choreo::GetTrajectory(fileName);
-      fmt::print("Loaded choreo trajectory: {}\n", fileName);
+      frc::DataLogManager::Log(fmt::format("Loaded choreo trajectory: {}\n", fileName));
     }
   }
 }
@@ -427,11 +428,19 @@ frc2::CommandPtr SwerveSubsystem::WheelRadius(frc2::sysid::Direction dir) {
                  },
                  [this] {
                    swerveDrive.Drive(0_mps, 0_mps, 0_rad_per_s, true);
-                   fmt::print("WHEEL RADIUS: {}\n\n\n\n\n",
+                   frc::DataLogManager::Log(fmt::format("WHEEL RADIUS: {}\n\n\n\n\n",
                               wheelRadData.effectiveWheelRadius
                                   .convert<units::inches>()
-                                  .value());
+                                  .value()));
                  },
                  {this}))
       .WithName("Wheel Radius Calculation");
 }
+
+frc2::CommandPtr SwerveSubsystem::TuneSteerPID(std::function<bool()> isDone) {
+  return frc2::cmd::Sequence(
+    frc2::cmd::RunOnce([this] {
+
+    })
+  );
+}

src/main/deploy/commit.txt

@@ -1 +1 @@
-3cae4b3
+fdc9431

src/main/include/str/SwerveDrive.h

@@ -11,6 +11,7 @@
 #include <networktables/DoubleTopic.h>
 #include <networktables/StructArrayTopic.h>
 #include <networktables/StructTopic.h>
+#include "str/struct/SwerveHelperStruct.h"
 
 #include <memory>
 #include <string>
@@ -175,5 +176,7 @@ class SwerveDrive {
       nt->GetStructTopic<frc::Pose2d>("PoseEstimatorPose").Publish()};
   nt::StructPublisher<frc::Pose2d> lookaheadPub{
       nt->GetStructTopic<frc::Pose2d>("LookaheadPose").Publish()};
+  nt::StructEntry<str::SwerveModuleSteerGains> steerGainsEntry{
+      nt->GetStructTopic<str::SwerveModuleSteerGains>("SteerGains").GetEntry(steerGainsMk4i)};
 };
 } // namespace str