PPHolonomicDriveController.h

#pragma once
 
#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#include <units/velocity.h>
#include <units/length.h>
#include <units/time.h>
#include <units/angular_velocity.h>
#include <frc/geometry/Translation2d.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <functional>
#include <optional>
#include "pathplanner/lib/util/GeometryUtil.h"
#include "pathplanner/lib/config/PIDConstants.h"
#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h"
#include "pathplanner/lib/controllers/PathFollowingController.h"
 
namespace pathplanner {
class PPHolonomicDriveController: public PathFollowingController {
public:
    PPHolonomicDriveController(PIDConstants translationConstants,
            PIDConstants rotationConstants, units::second_t period = 0.02_s);
 
    constexpr void setEnabled(bool enabled) {
        m_enabled = enabled;
    }
 
    inline void reset(const frc::Pose2d &currentPose,
            const frc::ChassisSpeeds &currentSpeeds) override {
        m_xController.Reset();
        m_yController.Reset();
        m_rotationController.Reset();
    }
 
    inline units::meter_t getPositionalError() override {
        return m_translationError.Norm();
    }
 
    frc::ChassisSpeeds calculateRobotRelativeSpeeds(
            const frc::Pose2d &currentPose,
            const PathPlannerTrajectoryState &referenceState) override;
 
    inline bool isHolonomic() override {
        return true;
    }
 
    [[deprecated("Use overrideRotationFeedback instead, with the output of your own PID controller")]]
    static inline void setRotationTargetOverride(
            std::function<std::optional<frc::Rotation2d>()> rotationTargetOverride) {
        PPHolonomicDriveController::rotationTargetOverride =
                rotationTargetOverride;
    }
 
    static inline void overrideXFeedback(
            std::function<units::meters_per_second_t()> xFeedbackOverride) {
        PPHolonomicDriveController::xFeedbackOverride = xFeedbackOverride;
    }
 
    static inline void clearXFeedbackOverride() {
        PPHolonomicDriveController::xFeedbackOverride = { };
    }
 
    static inline void overrideYFeedback(
            std::function<units::meters_per_second_t()> yFeedbackOverride) {
        PPHolonomicDriveController::yFeedbackOverride = yFeedbackOverride;
    }
 
    static inline void clearYFeedbackOverride() {
        PPHolonomicDriveController::yFeedbackOverride = { };
    }
 
    static inline void overrideXYFeedback(
            std::function<units::meters_per_second_t()> xFeedbackOverride,
            std::function<units::meters_per_second_t()> yFeedbackOverride) {
        overrideXFeedback(xFeedbackOverride);
        overrideYFeedback(yFeedbackOverride);
    }
 
    static inline void clearXYFeedbackOverride() {
        clearXFeedbackOverride();
        clearYFeedbackOverride();
    }
 
    static inline void overrideRotationFeedback(
            std::function<units::radians_per_second_t()> rotationFeedbackOverride) {
        PPHolonomicDriveController::rotationFeedbackOverride =
                rotationFeedbackOverride;
    }
 
    static inline void clearRotationFeedbackOverride() {
        PPHolonomicDriveController::rotationFeedbackOverride = { };
    }
 
    static inline void clearFeedbackOverrides() {
        clearXYFeedbackOverride();
        clearRotationFeedbackOverride();
    }
 
private:
    frc::PIDController m_xController;
    frc::PIDController m_yController;
    frc::PIDController m_rotationController;
 
    frc::Translation2d m_translationError;
    bool m_enabled = true;
 
    static std::function<std::optional<frc::Rotation2d>()> rotationTargetOverride;
 
    static std::function<units::meters_per_second_t()> xFeedbackOverride;
    static std::function<units::meters_per_second_t()> yFeedbackOverride;
    static std::function<units::radians_per_second_t()> rotationFeedbackOverride;
};
}