# Swerve Drive Cannot Drive Straight

Hello, we are struggling with having our robot drive straight and we are unsure why, we are applying a constant percent output and there is a slight drift to the robot at higher speeds.
We are using the SDS mk4’s with Falcon 500s on all of them.

GitHub - FRC-509/2023ChargedUp: chawged uwp this is our github page and the code we are using at the moment.

Thanks for any help.

If you are running things open loop there will be a variety of factors that cause drift (i.e. subtle differences in friction between gearboxes). You really need to be running things in velocity mode to counter this. Or more generally: open loop with swerve has very limited utility/flexibility.

If you are running closed loop then you need to make sure all the azimuth angles are aligned, if one swerve is at the wrong angle the behavior you mentioned happens.

We tried using a closed loop with constants that the sysid tool that WPIlib provides and the same drifting behavior is occuring when moving in a straight line. We updated our code in the GitHub linked above in a branch named dev if you wouldn’t mind looking.

https://imgur.com/a/v3vWx5R here is video

Have you attempted to internally track and change what angle you want based off of the input and perpetually control the robot based off angles instead of feeding in angular velocity?

I haven’t tried doing that. how would I approach doing so?

Create a PID for the angular velocity sent to the drive function and do not use field relative chassis speeds then calculate the angular velocity based off the desired angle (which can be mutated based off of desired angular velocity).

Even when hard-coding our angular and strafe velocity to 0, and physically locking the modules straight with 3d-printed blocks, the robot still drifts; We used SysId for getting feedforward constants and PID constants for closed-loop velocity, but we are pretty sure the PID isn’t pushing hard enough, and the velocity setpoints are never actually being reached. In fact, decreasing the P gain does not seem to worsen the condition either, so it might be that the PID is not doing anything at all. However, we don’t know how to confirm this, nor do we know how to manually
tune closed-loop velocity while on the ground.

I would recommend logging the measured + target velocity for each wheel, and see if one side consistently falls farther behind than the other. You can also log the voltage output for each side module - if your PID is working properly, the modules that are measured as falling farther behind should have higher voltages applied to them than the better-tracking ones.

It looks like our PID isn’t working as it should. How can we properly tune it manually?

My realm of knowledge is all in simulation and theory, so I’m afraid I can’t help you too much there. That said, I think sysid can recommend some PID constants based on your system and controller characteristics. Also, if you haven’t yet, you could use feed forward for your wheel velocity (and possibly also for the module rotation, depending on how the control is implemented) which should reduce the amount of work your PID has to do.

Another option for PID tuning is to follow the rough outline of start with all 0, increase P until you get steady-state oscillation, then cut it in half and increase D until you get a nice approach to the setpoint. You can then play with P and D to see if you can get better performance.

Remember that each module will have a velocity PID controller for the velocity and a position PID controller for the heading, which would have separate tuning constants.

Finally, make sure to tune with a fully-weighted robot on the carpet, not on blocks. A well-tuned swerve for driving will have weird behavior on blocks, which is normal.

For reference, 449’s Mk4I L2 constants can be found here: robot2023/SwerveConstants.kt at main · blair-robot-project/robot2023 · GitHub. We just use P for both PID controllers, so maybe we didn’t do it properly, but it works fine so eh.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.