Hello, me and my team are fairly new to control loops within FRC, having only used a very barebones PID control the 2023 season.
Our bot uses a wrist mechanism to intake both cones and cubes on mid & high, which in turn is assisted via an elevator. Both mechanisms utilize a basic PID to prevent overshoot and potential damage. However, I heard that feedforward control in addition to our PID could greatly help our mechanisms, especially the wrist, work smoothly, as it currently doesn’t have the prettiest control loop, with what I can only assume to be gravity causing it to slam down upon our frame. So I have a few questions:
1.) How can feedforward be used efficiently in practice?
2.) How can I implement it to our robot?
3.) Is there any certain specialized feedforward control that would be good to know in the future?
Disclaimer: I’m a fairly amateur programmer, so there are still quite a few things I’ll have no idea about. Thanks in advance.
In simplicity PID gets u to where u want to be and Feed forward holds it in position. Using the wrist example, essentially an arm. PID uses the error of the angle u want to be at to move it to that angle, the larger the error, the more power it applies based on ur PID variables. However once u reach the set angle u have no error and the power to the motor based on the PID equation is 0. However in the real world I have gravity which is applying a relative force downwards at all times regardless of which direction the wrist is moving. The feed forward is what holds it in position and counters the force of gravity. The easiest way to start to determine the feed forward value is figure out what voltage holds the wrist parallel with the ground, 0° from the horizontal and then what ever angle the wrist is at the feed forward will apply a proportion of that. The other piece is while the arm is moving the feed forward will add or subtract power based on direction of motion, moving up it provides additional to counter gravity, when moving down it slows the power since gravity is on it side to help over powering the mechanism.
This should help with the slamming down but that will still need to be tuned with PID values.
Depends on what feedforwards you are running. If you are only running gravity feedforward, sure. But feedforwards are also used to overcome system dynamics including inertia, the motor’s own back EMF, viscous, and coulomb friction. These feedforwards aren’t used to keep a station stationary, but to have the motor reach its target velocity without the feedback controller having to do anything. This means that the feedback controller can be tuned to better reject disturbances.
Motion profiling and its associated feedforwards. For high inertia systems or systems that require really high accuracy, instead of PIDing to a postion, you instead want to use feedforward control to follow a desired motion profile and then use a velocity PID to reject disturbances so that the motor exactly follows the desired motion profile.
Additionally, I would look into anti integral windup. When the motor is saturated (meaning that it is being ran at 100% power) and there is any integral control, the integral term will build up and stay “wound up” causing overshoot. There’s a few mechanism you can use to avoid this.
The most effective method is to write a custom PID controller that will not integrate whenever the input is saturated. Additional ways to make integral control behave is to apply an integral cap to prevent the integral from growing extremely large and also add a method decrease the integral buildup quickly whenever it overshoots.
I would recommend avoiding using I altogether because of the issues with integral you mentioned, since in FRC feedforwards can usually handle whatever I would be used for.
My intuition tells me that this is likely powerful enough to not need a feedforward. If you want to try one i would start with a gravity feedforward. This can be done by adding a certain voltage to the motor to counteract the force of gravity, but not induce any movement. For a rotating arm the force of gravity will scale with the cosine of the angle of the arm with the horizontal (if you think about holding out your arm, it takes the most effort to hold up when it is straight out, and none when it is pointing down). You will have to find the voltage needed to counteract this force either with some math or by hand tuning it, personally i think hand tuning is fine. To actually use a feedforward like this i would look at this wpilib docs article about singlejointed arm feedforwards. For more of the theory behind controls, take a look at the wpilib docs intro to control theory
I disagree, integral control has its purpose and its not good to blanket avoid it. Anti-windup is not particularly difficult to implement and is extremely effective. Yes having a model of the system and using feedforwards is better, but its not always necessary and for simple system, PID with anti-windup is really quick to implement and effective at what it does. Additionally, for systems with disturbances, integral control can significant help when other factors such as latency or noise amplification prevent you from further increasing kP or kD.
WPILib has a bunch of feedforward models, including ones for arms and elevators, so you don’t need to write your own. They also have several example projects that show how to do it.
ArmBot (Java, C++): Demonstrates the use of a ProfiledPIDSubsystem to control a robot arm.
ArmBotOffboard (Java, C++): Demonstrates the use of a TrapezoidProfileSubsystem in conjunction with a “smart motor controller” to control a robot arm.
Elevator with profiled PID controller (Java, C++): Demonstrates the use of the ProfiledPIDController class to control the position of an elevator mechanism.
Elevator with trapezoid profiled PID (Java, C++): Demonstrates the use of the TrapezoidProfile class in conjunction with a “smart motor controller” to control the position of an elevator mechanism.
instead of PIDing to a postion, you instead want to use feedforward control to follow a desired motion profile and then use a velocity PID to reject disturbances so that the motor exactly follows the desired motion profile.
How would i implement this in the code?
When the motor is saturated (meaning that it is being ran at 100% power) and there is any integral control, the integral term will build up and stay “wound up” causing overshoot.
This is meant to decrease the noise (large distance between the error and current state), yes? What value should i set the cap at? I havent really messed with my own custom PID before, although i did make a super primitive P-control loop when I first started learning.
For a rotating arm the force of gravity will scale with the cosine of the angle of the arm with the horizontal (if you think about holding out your arm, it takes the most effort to hold up when it is straight out, and none when it is pointing down).
Why do you use the cosine? Ive seen it in examples, I just dont get why
Here’s what, in practice, your pseudocode should look like:
constructor() {
kG = ??
kP = ??
kI = 0
kD = ??
pid = new PID(kP,kI,kD)
}
periodic() {
position = getEncoder()
feedforward = cos(position) * kG
effort = pid.calculate(position)
effort += feedforward
controller.set(effort)
}
That’s basically it. kG will be the effort (voltage, usually) required to hold the arm perfectly horizontal. You can experimentally measure this or estimate it with Recalc. kP and kD you find experimentally afterwards, start with feedforward first. You typically will not need any kI or only need a very very small kI, so tune it last.
No. Noise is an entirely different problem. Noise specifically refers to random variations in sensor readings that are not reflective of the true thing the sensor is measuring.
Integral windup is a specific issue that occurs when an input is saturated. In FRCFIRST Robotics Competition, this generally means you are sending full power to the motor already. Without any anti-windup measures, the integral term will build up while not affecting the output because you can’t send any more power to the motor. But the intergral term is still increasing. Once you get close to your setpoint, most of your feedback controller wants to stop, but because a very large integral term has built up, the output of the controller will still be full power, or that the controller is “wound up”, causing a massive overshoot.
Anti windup measures such as not integrating when saturated (also known as clamping), capping the intergral term, and lowering/clearing the integral term on overshoot all prevent this issue which allows you to run more aggressive integral gains without windup ever being an issue.
Because of physics & trig. Gravity will apply a torque on the arm based on how far away the center of mass is horizontally from the pivot point. If you do the trig, you will find this distance to be the distance from the center of mass to the pivot point times the cosine of the arm angle.
Edit: it doesn’t do precisely what the quote above says; it uses a position PD controller (no I) to reduce errors in position over the course of the move, and at the two end positions, while the feedforward does most of the heavy lifting.
ProfiledPIDController combines a PID controller with a trapezoidal profile generator – i.e., a rate limiter on setpoint change. That way, you don’t tell the elevator, “ok, go to the top NOW” and it goes full power trying to do this. Instead, you command the elevator, “go to some location a reasonable distance away” and then you keep moving the goal at a rate it can keep up with.
6.7 Integral control
A common way of implementing integral control is to add an additional state that is the
integral of the error of the variable intended to have zero steady-state error.
54
Chapter 6. Continuous state-space control
We’llLimelight, an integrated vision coprocessor present two methods:
Augment the plant. For an arm, one would add an “integral of position” state.
Estimate the “error” in the control input (the difference between what was applied versus what was observed to happen) via the observer and compensate for it. We’llLimelight, an integrated vision coprocessor call this “input error estimation”.
In FRCFIRST Robotics Competition, avoid integral control unless you have a very good reason to use it. Integral
control adds significant complexity, and steady-state error can often be avoided with a
motion profile, a well-tuned feedforward, and proportional feedback (i.e., more deterministic options you should be using anyway).
6.7.1 Plant augmentation
Caveats
First, unconstrained integral control exhibits integral windup on a unitstep input. When the error reaches zero, the integrator may still have a large positive accumulated error from the initial ramp-up. The integrator makes the system overshoot until the accumulated error is unwound by negative errors. Poor tuning can lead to instability.[6] Integrating only when close to the reference somewhat mitigates integral windup.
Second, unconstrained integral control is a poor choice for modeled dynamics compensation. Feedforwards provide more precise compensation since we already know beforehand how to counteract the undesirable dynamics.
Third, unconstrained integral control is a poor choice for unmodeled dynamics compensation. To choose proper gains, the integrator must be tuned online when the unmodeled dynamics are present, which may be inconvenient or unsafe in some circumstances. Furthermore, accumulation even when the system is following the model means it still compensates for modeled dynamics despite our intent otherwise. Prefer the approach in subsection 6.7.2.
Once you get close to your setpoint, most of your feedback controller wants to stop, but because a very large integral term has built up, the output of the controller will still be full power, or that the controller is “wound up”, causing a massive overshoot.
Ahhhh I see, that makes a lot more sense, thank you. I believe I saw a few examples of anti-windup in some FTCFIRST Tech Challenge guides to help me out.
Gravity will apply a torque on the arm based on how far away the center of mass is horizontally from the pivot point. If you do the trig, you will find this distance to be the distance from the center of mass to the pivot point times the cosine of the arm angle.
Ohhhh okay okay. My math isn’t too up-to-speed with more advanced trig functions, so I’m a bit lacking there.
That’s basically it. kG will be the effort (voltage, usually) required to hold the arm perfectly horizontal.
Ive been playing around with the tuner, it’s very fascinating. Is kG supposed to be the primary constant that’llLimelight, an integrated vision coprocessor aid in counteracting gravity? And kVKey Volunteer the max velocity, I suppose? I understand kinda the concept of feedforward, it’s just actual execution
kVKey Volunteer describes how much voltage is needed to hold (or “cruise”) at a given constant velocity while overcoming the counter-electromotive force and any additional friction that increases with speed (including viscous drag and some churning losses). The relationship between speed and voltage (at constant acceleration) is almost entirely linear (for FRCFIRST Robotics Competition-legal components) because of how permanent-magnet DC motors work.
I would also like note that the benefit of adding a gravity feedforward is significantly less in multi-jointed arm where forces accumulate throughout the joints which could result in what I call a “ping-pong” effect from the rotational inertia of the links. Depending on the geometry and mainly desired speed and responsiveness of your arm, this effect may be more significant than the gravity.
As others have mentioned above, you COULD* get away without all of this if your motors/mechanisms are overkill enough. Within the scope of FRC, I’d highly recommend implementing feedforward control for lower-dimensional systems like drivetrains and elevators, but I’d advise to approach multi-dimensional systems cautiously as their are dynamics are more complex.
*Disclaimer: Just because you can, doesn’t mean you should. Know your limits, and play within it. Team 1114 (the team that recommended gravity feedforward to us this year, thanks Caleb!) built a great multi-jointed arm this year, and they’ve achieved blazing fast speeds by utilizing gravity feedforward alongside a hard stop for their “lower link” (the link between shoulder and elbow) which from what I could tell, significantly mitigated the “ping-pong” effect of their upper link under acceleration and deceleration. Year after year they implement the simplest yet most effective solutions to their problems by understanding their system in depth.
I’m not sure what you’re getting at - a correctly computed gravity feedforward for each joint of a multi-joint arm will almost certainly improve performance compared to no feedforward at all. It won’t be as good as a full nonlinear dynamics model, but it’s easy to implement.
A naively-applied single-joint feedforward is, of course, not guaranteed to work well for a multi-joint arm.
If you’re not following your profiles closely enough, you may want to swap the gravity portion of the feedforward for a feedback linearizer, which is the same thing but computed off of the measured angle(s) instead of the desired ones. This is less-stable but doesn’t go wonky if you depart from the intended motion. A better solution is to tune your profile constraints to ensure you can actually follow the profiles.
My point is that understanding the characteristics of your own system better could lead you to simpler solutions with less points of failure. Tuning profile constraints, geometry, gearing are all viable avenues to explore that can lead to quicker insight into the dynamics of your system without adding complexity.
I guess what I’m suggesting is that perhaps investigating the existing control system through logging the motor speeds, current readings could reveal what your issue is better than experimenting with a control system you are less familiar with and a system whose dynamics fully understand.
No doubt that understanding the physics and math behind it is key to extracting the maximum out of the system, but the basic steps like tuning gear ratios and understanding the behavior of the motors and existing control loops should not be ignored. Other mechanical/electrical solutions may also exist that add less complexity