diff --git a/src/main/cpp/subsystems/SwerveSubsystem.cpp b/src/main/cpp/subsystems/SwerveSubsystem.cpp index 5e69968..ec07fb3 100644 --- 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 index e374e0a..c094a7c 100644 --- 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();