Swerve Drives Drifting Over Course of Driving

It seems like you are dealing with a pretty common known swerve drive issue. Check out this post for a more comprehensive (and math-heavy) explanation of the issue. To compensate for the drift while translating, my team runs a simple P controller on the rotation so that we constantly align to our last gyro heading. Check out our build blog and our github for more details

Essentially it is this

subconsciously adjusting for this drift

but done automatically instead of by the driver