added stop to swerve drive

frc2053 · Oct 20, 2024 · 6355b9e · 6355b9e
1 parent b397bca
commit 6355b9e
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 3 deletions.
diff --git a/src/main/cpp/subsystems/SwerveSubsystem.cpp b/src/main/cpp/subsystems/SwerveSubsystem.cpp
@@ -119,9 +119,7 @@ void SwerveSubsystem::SetupPathplanner() {
         swerveDrive.SetModuleStates(
             consts::swerve::physical::KINEMATICS.ToSwerveModuleStates(
                 speedsToSend),
-            true, false,
-            {ff[0].torqueCurrent, ff[1].torqueCurrent, ff[2].torqueCurrent,
-             ff[3].torqueCurrent});
+            true, false);
       },
       ppControllers, consts::swerve::pathplanning::config,
       []() { return str::IsOnRed(); }, this);
@@ -143,6 +141,12 @@ frc::Translation2d SwerveSubsystem::GetFrontAmpLocation() {
   return ampToGoTo;
 }
 
+frc2::CommandPtr SwerveSubsystem::Stop() {
+  return frc2::cmd::RunOnce([this] {
+    swerveDrive.Drive(0_fps, 0_fps, 0_rad_per_s, false, false);
+  }, {this});
+}
+
 bool SwerveSubsystem::IsNearAmp() {
   return GetRobotPose().Translation().Distance(GetAmpLocation()) <
          consts::yearSpecific::closeToAmpDistance;

diff --git a/src/main/include/subsystems/SwerveSubsystem.h b/src/main/include/subsystems/SwerveSubsystem.h
@@ -85,6 +85,8 @@ class SwerveSubsystem : public frc2::SubsystemBase {
     return autoFactory;
   }
 
+  frc2::CommandPtr Stop();
+
  private:
   void SetupPathplanner();
   frc::Translation2d GetAmpLocation();