Differential.h

#pragma once
#include <frc/Notifier.h>
#include <frc/estimator/DifferentialDrivePoseEstimator.h>
#include <valkyrie/BaseSubsystem.h>
#include <valkyrie/controllers/BaseController.h>
#include <valkyrie/drivetrain/differential/Requests.h>
#include <valkyrie/gyro/Gyro.h>
#include <valkyrie/util/CoordinateFrames.h>
#include <valkyrie/util/LockedAccess.h>
 
namespace valor {
namespace differential {
 
class Differential : public BaseSubsystem {
   public:
    Differential(std::vector<std::unique_ptr<BaseController>> leftMotors, std::vector<std::unique_ptr<BaseController>> rightMotors,
                 std::unique_ptr<gyro::Gyro> gyro, units::meter_t trackWidth, units::meter_t wheelDiameter);
 
    FrameWrapper<FieldFrame, frc::Pose2d> GetPose() const { return estimator->GetEstimatedPosition(); }
 
    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::DifferentialDriveWheelPositions GetWheelPositions() const;
    frc::DifferentialDriveWheelSpeeds GetWheelSpeeds() const;
 
    void ApplyRequest(requests::ApplyChassisSpeeds request);
    void ApplyRequest(requests::ApplyWheelSpeeds request);
 
    void ResetOdometry(frc::Pose2d pose);
 
    void StartOdomThread(units::hertz_t freq) { odomThread.StartPeriodic(1 / freq); }
 
   protected:
    virtual void UpdateOdometry();
 
    std::vector<std::unique_ptr<BaseController>> leftMotors;
    std::vector<std::unique_ptr<BaseController>> rightMotors;
    std::unique_ptr<gyro::Gyro> gyro;
 
   private:
    decltype(1_m / 1_tr) wheelConversion;
    frc::DifferentialDriveKinematics kinematics;
    frc::Notifier odomThread{[this] { UpdateOdometry(); }};
    util::Locked<frc::DifferentialDrivePoseEstimator> rawEstimator{kinematics, gyro->GetRotation().ToRotation2d(), GetWheelPositions().left,
                                                                   GetWheelPositions().right, frc::Pose2d{}};
 
   protected:
    util::Locked<frc::DifferentialDrivePoseEstimator> estimator{kinematics, gyro->GetRotation().ToRotation2d(), GetWheelPositions().left,
                                                                GetWheelPositions().right, frc::Pose2d{}};
};
 
}  // namespace differential
}  // namespace valor