Hello,
My team has a swerve drive. We use the optimize method so that our swerve modules never have to turn more than 90 degrees–they just invert themselves and turn the shorter way. This year we are using WPILib’s SwerveControllerCommand, and everything works great except for the turning of the modules. When we try to have it run a backwards trajectory, instead of inverting the modules and moving backwards, it turns the modules around 180 degrees and runs them forwards. How do we fix this?
Can you share your code?
The SwerveControllerCommand takes in an outputModuleStates Consumer with an array of desired SwerveModuleStates, essentially mirroring the output of the kinematics object you use in teleop control. The SwerveModuleState.optimize function in WPILib is run on a per module basis, meaning that you would always do optimization downstream of the kinematics (irrespective of whether the kinematics are done through teleop or through the SwerveControllerCommand). So either in your consumer you should call the optimize function on each array element before setting the state to your module or just in your SwerveModule class you should always call the optimize function when it is commanded a new state no matter when/which other class is calling it.
Have you looked at how the SwerveControllerCommand example project calls optimize?