WorldAlign.h

 
#pragma once
#include <utility>
 
#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#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/controllers/PIDF.h"
#include "valkyrie/Loggable.h"
 
namespace valor {
 
class WorldAlign : public valor::Loggable {
   public:
    WorldAlign(valor::PIDF x, valor::PIDF y, valor::PIDF rot, frc::TrapezoidProfile<units::meter>::Constraints xConstraints,
               frc::TrapezoidProfile<units::meter>::Constraints yConstraints,
               frc::TrapezoidProfile<units::radian>::Constraints rotConstraints);
 
    static frc::Pose2d offsetGetter(frc::Pose2d, frc::Transform2d);
 
    template <class Distance, class Velocity = units::compound_unit<Distance, units::inverse<units::seconds>>,
              class Acceleration = units::compound_unit<Velocity, units::inverse<units::seconds>>>
    struct StateComponent {
        units::unit_t<Distance> distance{0};
        units::unit_t<Velocity> velocity{0};
        units::unit_t<Acceleration> acceleration{0};
    };
 
    struct StateVector {
        StateComponent<units::meter> xState;
        StateComponent<units::meter> yState;
        StateComponent<units::radian> rotState;
    };
    void setPID(valor::PIDF x, valor::PIDF y, valor::PIDF rot);
    void setPIDx(valor::PIDF);
    void setPIDy(valor::PIDF);
    void setPIDrot(valor::PIDF);
 
    std::array<valor::PIDF, 3> getPID();
    valor::PIDF getPIDx();
    valor::PIDF getPIDy();
    valor::PIDF getPIDrot();
 
    void resetControllers(frc::Pose2d, frc::ChassisSpeeds);
    void resetXController(units::meter_t, units::meters_per_second_t);
    void resetYController(units::meter_t, units::meters_per_second_t);
    void resetRotController(frc::Rotation2d, units::radians_per_second_t);
 
    void setConstraints(frc::TrapezoidProfile<units::meter>::Constraints x, frc::TrapezoidProfile<units::meter>::Constraints y,
                        frc::TrapezoidProfile<units::radian>::Constraints rot);
    void setConstraintsx(frc::TrapezoidProfile<units::meter>::Constraints);
    void setConstraintsy(frc::TrapezoidProfile<units::meter>::Constraints);
    void setConstraintsrot(frc::TrapezoidProfile<units::radian>::Constraints);
 
    void enableContinuousRotation(bool enable = true, units::radian_t minimum = units::radian_t{-std::numbers::pi},
                                  units::radian_t maximum = units::radian_t{std::numbers::pi});
 
    frc::TrapezoidProfile<units::meter>::Constraints getConstraintsx();
    frc::TrapezoidProfile<units::meter>::Constraints getConstraintsy();
    frc::TrapezoidProfile<units::radian>::Constraints getConstraintsrot();
 
    void setGoal(frc::Pose2d, StateVector, bool rotate);
    std::pair<frc::Pose2d, StateVector> getGoal();
 
    frc::ChassisSpeeds calculate(frc::Pose2d, frc::ChassisSpeeds);
    frc::ChassisSpeeds calculate(frc::Pose2d, frc::ChassisSpeeds, StateVector);
 
    void LoggablePeriodic() override;
 
   private:
    frc::ProfiledPIDController<units::meter> xController, yController;
    frc::ProfiledPIDController<units::radian> rotController;
 
    valor::PIDF xPIDF, yPIDF, rotPIDF;  // Store PIDF values to access kV
 
    frc::Pose2d goalPosition, currPosition;
    frc::ChassisSpeeds currSpeed;
    StateVector goalState, setpointState;
    double calculatedXVal, calculatedYVal;
};
 
}  // namespace valor