TrajectoryFollower.h

#pragma once
 
#include <algorithm>
#include <cmath>
#include <functional>
#include <optional>
 
#include <choreo/trajectory/SwerveSample.h>
#include <choreo/trajectory/Trajectory.h>
#include <frc/Timer.h>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/geometry/Pose2d.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <frc2/command/Command.h>
#include <frc2/command/CommandHelper.h>
#include <frc2/command/CommandPtr.h>
#include <frc2/command/Commands.h>
#include <frc2/command/FunctionalCommand.h>
#include <frc2/command/SequentialCommandGroup.h>
#include <units/acceleration.h>
#include <units/time.h>
 
#include "valkyrie/Loggable.h"
#include "valkyrie/WorldAlign.h"
#include "valkyrie/controllers/BaseController.h"
#include "valkyrie/controllers/PIDController.h"
#include "valkyrie/controllers/PIDF.h"
#include "valkyrie/drivetrain/swerve/Requests.h"
 
namespace valor {

struct AccelerationState {
    units::meters_per_second_squared_t ax;
    units::meters_per_second_squared_t ay;
    units::radians_per_second_squared_t alpha;
};

struct AutoSwerveState {
    units::second_t timestamp;
    frc::Pose2d pose;
    frc::ChassisSpeeds speeds;
    AccelerationState accel;
};

class TrajectoryFollower {
   public:
    struct Config {
        std::function<void(frc::Pose2d)> resetOdom;
        std::function<void(swerve::requests::ApplyChassisSpeeds)> drive;
        std::function<frc::Pose2d()> poseGetter;
        std::function<std::vector<frc::SwerveModuleState>()> swerveStatesGetter;
        units::meter_t wheelRadius;
        frc::DCMotor driveMotor;
        double driveGearing;
        frc::AprilTagField field = frc::AprilTagField::kDefaultField;
    };

    TrajectoryFollower() = delete;
    TrajectoryFollower(TrajectoryFollower&) = delete;
    TrajectoryFollower(TrajectoryFollower&&) = delete;

    enum class TrajectoryEndCondition { TIME, POSITION };
 
    enum class FieldSymmetry { MIRROR, ROTATE };

    static void Register(Config config);

    static frc2::CommandPtr FollowPath(std::optional<choreo::Trajectory<choreo::SwerveSample>> traj,
                                       std::optional<TrajectoryEndCondition> end, std::optional<units::meter_t> tol,
                                       std::optional<std::function<bool()>> inter, bool isMirrored, bool resetOdom);
    static void SetPID(WorldAlign::TranslationalPIDF_t x, WorldAlign::TranslationalPIDF_t y, WorldAlign::RotationalPIDF_t rot);
 
   private:
    static AutoSwerveState ConvertChoreoSampleIntoSwerveState(choreo::SwerveSample sample);
    static AutoSwerveState Interop(units::second_t time, std::vector<AutoSwerveState> traj);
    static frc2::CommandPtr GetFollowCommand(std::optional<choreo::Trajectory<choreo::SwerveSample>> traj, TrajectoryEndCondition end,
                                             std::optional<units::meter_t> tol, std::optional<std::function<bool()>> interruption,
                                             bool isMirrored);
    static AutoSwerveState FlipSwerveState(AutoSwerveState state, FieldSymmetry sym);
    static AutoSwerveState MirrorSwerveState(AutoSwerveState state);
 
    static bool IsAllianceFlipped();

    static std::unique_ptr<PIDController<units::meter, units::meters_per_second>> xController;
    static std::unique_ptr<PIDController<units::meter, units::meters_per_second>> yController;
    static std::unique_ptr<PIDController<units::radian, units::radians_per_second>> rotController;
    static frc::Timer timer;
 
    static std::optional<Config> config;
 
    static std::optional<frc::AprilTagFieldLayout> field;
 
    static Loggable loggable;
};
}  // namespace valor