PID and Encoder Control

My team is working on swerve drive and for the turning we decided to use the PID system along with encoders to tell the turning motor where to turn. I have printed both the SetPoint and the Encoder values to the Dashboard. Both are printing the correct values. We are using the DIO ports on the RoboRio for our encoders. My question is how can get the SetPoint value to actually be written to the motor? (Don’t mind the horrible wiring…still developing the electronics board)Thanks in advance.

This is a velocity PID on the speed(wheel) motor?

We’ve got the speed motors all figured out, right now it’s just the setting the turning motor to the PID Encoder values.

Are you using SwerveDriveKinematics and the WPILIb drivetrain modeling libraries?

No, we’ve done some custom coding. We could try it that way if it’s easier though.

This won’t directly help with controlling the module, but it will help with the math.
WPILib’s examples use a ProfiledPIDController for the turning motors. Are you familiar with trapezoidal profiling?

Not very familiar with trapezoidal profiling; we’ve got all of the math figured out now it’s more of a question of writing our values to the encoder. We’re looking into how the WPILib example does this now.

WPILib uses a profile for that turning because it’s positional control over only a few rotations. So it basically ramps up the velocity target (to prevent “floor the gas” syndrome) and then ramps it down to stop right at the target.