SwerveModule.h

#pragma once
 
#include <numbers>
 
#include <ctre/phoenix6/CANcoder.hpp>
#include <frc/DutyCycleEncoder.h>
#include <frc/Filesystem.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/geometry/Translation2d.h>
#include <frc/kinematics/SwerveModulePosition.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <frc/simulation/DCMotorSim.h>
#include <units/voltage.h>
#include <valkyrie/Loggable.h>
#include <valkyrie/controllers/BaseController.h>
#include <valkyrie/drivetrain/swerve/Requests.h>
 
namespace valor {
namespace swerve {

class SwerveModule : public valor::Loggable {
   public:
    struct Config {
        std::unique_ptr<BaseController> azimuthMotor;
        std::unique_ptr<BaseController> driveMotor;
        frc::Translation2d location;
 
        // Simulation constants
        units::kilogram_square_meter_t driveInertia = 0.035_kg_sq_m;
        units::kilogram_square_meter_t azimuthInertia = 0.01_kg_sq_m;
    };
 
    SwerveModule(Config config, units::meter_t wheelDiameter);
 
    frc::SwerveModuleState GetState() const;
 
    constexpr frc::SwerveModuleState GetDesiredState() const { return desiredState; }
 
    frc::SwerveModulePosition GetPosition() const;
 
    units::meters_per_second_t GetMaxSpeed() const;
 
    frc::Translation2d GetLocation() const { return location; }
 
    constexpr units::meter_t GetWheelDiameter() const { return wheelConversion * 1_tr / std::numbers::pi; }
 
    void ApplyRequest(requests::ApplyState);
 
    inline BaseController& GetDriveMotor() const { return *driveMotor; }
 
    inline BaseController& GetAzimuthMotor() const { return *azimuthMotor; }
 
    void Reset();
 
    void LoggablePeriodic() override;
 
    // Doesn't override anything, just for convention - called explicitly from
    // Swerve
    void SimulationPeriodic();
 
   private:
    std::unique_ptr<BaseController> azimuthMotor;
    std::unique_ptr<BaseController> driveMotor;
 
    decltype(1_m / 1_tr) wheelConversion;
    frc::Translation2d location;
 
    frc::sim::DCMotorSim driveSim;
    frc::sim::DCMotorSim azimuthSim;
 
    frc::SwerveModuleState desiredState;
};
 
}  // namespace swerve
}  // namespace valor