diff --git a/src/main/cpp/cowlib/SwerveModule.cpp b/src/main/cpp/cowlib/SwerveModule.cpp index 2fb22b3..c06b39d 100644 --- a/src/main/cpp/cowlib/SwerveModule.cpp +++ b/src/main/cpp/cowlib/SwerveModule.cpp @@ -4,6 +4,7 @@ #include "Constants.h" #include "frc/geometry/Rotation2d.h" +#include "frc/kinematics/SwerveModulePosition.h" #include "frc/kinematics/SwerveModuleState.h" #include "rev/CANSparkMax.h" #include "units/angle.h" @@ -48,3 +49,13 @@ void SwerveModule::drive(frc::SwerveModuleState state) { units::turn_t SwerveModule::GetEncoder() { return this->encoder.GetAbsolutePosition().GetValue(); } + +frc::Rotation2d SwerveModule::GetRotation() { + return frc::Rotation2d(this->GetEncoder()); +} + +frc::SwerveModulePosition SwerveModule::GetPosition() { + return frc::SwerveModulePosition( + units::meter_t(this->driveMotor.GetEncoder().GetPosition()), + this->GetRotation()); +} diff --git a/src/main/include/cowlib/SwerveModule.h b/src/main/include/cowlib/SwerveModule.h index b216601..0fcd46f 100644 --- a/src/main/include/cowlib/SwerveModule.h +++ b/src/main/include/cowlib/SwerveModule.h @@ -4,9 +4,11 @@ #pragma once +#include "frc/geometry/Rotation2d.h" #include "units/angle.h" #include #include +#include #include #include @@ -26,4 +28,6 @@ public: void drive(double speedMetersPerSecond, double angle); void drive(frc::SwerveModuleState state); units::turn_t GetEncoder(); + frc::Rotation2d GetRotation(); + frc::SwerveModulePosition GetPosition(); };