diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..cc27455 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,15 @@ +# MAXSwerve C++ Template Changelog + +## [2023.1] - 2023-02-03 + +### Added + +- Added a configurable rate limiting system to prevent excessive loads from causing premature wheel failure. + +### Fixed + +- Turning SPARK MAX not using the correct feedback device. + +## [2023.0] - 2023-01-18 + +Initial release of MAXSwerve robot project. \ No newline at end of file diff --git a/README.md b/README.md index 2d00e78..c69fbe7 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,8 @@ -# MAXSwerve C++ Template +# MAXSwerve C++ Template v2023.1 + +See [the online changelog](https://github.com/REVrobotics/MAXSwerve-Cpp-Template/blob/main/CHANGELOG.md) for information about updates to the template that may have been released since you created your project. + +## Description A template project for an FRC swerve drivetrain that uses REV MAXSwerve Modules. diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 27a7d1f..76973e1 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -34,13 +34,14 @@ RobotContainer::RobotContainer() { // Turning is controlled by the X axis of the right stick. m_drive.SetDefaultCommand(frc2::RunCommand( [this] { - m_drive.Drive(-units::meters_per_second_t{frc::ApplyDeadband( - m_driverController.GetLeftY(), 0.06)}, - -units::meters_per_second_t{frc::ApplyDeadband( - m_driverController.GetLeftX(), 0.06)}, - -units::radians_per_second_t{frc::ApplyDeadband( - m_driverController.GetRightX(), 0.06)}, - true); + m_drive.Drive( + -units::meters_per_second_t{frc::ApplyDeadband( + m_driverController.GetLeftY(), OIConstants::kDriveDeadband)}, + -units::meters_per_second_t{frc::ApplyDeadband( + m_driverController.GetLeftX(), OIConstants::kDriveDeadband)}, + -units::radians_per_second_t{frc::ApplyDeadband( + m_driverController.GetRightX(), OIConstants::kDriveDeadband)}, + true, true); }, {&m_drive})); } @@ -95,5 +96,6 @@ frc2::Command* RobotContainer::GetAutonomousCommand() { return new frc2::SequentialCommandGroup( std::move(swerveControllerCommand), frc2::InstantCommand( - [this]() { m_drive.Drive(0_mps, 0_mps, 0_rad_per_s, false); }, {})); + [this]() { m_drive.Drive(0_mps, 0_mps, 0_rad_per_s, false, false); }, + {})); } diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index 0dc0c49..a8dba8b 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -10,6 +10,7 @@ #include #include "Constants.h" +#include "utils/SwerveUtils.h" using namespace DriveConstants; @@ -37,18 +38,80 @@ void DriveSubsystem::Periodic() { void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, - units::radians_per_second_t rot, - bool fieldRelative) { - // Adjust input based on max speed - xSpeed *= kMaxSpeed.value(); - ySpeed *= kMaxSpeed.value(); - rot *= kMaxAngularSpeed.value(); + units::radians_per_second_t rot, bool fieldRelative, + bool rateLimit) { + double xSpeedCommanded; + double ySpeedCommanded; + + if (rateLimit) { + // Convert XY to polar for rate limiting + double inputTranslationDir = atan2(ySpeed.value(), xSpeed.value()); + double inputTranslationMag = + sqrt(pow(xSpeed.value(), 2) + pow(ySpeed.value(), 2)); + + // Calculate the direction slew rate based on an estimate of the lateral + // acceleration + double directionSlewRate; + if (m_currentTranslationMag != 0.0) { + directionSlewRate = + abs(DriveConstants::kDirectionSlewRate / m_currentTranslationMag); + } else { + directionSlewRate = 500.0; // some high number that means the slew rate + // is effectively instantaneous + } + + double currentTime = wpi::Now() * 1e-6; + double elapsedTime = currentTime - m_prevTime; + double angleDif = SwerveUtils::AngleDifference(inputTranslationDir, + m_currentTranslationDir); + if (angleDif < 0.45 * std::numbers::pi) { + m_currentTranslationDir = SwerveUtils::StepTowardsCircular( + m_currentTranslationDir, inputTranslationDir, + directionSlewRate * elapsedTime); + m_currentTranslationMag = m_magLimiter.Calculate(inputTranslationMag); + } else if (angleDif > 0.85 * std::numbers::pi) { + if (m_currentTranslationMag > + 1e-4) { // some small number to avoid floating-point errors with + // equality checking + // keep currentTranslationDir unchanged + m_currentTranslationMag = m_magLimiter.Calculate(0.0); + } else { + m_currentTranslationDir = + SwerveUtils::WrapAngle(m_currentTranslationDir + std::numbers::pi); + m_currentTranslationMag = m_magLimiter.Calculate(inputTranslationMag); + } + } else { + m_currentTranslationDir = SwerveUtils::StepTowardsCircular( + m_currentTranslationDir, inputTranslationDir, + directionSlewRate * elapsedTime); + m_currentTranslationMag = m_magLimiter.Calculate(0.0); + } + m_prevTime = currentTime; + + xSpeedCommanded = m_currentTranslationMag * cos(m_currentTranslationDir); + ySpeedCommanded = m_currentTranslationMag * sin(m_currentTranslationDir); + m_currentRotation = m_rotLimiter.Calculate(rot.value()); + + } else { + xSpeedCommanded = xSpeed.value(); + ySpeedCommanded = ySpeed.value(); + m_currentRotation = rot.value(); + } + + // Convert the commanded speeds into the correct units for the drivetrain + units::meters_per_second_t xSpeedDelivered = + xSpeedCommanded * DriveConstants::kMaxSpeed; + units::meters_per_second_t ySpeedDelivered = + ySpeedCommanded * DriveConstants::kMaxSpeed; + units::radians_per_second_t rotDelivered = + m_currentRotation * DriveConstants::kMaxAngularSpeed; auto states = kDriveKinematics.ToSwerveModuleStates( - fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, - frc::Rotation2d(units::radian_t{m_gyro.GetAngle()})) - : frc::ChassisSpeeds{xSpeed, ySpeed, rot}); + fieldRelative + ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( + xSpeedDelivered, ySpeedDelivered, rotDelivered, + frc::Rotation2d(units::radian_t{m_gyro.GetAngle()})) + : frc::ChassisSpeeds{xSpeedDelivered, ySpeedDelivered, rotDelivered}); kDriveKinematics.DesaturateWheelSpeeds(&states, DriveConstants::kMaxSpeed); diff --git a/src/main/cpp/utils/SwerveUtils.cpp b/src/main/cpp/utils/SwerveUtils.cpp new file mode 100644 index 0000000..ab6623f --- /dev/null +++ b/src/main/cpp/utils/SwerveUtils.cpp @@ -0,0 +1,66 @@ +#include "utils/SwerveUtils.h" + +#include +#include + +double SwerveUtils::StepTowards(double _current, double _target, + double _stepsize) { + if (abs(_current - _target) <= _stepsize) { + return _target; + } else if (_target < _current) { + return _current - _stepsize; + } else { + return _current + _stepsize; + } +} + +double SwerveUtils::StepTowardsCircular(double _current, double _target, + double _stepsize) { + _current = WrapAngle(_current); + _target = WrapAngle(_target); + + double temp = _target - _current; + double stepDirection = temp > 0 ? 1 : temp < 0 ? -1 : 0; // Get sign + double difference = abs(_current - _target); + + if (difference <= _stepsize) { + return _target; + } else if (difference > std::numbers::pi) { // does the system need to wrap + // over eventually? + // handle the special case where you can reach the target in one step while + // also wrapping + if (_current + 2 * std::numbers::pi - _target < _stepsize || + _target + 2 * std::numbers::pi - _current < _stepsize) { + return _target; + } else { + return WrapAngle(_current - + stepDirection * + _stepsize); // this will handle wrapping gracefully + } + + } else { + return _current + stepDirection * _stepsize; + } +} + +double SwerveUtils::AngleDifference(double _angleA, double _angleB) { + double difference = abs(_angleA - _angleB); + return difference > std::numbers::pi ? (2 * std::numbers::pi) - difference + : difference; +} + +double SwerveUtils::WrapAngle(double _angle) { + double twoPi = 2 * std::numbers::pi; + + if (_angle == + twoPi) { // Handle this case separately to avoid floating point errors + // with the floor after the division in the case below + return 0.0; + } else if (_angle > twoPi) { + return _angle - twoPi * floor(_angle / twoPi); + } else if (_angle < 0.0) { + return _angle + twoPi * (floor((-_angle) / twoPi) + 1); + } else { + return _angle; + } +} \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index d3c27a2..b725a0f 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -30,6 +30,10 @@ namespace DriveConstants { constexpr units::meters_per_second_t kMaxSpeed = 4.8_mps; constexpr units::radians_per_second_t kMaxAngularSpeed{2 * std::numbers::pi}; +constexpr double kDirectionSlewRate = 1.2; // radians per second +constexpr double kMagnitudeSlewRate = 1.8; // percent per second (1 = 100%) +constexpr double kRotationalSlewRate = 2.0; // percent per second (1 = 100%) + // Chassis configuration constexpr units::meter_t kTrackWidth = 0.6731_m; // Distance between centers of right and left wheels on robot @@ -133,4 +137,5 @@ extern const frc::TrapezoidProfile::Constraints namespace OIConstants { constexpr int kDriverControllerPort = 0; +constexpr double kDriveDeadband = 0.05; } // namespace OIConstants diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index 556abc8..ae4f6e3 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -5,6 +5,7 @@ #pragma once #include +#include #include #include #include @@ -36,10 +37,11 @@ class DriveSubsystem : public frc2::SubsystemBase { * @param rot Angular rate of the robot. * @param fieldRelative Whether the provided x and y speeds are relative to * the field. + * @param rateLimit Whether to enable rate limiting for smoother control. */ void Drive(units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, units::radians_per_second_t rot, - bool fieldRelative); + bool fieldRelative, bool rateLimit); /** * Sets the wheels into an X formation to prevent movement. @@ -111,6 +113,17 @@ class DriveSubsystem : public frc2::SubsystemBase { // The gyro sensor frc::ADIS16470_IMU m_gyro; + // Slew rate filter variables for controlling lateral acceleration + double m_currentRotation = 0.0; + double m_currentTranslationDir = 0.0; + double m_currentTranslationMag = 0.0; + + frc::SlewRateLimiter m_magLimiter{ + DriveConstants::kMagnitudeSlewRate / 1_s}; + frc::SlewRateLimiter m_rotLimiter{ + DriveConstants::kRotationalSlewRate / 1_s}; + double m_prevTime = wpi::Now() * 1e-6; + // Odometry class for tracking robot pose // 4 defines the number of modules frc::SwerveDriveOdometry<4> m_odometry; diff --git a/src/main/include/utils/SwerveUtils.h b/src/main/include/utils/SwerveUtils.h new file mode 100644 index 0000000..7861b9f --- /dev/null +++ b/src/main/include/utils/SwerveUtils.h @@ -0,0 +1,51 @@ +class SwerveUtils { + public: + /** + * Steps a value towards a target with a specified step size. + * + * @param _current The current or starting value. Can be positive or + * negative. + * @param _target The target value the algorithm will step towards. Can be + * positive or negative. + * @param _stepsize The maximum step size that can be taken. + * @return The new value for {@code _current} after performing the specified + * step towards the specified target. + */ + static double StepTowards(double _current, double _target, double _stepsize); + + /** + * Steps a value (angle) towards a target (angle) taking the shortest path + * with a specified step size. + * + * @param _current The current or starting angle (in radians). Can lie + * outside the 0 to 2*PI range. + * @param _target The target angle (in radians) the algorithm will step + * towards. Can lie outside the 0 to 2*PI range. + * @param _stepsize The maximum step size that can be taken (in radians). + * @return The new angle (in radians) for {@code _current} after performing + * the specified step towards the specified target. This value will always lie + * in the range 0 to 2*PI (exclusive). + */ + static double StepTowardsCircular(double _current, double _target, + double _stepsize); + + /** + * Finds the (unsigned) minimum difference between two angles including + * calculating across 0. + * + * @param _angleA An angle (in radians). + * @param _angleB An angle (in radians). + * @return The (unsigned) minimum difference between the two angles (in + * radians). + */ + static double AngleDifference(double _angleA, double _angleB); + + /** + * Wraps an angle until it lies within the range from 0 to 2*PI (exclusive). + * + * @param _angle The angle (in radians) to wrap. Can be positive or negative + * and can lie multiple wraps outside the output range. + * @return An angle (in radians) from 0 and 2*PI (exclusive). + */ + static double WrapAngle(double _angle); +};