WorldAlign.h

#pragma once
#include <utility>
 
#include <frc/geometry/Pose2d.h>
#include <frc/geometry/Transform2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/trajectory/TrapezoidProfile.h>
#include <networktables/NetworkTableInstance.h>
#include <networktables/StructTopic.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/base.h>
#include <units/length.h>
#include <units/time.h>
#include <units/velocity.h>
#include <valkyrie/Loggable.h>
#include <valkyrie/controllers/PIDF.h>
#include <valkyrie/controllers/ProfiledPIDController.h>
#include <valkyrie/drivetrain/swerve/Swerve.h>
 
namespace valor {

class WorldAlign : public valor::Loggable {
   public:
    using TranslationalPIDF_t = valor::PIDF<units::meter, units::meters_per_second>;
    using RotationalPIDF_t = valor::PIDF<units::radian, units::radians_per_second>;

 
    WorldAlign();

    void SetPID(TranslationalPIDF_t x, TranslationalPIDF_t y, RotationalPIDF_t rot);

    inline TranslationalPIDF_t GetXPID() const { return xController.GetPID(); }

    inline TranslationalPIDF_t GetYPID() const { return yController.GetPID(); }

    inline RotationalPIDF_t GetRotationalPID() const { return rotationalController.GetPID(); }

    void ResetControllers(frc::Pose2d, frc::ChassisSpeeds);

    void SetGoal(frc::Pose2d goal, frc::ChassisSpeeds state);
 
    void SetPositionTolerance(units::meter_t xTol, units::meter_t yTol, units::radian_t rotTol);
 
    void SetVelocityTolerance(units::meters_per_second_t xTol, units::meters_per_second_t yTol, units::radians_per_second_t rotTol);

    std::pair<frc::Pose2d, frc::ChassisSpeeds> GetGoal() const;
 
    std::pair<frc::Pose2d, frc::ChassisSpeeds> GetSetpoint() const;

    frc::ChassisSpeeds Calculate(frc::Pose2d pose, frc::ChassisSpeeds currentSpeeds);
 
    constexpr bool AtGoal() const { return xController.AtGoal() && yController.AtGoal() && rotationalController.AtGoal(); }
 
    constexpr bool AtSetpoint() const { return xController.AtSetpoint() && yController.AtSetpoint() && rotationalController.AtSetpoint(); }
 
    inline ProfiledPIDController<units::meter, units::meters_per_second>& GetXController() { return xController; }
 
    inline ProfiledPIDController<units::meter, units::meters_per_second>& GetYController() { return yController; }
 
    inline ProfiledPIDController<units::radian, units::radians_per_second>& GetRotationalController() { return rotationalController; }
 
    void LoggablePeriodic() override;
 
   private:
    ProfiledPIDController<units::meter, units::meters_per_second> xController{{}}, yController{{}};
    ProfiledPIDController<units::radian, units::radians_per_second> rotationalController{{}};
 
    frc::Pose2d currPosition;
    frc::ChassisSpeeds currSpeed;
};
 
class WorldAlignCommand : public WorldAlign, public frc2::CommandHelper<frc2::Command, WorldAlignCommand> {
   public:
    WorldAlignCommand(swerve::Swerve& swerve);
 
    void Initialize() override;
 
    void Execute() override;
 
    bool IsFinished() override;
 
    constexpr void EnableRotation(bool e) { rotationEnabled = e; }
 
   private:
    swerve::Swerve& swerve;
    bool rotationEnabled = true;
};
 
}  // namespace valor