SwerveSetpointGenerator.h

#include "pathplanner/lib/config/RobotConfig.h"
#include "pathplanner/lib/util/DriveFeedforwards.h"
#include "pathplanner/lib/util/swerve/SwerveSetpoint.h"
#include "pathplanner/lib/path/PathConstraints.h"
 
#include <frc/kinematics/SwerveModuleState.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/RobotController.h>
#include <optional>
 
using namespace pathplanner;
 
namespace pathplanner {
class SwerveSetpointGenerator {
public:
 
    SwerveSetpointGenerator();
 
    SwerveSetpointGenerator(const RobotConfig &config,
            units::turns_per_second_t maxSteerVelocity);
 
    SwerveSetpoint generateSetpoint(SwerveSetpoint prevSetpoint,
            frc::ChassisSpeeds desiredStateRobotRelative,
            std::optional<PathConstraints> constraints, units::second_t dt,
            units::volt_t inputVoltage);
 
    SwerveSetpoint generateSetpoint(SwerveSetpoint prevSetpoint,
            frc::ChassisSpeeds desiredStateRobotRelative,
            std::optional<PathConstraints> constraints, units::second_t dt);
 
    SwerveSetpoint generateSetpoint(SwerveSetpoint prevSetpoint,
            frc::ChassisSpeeds desiredStateRobotRelative, units::second_t dt,
            units::volt_t inputVoltage) {
        return generateSetpoint(prevSetpoint, desiredStateRobotRelative,
                std::nullopt, dt, inputVoltage);
    }
 
    SwerveSetpoint generateSetpoint(SwerveSetpoint prevSetpoint,
            frc::ChassisSpeeds desiredStateRobotRelative, units::second_t dt) {
        return generateSetpoint(prevSetpoint, desiredStateRobotRelative,
                std::nullopt, dt);
    }
 
    inline static bool flipHeading(frc::Rotation2d prevToGoal) {
        return units::math::abs(prevToGoal.Radians()).value() > PI / 2.0;
    }
 
    inline static units::radian_t unwrapAngle(double ref, double angle) {
        double diff = angle - ref;
        if (diff > PI) {
            return units::radian_t(angle - 2.0 * PI);
        } else if (diff < -PI) {
            return units::radian_t(angle + 2.0 * PI);
        } else {
            return units::radian_t(angle);
        }
    }
 
private:
    double kEpsilon = 1e-6;
 
    RobotConfig m_robotConfig;
    units::turns_per_second_t maxSteerVelocity;
    units::volt_t brownoutVoltage;
 
    double findSteeringMaxS(units::meters_per_second_t x_0,
            units::meters_per_second_t y_0, units::radian_t f_0,
            units::meters_per_second_t x_1, units::meters_per_second_t y_1,
            units::radian_t f_1, units::radian_t max_deviation);
 
    inline bool isValidS(double s) {
        return s >= 0.0 && s <= 1.0 && std::isfinite(s);
    }
 
    double findDriveMaxS(units::meters_per_second_t x_0,
            units::meters_per_second_t y_0, units::meters_per_second_t x_1,
            units::meters_per_second_t y_1,
            units::meters_per_second_t max_vel_step);
 
    inline bool epsilonEquals(double a, double b, double epsilon) {
        return (a - epsilon <= b) && (a + epsilon >= b);
    }
 
    inline bool epsilonEquals(double a, double b) {
        return epsilonEquals(a, b, kEpsilon);
    }
 
    inline bool epsilonEquals(frc::ChassisSpeeds s1, frc::ChassisSpeeds s2) {
        return epsilonEquals(s1.vx.to<double>(), s2.vx.to<double>())
                && epsilonEquals(s1.vy.to<double>(), s2.vy.to<double>())
                && epsilonEquals(s1.omega.to<double>(), s2.omega.to<double>());
    }
 
};
}