added ability to change steer gains from dashboard

frc2053 · Sep 5, 2024 · 4b09746 · 4b09746
1 parent 44febdd
commit 4b09746
Show file tree

Hide file tree

Showing 9 changed files with 74 additions and 80 deletions.
diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp
@@ -49,6 +49,8 @@ void RobotContainer::ConfigureBindings() {
   // controller.X().WhileTrue(swerveSubsystem.SysIdDriveDynamicTorque(frc2::sysid::Direction::kForward));
   // controller.Y().WhileTrue(swerveSubsystem.SysIdDriveDynamicTorque(frc2::sysid::Direction::kReverse));
 
+  controller.POVDown().OnTrue(swerveSubsystem.TuneSteerPID([this] { return controller.Start().Get(); }));
+
   controller.A().WhileTrue(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::AMP; }));
   controller.A().OnFalse(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::OFF; }));
 

diff --git a/src/main/cpp/str/SwerveDrive.cpp b/src/main/cpp/str/SwerveDrive.cpp
@@ -182,7 +182,6 @@ void SwerveDrive::UpdateNTEntries() {
   lookaheadPub.Set(GetPredictedPose(1_s, 1_s));
   estimatorPub.Set(GetPose());
   isSlippingPub.Set(IsSlipping());
-  steerGainsEntry.Set(steerGainsMk4i);
 }
 
 void SwerveDrive::SimulationPeriodic() {
@@ -337,3 +336,13 @@ bool SwerveDrive::IsSlipping() {
       });
   return (maxIt->speed / minIt->speed) > slipCoeff;
 }
+
+str::SwerveModuleSteerGains SwerveDrive::GetSteerGains() const {
+  return modules[0].GetSteerGains();
+}
+
+void SwerveDrive::SetSteerGains(str::SwerveModuleSteerGains newGains) {
+  for(int i = 0; i < 4; i++) {
+    modules[i].SetSteerGains(newGains);
+  }
+}
diff --git a/src/main/cpp/str/SwerveModule.cpp b/src/main/cpp/str/SwerveModule.cpp
@@ -328,6 +328,10 @@ void SwerveModule::SetSteerGains(str::SwerveModuleSteerGains newGains) {
   }
 }
 
+str::SwerveModuleSteerGains SwerveModule::GetSteerGains() const {
+  return steerGains;
+}
+
 void SwerveModule::SetDriveGains(str::SwerveModuleDriveGains newGains) {
   driveGains = newGains;
   ctre::phoenix6::configs::Slot0Configs driveSlotConfig{};

diff --git a/src/main/cpp/str/struct/SwerveHelperStruct.cpp b/src/main/cpp/str/struct/SwerveHelperStruct.cpp
diff --git a/src/main/cpp/subsystems/SwerveSubsystem.cpp b/src/main/cpp/subsystems/SwerveSubsystem.cpp
@@ -438,9 +438,58 @@ frc2::CommandPtr SwerveSubsystem::WheelRadius(frc2::sysid::Direction dir) {
 }
 
 frc2::CommandPtr SwerveSubsystem::TuneSteerPID(std::function<bool()> isDone) {
+  std::string tablePrefix = "SwerveDrive/steerGains/";
   return frc2::cmd::Sequence(
-    frc2::cmd::RunOnce([this] {
-
-    })
-  );
+      frc2::cmd::RunOnce(
+          [tablePrefix, this] {
+            frc::SmartDashboard::PutNumber(tablePrefix + "setpoint", 0);
+            frc::SmartDashboard::PutNumber(
+                tablePrefix + "mmCruiseVel", consts::swerve::gains::MK4I_STEER_CRUISE_VEL.value());
+            frc::SmartDashboard::PutNumber(
+                tablePrefix + "mmKA", consts::swerve::gains::MK4I_STEER_MOTION_MAGIC_KA.value());
+            frc::SmartDashboard::PutNumber(
+                tablePrefix + "mmKV", consts::swerve::gains::MK4I_STEER_MOTION_MAGIC_KV.value());
+            frc::SmartDashboard::PutNumber(
+                tablePrefix + "kA", consts::swerve::gains::MK4I_STEER_KA.value());
+            frc::SmartDashboard::PutNumber(
+                tablePrefix + "kV", consts::swerve::gains::MK4I_STEER_KV.value());
+            frc::SmartDashboard::PutNumber(
+                tablePrefix + "kS", consts::swerve::gains::MK4I_STEER_KS.value());
+            frc::SmartDashboard::PutNumber(
+                tablePrefix + "kP", consts::swerve::gains::MK4I_STEER_KP.value());
+            frc::SmartDashboard::PutNumber(
+                tablePrefix + "kI", consts::swerve::gains::MK4I_STEER_KI.value());
+            frc::SmartDashboard::PutNumber(
+                tablePrefix + "kD", consts::swerve::gains::MK4I_STEER_KD.value());
+            frc::SwerveModuleState zeroState{0_mps, frc::Rotation2d{0_rad}};
+            swerveDrive.SetModuleStates({zeroState,zeroState,zeroState,zeroState});
+          },
+          {this}),
+      frc2::cmd::Run(
+          [this, tablePrefix] {
+            str::SwerveModuleSteerGains newGains{
+                units::turns_per_second_t{frc::SmartDashboard::GetNumber(tablePrefix + "mmCruiseVel", 0)},
+                str::gains::radial::turn_volt_ka_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "mmKA", 0)},
+                str::gains::radial::turn_volt_kv_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "mmKV", 0)},
+                str::gains::radial::turn_amp_ka_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "kA", 0)},
+                str::gains::radial::turn_amp_kv_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "kV", 0)},
+                units::ampere_t{frc::SmartDashboard::GetNumber(tablePrefix + "kS", 0)},
+                str::gains::radial::turn_amp_kp_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "kP", 0)},
+                str::gains::radial::turn_amp_ki_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "kI", 0)},
+                str::gains::radial::turn_amp_kd_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "kD", 0)}
+            };
+
+            if (newGains != swerveDrive.GetSteerGains()) {
+              for (int i = 0; i < 4; i++) {
+                swerveDrive.SetSteerGains(newGains);
+              }
+            }
+
+            for (int i = 0; i < 4; i++) {
+              frc::SwerveModuleState state{0_mps, frc::Rotation2d{units::degree_t{frc::SmartDashboard::GetNumber(tablePrefix + "setpoint", 0)}}};
+              swerveDrive.SetModuleStates({state, state, state, state});
+            }
+          },
+          {this})
+          .Until(isDone));
 }
diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt
@@ -1 +1 @@
-fdc9431
+44febdd
diff --git a/src/main/include/str/SwerveDrive.h b/src/main/include/str/SwerveDrive.h
@@ -11,7 +11,6 @@
 #include <networktables/DoubleTopic.h>
 #include <networktables/StructArrayTopic.h>
 #include <networktables/StructTopic.h>
-#include "str/struct/SwerveHelperStruct.h"
 
 #include <memory>
 #include <string>
@@ -63,6 +62,9 @@ class SwerveDrive {
   void LogMk4nSteerVoltage(frc::sysid::SysIdRoutineLog *log);
   void LogDriveVoltage(frc::sysid::SysIdRoutineLog *log);
 
+  str::SwerveModuleSteerGains GetSteerGains() const;
+  void SetSteerGains(str::SwerveModuleSteerGains newGains);
+
 private:
   std::array<units::ampere_t, 4> ConvertModuleForcesToTorqueCurrent(
       const std::array<units::newton_t, 4> &xForce,
@@ -176,7 +178,5 @@ 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
diff --git a/src/main/include/str/SwerveModule.h b/src/main/include/str/SwerveModule.h
@@ -47,6 +47,7 @@ class SwerveModule {
   void SetDriveToVoltage(units::volt_t voltsToSend);
   void SetSteerGains(str::SwerveModuleSteerGains newGains);
   void SetDriveGains(str::SwerveModuleDriveGains newGains);
+  str::SwerveModuleSteerGains GetSteerGains() const;
 
 private:
   bool ConfigureSteerMotor(bool invertSteer, units::scalar_t steerGearing,

diff --git a/src/main/include/str/struct/SwerveHelperStruct.h b/src/main/include/str/struct/SwerveHelperStruct.h