This season, our team switched to using swerve instead of tank drive, and in the process, we migrated from Pathweaver to Path Planner for the holonomic capabilities. After following some steps from the original WPILib docs to characterize, and parts of the Path Planner library, we updated our code to match with swerve, and got our robot to drive straight. Then, when we tried to add spin to our path, it started spinning more than twice what it was supposed to (~2.2 times more).
Here is our code for this season: Code
Here is the path we are trying to run:
And here is the path we end up running: Spin path
I would check ur PID values for the theta controller. If P is to large you will overshoot and since the Theta controller is continuous, its probably faster to keep spinning in the same direction than to oscilate.
I had originally thought so too, so we reduced our theta Pid all the way down to 0 at some point but it made no change. The interesting part was that SmartDashboard showed that we were -144 degrees off, consistently through all PID values we tried
I suggest checking units: are your compass reading units correctly converted to the units of your Path?
It was enormously helpful to put the odometry and raw gyro readings on Glass (Using the SmartDashboard.putXXX methods) and compare what was happening physically to what we were seeing there.
We tried that, and our gyro seems to be accurate with what the Path units are. One idea we had was to put the entire theta PID Controller onto SmartDashboard, but because we were using SwerveAutoBuilder, we could only pass in the PID Constants rather than an entire controller.
I looked through you code and your SwerveDrivePoseEstimator is using the pigeon’s pitch for the robot’s yaw. Your SwerveDriveOdometry is using the pigeon’s yaw. Which is correct? Are you using the pose estimator?
If the pigeon is not mounted flat with X pointed forward, I suggest you use the pigeon’s Custom Mounting Orientation feature from section 6.6
1 Like
