I’m in the process of implementing the c++ SwerveDriveKinematics libraries on our swerve robot, and I’ve reached an unusual problem. In the code for each swerve module, there is a function to set the desired state. The issue is with state optimization. When frc::SwerveModuleState::Optimize() is used, forward movement works fine, but any rotation above 45 degrees in either direction causes the angle of the swerve module to switch to a vastly different angle, and begin oscillating uncontrollably. When frc::SwerveModuleState::Optimize() is commented out, however, the angles are not optimized but the robot drives as intended.
Here’s all the code involved in driving the module:
units::degree_t SwerveModule::getAngle() { return units::degree_t{m_angleEncoder.GetPosition()};}
void SwerveModule::SetDesiredState(const frc::SwerveModuleState& desiredState) {
// Optimize the reference state to avoid spinning further than 90 degrees
frc::Rotation2d const current_rotation = getAngle();
auto const [optimized_speed, optimized_angle] = frc::SwerveModuleState::Optimize(desiredState, current_rotation);
// Set the motor outputs.
m_driveMotor.Set((double) optimized_speed);
m_angleMotor.Set(TalonFXControlMode::Position, optimized_angle.Degrees().value()*(4096.0 / 360.0));
}
If you have any knowledge of what might be causing this, your help would be greatly appreciated!
I know base falcon swerve has their own optimize function and it has a comment that says the wpilib one doesn’t work. Try copying the logic and see if that fixes the issue.
What’s in your getAngle() function? I had a very similar problem in Java caused by getAngle using the absolute encoder with all its latency rather than the turning motor’s actual encoder.
Specifically, it’s this method which makes sure if your wheel is reading 400 degrees and you command 225 degrees module position with a speed of 1, you’ll set an angle of 405 degrees with a speed of -1 (if I did the math right…). The point is that the CTRE internal encoder keeps accumulating after 360 degrees, it doesn’t go from 359 back to 0.
OP, it sounds like you are commanding it to go from 400 degrees all the way back around to 225 degrees, in the example above.
Except, this happens when set on blocks when the encoder starts measuring zero. As soon as the encoder angle exceeds 45 degrees in either direction, the oscillations begin. As well copying the logic causes identical results.
It looks like some part of your code is not treating current angles of “just less than 360°” and “just over 0°” as being close to each other. I can’t tell from what you’ve shared exactly where the problem is.
We dealt with the same issue, only difference being that we used Java. To deal with this, I wrote an optimize method that goes through every case of swerve module heading.
In addition to trying to get to our setpoint, I also thought of the possibilities where we would be crossing the 0->360 barrier on the encoder, and also when it would be fastest to have the wheel reverse. For more implementation, you can check out our robot code: FRC2023/SwerveModuleMK4.java at main · Team2338/FRC2023 · GitHub. Happy coding!
I think the problem is happening because the m_angleEncoder is returning angles in the opposite direction from the direction of the m_angleMotor. You can see that in your graphs: when the Desired angle goes up from 0 to +45, the Current angle goes down from 0 to -45 (360 to 315).
That doesn’t cause any problem at first, because as long as the difference is less than 90 degrees, Optimize() leaves the desired angle alone. Once you go past 45 degrees, though, the difference becomes more than 90 degrees and all hell breaks loose – Optimize() thinks something like: “the desired angle is 60 degrees, but the module is at -60 now, so it’ll be closer to go to -120 instead and reverse the drive motor”. That doesn’t work too well, because the swerve module is actually at +60 degrees, not -60!
Because your swerve drives correctly when you comment Optimize() out, the “Desired angle” must be correct, and the “Current angle” is the one that needs to be inverted. So maybe just putting a minus sign in front of m_angleEncoder.GetPosition() in your first line will fix the problem.
It makes it so no swerve module ever has to rotate by more than 90 degrees, to get to where you are asking it to go – if you ask it to turn to a direction wihich is more than 90 degrees from where it is now, Optimize() will instead have it go the opposite direction until it is lined up the way it needs to be, and will also reverse the drive motor so the module moves in the direction you desire. For example: if the swerve module forward direction is currently pointing to 180 degrees, and you ask it to go to 10 degrees, Optimize() will change that so it moves to 190 degrees instead, and moves the drive motor in reverse; the resulting motion is the same, but the module only had to rotate by 10 degrees rather than 170 degrees.
There’s a good description of it here: WPILib docs: Optimize() function
