Mecanum.h

#pragma once
#include <frc/Notifier.h>
#include <frc/estimator/MecanumDrivePoseEstimator.h>
#include <valkyrie/BaseSubsystem.h>
#include <valkyrie/controllers/BaseController.h>
#include <valkyrie/drivetrain/mecanum/Requests.h>
#include <valkyrie/gyro/Gyro.h>
#include <valkyrie/util/CoordinateFrames.h>
#include <valkyrie/util/LockedAccess.h>
 
namespace valor {
namespace mecanum {
 
class Mecanum : public BaseSubsystem {
   public:
    static constexpr int NUM_WHEELS = 4;
 
    Mecanum(wpi::array<std::pair<std::unique_ptr<BaseController>, frc::Translation2d>, NUM_WHEELS> wheels, std::unique_ptr<gyro::Gyro> gyro,
            units::meter_t wheelDiameter);
 
    inline FrameWrapper<FieldFrame, frc::Pose2d> GetPose() const { return estimator->GetEstimatedPosition(); }
 
    inline FrameWrapper<FieldFrame, frc::Pose2d> GetRawPose() const { return rawEstimator->GetEstimatedPosition(); }
 
    inline FrameWrapper<FieldFrame, frc::ChassisSpeeds> GetFieldRelativeSpeeds() const { return GetRobotRelativeSpeeds(); }
 
    FrameWrapper<RobotFrame, frc::ChassisSpeeds> GetRobotRelativeSpeeds() const;
 
    frc::MecanumDriveWheelPositions GetWheelPositions() const;
    frc::MecanumDriveWheelSpeeds GetWheelSpeeds() const;
 
    void ApplyRequest(requests::ApplyChassisSpeeds);
    void ApplyRequest(requests::ApplyWheelSpeeds);
 
    void ResetOdometry(frc::Pose2d pose);
 
    void StartOdomThread(units::hertz_t freq) { odomThread.StartPeriodic(1 / freq); }
 
   protected:
    struct Wheels {
        std::unique_ptr<BaseController> frontLeft;
        std::unique_ptr<BaseController> frontRight;
        std::unique_ptr<BaseController> rearLeft;
        std::unique_ptr<BaseController> rearRight;
    };
 
    virtual void UpdateOdometry();
 
    std::unique_ptr<gyro::Gyro> gyro;
    Wheels wheels;
 
   private:
    frc::MecanumDriveKinematics GenerateKinematics(
        wpi::array<std::pair<std::unique_ptr<BaseController>, frc::Translation2d>, NUM_WHEELS> wheels);
 
    decltype(1_m / 1_tr) wheelConversion;
    frc::MecanumDriveKinematics kinematics;
    frc::Notifier odomThread{[this] { UpdateOdometry(); }};
    util::Locked<frc::MecanumDrivePoseEstimator> rawEstimator{kinematics, gyro->GetRotation().ToRotation2d(), GetWheelPositions(),
                                                              frc::Pose2d{}};
 
   protected:
    util::Locked<frc::MecanumDrivePoseEstimator> estimator{kinematics, gyro->GetRotation().ToRotation2d(), GetWheelPositions(),
                                                           frc::Pose2d{}};
};
 
}  // namespace mecanum
}  // namespace valor