PPLTVController.h

#pragma once
 
#include "pathplanner/lib/controllers/PathFollowingController.h"
#include <frc/controller/LTVUnicycleController.h>
 
namespace pathplanner {
class PPLTVController: public frc::LTVUnicycleController,
        public PathFollowingController {
public:
    PPLTVController(units::second_t dt,
            units::velocity::meters_per_second_t maxVelocity = 9_mps) : LTVUnicycleController(
            dt, maxVelocity), m_lastError(0_m) {
    }
 
    PPLTVController(const wpi::array<double, 3> &Qelems,
            const wpi::array<double, 2> &Relems, units::second_t dt,
            units::meters_per_second_t maxVelocity = 9_mps) : LTVUnicycleController(
            Qelems, Relems, dt, maxVelocity), m_lastError(0_m) {
    }
 
    frc::ChassisSpeeds calculateRobotRelativeSpeeds(
            const frc::Pose2d &currentPose,
            const PathPlannerTrajectoryState &targetState) override {
        m_lastError = currentPose.Translation().Distance(
                targetState.pose.Translation());
 
        return Calculate(currentPose, targetState.pose,
                targetState.linearVelocity, targetState.fieldSpeeds.omega);
    }
 
    inline void reset(const frc::Pose2d &currentPose,
            const frc::ChassisSpeeds &currentSpeeds) override {
        m_lastError = 0_m;
    }
 
    inline units::meter_t getPositionalError() override {
        return m_lastError;
    }
 
    inline bool isHolonomic() override {
        return false;
    }
 
private:
    units::meter_t m_lastError;
};
}