Fast WCD Turning Command

[anon37142155]

We have a Andymark Kit drive base, not sure if that affects anything. The goal is to create a turning command that turns an X amount of degrees. I’ve attempted to do a turning command through PID, but struggled to get a fast turning robot with a high P without oscillation. I’ve tried messing with the D value a little, but wasn’t able to get that to work.

What is a good way to do this? Would a possible solution to this to use a ProfiledPIDController, and if so how do you define the constraints for it?

[s-neff]

What are you using for feedback?

[anon37142155]

Gyro (specifically pigeon2), I haven’t considered it, but would trying to use how much the wheels spun to determine rotation work better?

[sam.mulvey747]

How fast is FAST? I’d be happy to provide our “PIDRotateCommand” for tank drive from a few years ago, but it wasn’t exceptionally quick. Great for autonomous though.

[s-neff]

No. Using a gyro is fine, that’s a high quality one. Using the wheel encoders is harder.

Mulvaney has a great suggestion - “what is fast?”. Maybe PID is enough.

Conceptually, the problem is you are building a PID controller to a single setpoint for a system with tons of inertia.

By the time you’re arriving at the setpoint, you have a ton of momentum from the turn, and just turning the control input to zero actually isn’t enough to stop your robot at the setpoint. You need to reduce or even reverse the control input before you actually arrive, for snappy performance.

A position PID controller alone wont do that mathematically. You’re on the right track with the D term, which will reduce acceleration, but there are better methods.

The first conceptual layer here is to use a PIDF form, where you predict the control input required (F), then apply the error correction (PID), rather than only applying error correction.

It’s easy for a flywheel, since a single flywheel rpm setting will use a set amount of energy. So F can be a constant ratio to the rpm setpoint. After the F is applied, the flywheel control is already “almost right”, and the PID doesn’t have to do much.

For a drivetrain turn, we need to predict the desired control input to replicate the effect of F. As a simplification, I recommend imagining four phases - start, accelerating, traveling, decelerating, and stopped.
A trapezoidal motion profile will use a velocity cap to set the maximum travel speed (traveling), an acceleration cap to set the maximum slope of the velocity (acceleration and deceleration), and solve for a series of velocity setpoints that result in the robot arriving at the final position with zero velocity (start and stop).
That series of setpoints is fed into the motors, and then a layer of PID helps the motors stay on target.
So when you’re in the decelerating phase, it’s now possible for your control theory to produce a reversing force - if you’re moving too fast, your controller will command the motors to slow down or even reverse themselves now.

Have you done “drive to distance” with motion profiling? This is the same concept but with angular velocity off the pigeon being your feedback instead of an encoder distance.

I don’t actually know if the wpi profiledPID command does this or not, I’m only an armchair programmer

[anon37142155]

@s-neff

Mulvaney has a great suggestion - “what is fast?”.

I honestly don’t know, I just want it to few snappy. Another problem I forgot to mention that accuracy is another problem that basically caused me to nuke the P value to be quite low.

@s-neff

Have you done “drive to distance” with motion profiling?

We’re just using a Ramsey with SysID values

@s-neff

I don’t actually know if the wpi profiledPID command does this or not, I’m only an armchair programmer

The WPILib in theory just does what you described, is there any good way to finding max velocity and max acceleration or is it just trial and error plus educated guesses?

[ngreen]

I think the P issue, definitely can be reduced by applying feedforward. You need some amount of voltage to overcome the friction in the drive, and if you are trying to do that only with P it has to be so large that by the time you get to the setpoint you will oscillate.

We did try the ProfiledPIDController a couple years ago, and I haven’t really mastered what the current go-to profiled turn code should look like. As far as the constraints, you can try to guess some starting values and tune them to give the consistent results. Or at least that is how I did it.

The output of the ProfiledPID goes to be added to the feedforward, and so you can use a smaller P to get the response around the setpoint. https://github.com/frc1108/Ackbar2022/blob/trunk/src/main/java/frc/robot/commands/drive/FieldOrientedTurn.java

That all said, there probably ways to inline this or write the command into your subsystem rather than like how I did it.

And the feedforward does just use the sysID paramters.

[gdefender]

I agree, feedforward is key. But, one simple thing to look at is your tolerance, is it wide enough? Also, in your code, check if you’re within tolerance and don’t apply any output.