Trying to use YAGSL with homemade swerve modules. The drive motors are being extremely erratic (starting/stopping).
I finally pared it down to a test case that just drives the robot with a constant vY of 0.7, a constant vX of 0.0, omega of 0, and field relative is false. Maximum speed for the SwerveDrive object is 1m/s.
In this specific case, YAGSL is requesting the correct setpoints for all the angle motors (90 degrees, straight forward, yay), but is only doing a non-zero setpoint for the left front drive motor. The setpoint for that motor is about 0.6 (wobbling around that +/- 0.01 or so).
The module files for the 4 corners are identical except for their locations, motor ids, and absolute encoder offsets.
I did just notice that wheel speed desaturatization wasn’t included anymore and added that back on line 157. It shouldn’t fix your issue but does affect others.
Found it. Looks like the cosine compensator in SwerveModule is causing me grief; until you have the steering motors pretty much right, the compensator could go to 0, which would shut down the drive motor.
The students just grabbed a static copy of the example, so it’s hard to update to pick up the changes that have crawled in in the last couple of weeks. I’m gonna go home, get this thing submoduled so we can pick up fixes, and see where we are tomorrow.
Good to know! i had added that this year to try to reduce drift while changing directions, WPILib implements it differently in a way that I am considering too but i didnt like how it would change the heading when you change directions.
so what is the expectation for the range of values returned by absolute encoders? The cosinecompensator code is return steerangleerror as -270; I would have expected +90?
I looked at the fix, and am not sure that it will help my issue; it looks like we’re introducing more opportunities for it to cut off the drive motor power.
the oddity I noticed last night was that the (normalized) steer motor error could be outside the -90…90 range (which would be the case we are testing for with the check for a negative cosineScalar). If optimize did it’s job, we should never be more than 90 degrees off. I need to look closer at this.
To facilitate the process of tuning this whole thing, I suggest to make the cosine compensator switchable from the json (so it can be turned off until everything is tuned). That way teams can get a start on tuning the drive PIDs without the compensator doing funny things to them.
don’t worry about implementing that suggestion; I wrote it last night, and should have a pull request ready late tonight (still need to test it, and we are having weather interfere with our work sessions).
I thought about this but I avoid changing JSONs at all costs to keep them as stable as possible. I would consider a programmatic way to turn it off instead however.
adding this option does not change the behaviour of existing json files. if a field is missing, then the value will be whatever is set in the constructor. in this case, saying
public boolean useCosineCompensator = true;
in ModuleJson.java and guarding the compensation code with
double cosineScalar = 1.0;
if (configuration.shouldCosineCompensate) {
// Taken from the CTRE SwerveModule class.
// https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46
/* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */
/* To reduce the "skew" that occurs when changing direction */
double steerMotorError = desiredState.angle.getDegrees() - getAbsolutePosition();
/* If error is close to 0 rotations, we're already there, so apply full power */
/* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */
cosineScalar = Math.cos(Units.degreesToRadians(steerMotorError));
/* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */
if (cosineScalar < 0.0)
{
cosineScalar = 0.0;
}
CANSparkMax cdm = (CANSparkMax) driveMotor.getMotor();
SmartDashboard.putNumber("motor." + cdm.getDeviceId() + ".cosineScalar", cosineScalar);
SmartDashboard.putNumber("motor." + cdm.getDeviceId() + ".steermotorerror", steerMotorError);
}
will make existing configurations act as they do now, and give the option of saying "useCosineCompensator": false, in the module configurations.