Swerve encoders

Technical Sensors
,
7

YAGSL seems to accomplish this quite well, I suggest you look in to their codebase.