I am working in a command based tank drive robot program testing profiled motion. I have a working command to drive forward using ProfiledPidCommand and encoders and it seems to work well.
I then copied the example of using a ProfiledPidCommand to rotate the robot to a new angle from the wpilib doc. This example is in the section called “Combining Motion Profiling and PID in Command-Based” and the example is called “TurnToAngleProfiled”.
Here the code is essentially the same as straight driving but the goal is ±180 degrees and the measurement is supplied by gyro.
I have not been able to get this to work. In the simple drive forward ProfiledPidCommand I wrote, the setpoint always runs a bit ahead of the measured position yielding an error with correct sign and magnitude for the command to drive the robot foward.
When I test my rotate command, the robot starts turning in the correct direction but then accelerates a lot, then switches direction twice before going into a spin in the opposite direction of the requested turn.
I have instrumented this program to look at all kinds of data about what is going on and what I see is the setpoint starts out ahead of the angle measurement starting the turn as expected, but quickly falls behind the angle measurement so the error gets larger and larger and when the angle gets larger than the setpoint, the error sign changes and that is what causes the robot to change direction. The difference continues to get larger and larger putting the robot into a spin.
On the straight driving command, the setpoint and encoder measurement keep pace but when doing the same thing with gyro angle the setpoint does not keep pace with the angle measurement. I can’t figure out what the issue is. I have tried varying all of the parameters fed into the ProfiledPidCommand but nothing helps. I have slowed the rate of turn down but no joy. The angle measurement is confirmed as accurate. Note that I dont have characterization yet for this robot so I arbitrarily selected the max rotation speed at a level below what visual observation shows the robot can do. Varying the max speed does change how fast things happen but the incorrect turn is unaffected by change in max rotation speed.
Note that this command works correctly in simulation.
I had this exact same problem when I was starting. Refactoring that example has been on my list of things to do for a while. In short IMO that example is really horrible for accomplishing that task well. You can get it to work with that example. It will take some significant tuning and even then it will work poorly.
So I would propose skipping you ahead to what will work well. Here is a post that describes what you want to do. This is more complicated but will give much better results.
What makes it horrible is it lacks a feedforward. Once you add that, it’ll work fine; no need to make it more complicated with cascading controllers. In summary, all you need is a ProfiledPIDController on angle with a SimpleMotorFeedforward for velocity and acceleration feedforward.
Remember, feedforward should be doing most of the work. You already know beforehand roughly how much voltage you need to apply (e.g., this much voltage to get this much steady-state velocity), so there’s no reason not to apply it up front so the feedback controller only has to deal with disturbances and minor modeling errors in the feedforward.
I found this still wasn’t enough to get it working the way I wanted. The problem is that the voltage you need to apply isn’t constant and while it can be approximated with a feed forward that still leaves a lot of work to do. If you do it this way you will need to have a “short turn” and a “long turn”.
Nope. All you need is wheel voltage = Kv * wheel velocity + Ka * wheel acceleration for feedforward. Make sure you pair it with aggressive feedback as well. My team used LQR for our drivetrain, but it all boils down to the same equations for the control law (proportional control on angle and angular velocity, then the equation above for feedforward on each wheel). The main thing LQR provided was good default feedback gains because I hate tuning PD controllers by hand; we just had to give it our feedforward gains from frc-characterization (granted, frc-char isn’t the easiest to set up). Estimation for Kv and Ka can work well too (Kv = 12 volts / max velocity, Ka = 12 volts / max acceleration).
Ok yeah, that’s different from the P and D gains, and won’t actually help fix the root cause. setTolerance() only affects whether the controller is considered “at the setpoint” and not the actual controller performance. Stopping before you get close to the target and oscillate avoids the problem area rather than fixing it.
Some possible sources of poor controller performance in your situation include:
Gearbox backlash introducing annoying nonlinearities when the motor shaft rotation direction changes. You can usually hear this by the drivetrain making a loud chattering noise.
Controller gains that are just too high for the system, or a poorly chosen ratio between P and D.
Measurement delay making what would otherwise be good gains induce oscillation. Excessive filtering of the measurements or CAN status frame latency (i.e., measurements that go from a Talon SRX sensor port to the RIO for control) are potential causes.