Programming Swerve

[feverittm]

Hello Everyone!

The team I work with created a swerve drive last season and has functional code to operate it. This code was fully functional, however it did not have any of the odometry or pose computation features that the new libraries include. While I could update the libraries (with quite a bit of work) to use an updated swerve library, I am not looking forward to this method.

Now I would like to remake this code using the new WPIlib swerve libraries. The example code looks reasonable and I understand the split between the module and the drivetrain definitions. However, when I have been looking through these examples I notice that a number of those implementations use a ‘invertible’ turning encoder. Our module is similar to the SDS MK2 module which uses an absolute MA3 encoder connected to the RoboRio for the turn position and in our case uses a CIM (I know overkill!) on a TalonSPX for the turn motor. This means that there is no concept of inverting the turn encoder.

I thought I understood that the inversion on each module was required to get the angle of the wheel correct.

How do you use the WPIlib swerve code which a module that uses an absolute angle feedback?

Thanks

[jjsessa]

Just write a method that calls your encoder, and returns your module angle, performing any math you need to get the units/bounding correct, heres how we do ours:

public double getMagAngle(){ // rads
   return new Rotation2d((getMagTicks() - turningEncoderZero) * SwerveModuleConstants.kTicks2Rads).rotateBy(new Rotation2d()).getRadians();
}

All the Rotation2D stuff at the end is just a lazy way to bound the angle to -Pi,Pi, which I dont think you need to do for WPILIB, we just do this to make things easier to visually debug.

Then you can just call this method in your PID controller

[thatmattguy]

Here’s how we used a MA3 encoder:

AnalogInput analogInput = new AnalogInput(0);
double offsetRads = 0; // Tune per to get 0 to face forward
Rotation2d gyroAngle = new Rotation2d((1.0 - analogInput.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI) + offsetRads)```

Just ensure that when you rotate the module counter-clockwise the angle increases. If it doesn't, negate the angle so that it does.