I am wondering if someone can tell me if I am way off on my though process on PID implementation for controlling the speed of a flywheel?
My understanding is that traditional PID is great when you want to reach a fixed point quickly and accurately where the motors are off at the desired target. In a flywheel situation, the motors should be continuously running to maintain a target velocity. If we are using traditional PID control, where motor_power = P + I + D, then when we hit our target, the P, I and D terms become 0 so we are getting a motor_power value of zero, which is not what we want for a constant flywheel velocity.
To me, it makes more sense to experimentally determine the motor_power value that gives you your desired target velocity (by using the debugger data streams) and define that as approximate_power and then use the equation:
motor_power = approximate_power + P + I + D
For example, lets say we determined that our flywheel needs a motor_power setting of 85 to maintain our target velocity. Using
motor_power = 85 + P + I + D should give us full 127 power on start up because the P, I and D terms should be large when our velocity error is large. As we approach the target velocity and our error terms begin to get small, the P, I and D terms should approach 0 and leave us with our desired 85 motor_power. If we have any target velocity overshoot, then the P, I and D terms should become negative and drop our motor_power down below 85 to correct the velocity. The only issue could be that the PID terms donāt get large enough (in the negative direction) to cause a rapid decrease in velocity (like simply setting motor_power to zero momentarily would do). Perhaps there would be a way to include some code that takes into account situations when overshoot gets too big, we could override the PID control and simply set motor_power = 0 until our error gets back into a reasonable range and then let PID take over again. If anyone has any idea on how to code something like that, I would really appreciate seeing how it could be doneā¦ I am a beginner at ROBOTC!
Perhaps this concept is just assumed in all the other threads on PID control or I just totally missed it, but to robotics noob like me, I canāt see any problems with the idea.
I donāt have any experience with the flywheel side of things, but what youāre talking about is adding a bias term to the PID calculation. That is pretty common to help with the loop stability, especially when you have constant forces that you need to compensate for (e.g. weight of a lift).
Can you please post your code, I think weād be more well-suited to help you, if we saw your code. But from what I see,
motor_power = approximate_power + P + I + D
is nowhere near what PID is.
You donāt use an approximate motor power, the P term is your error, or how far away from the target you are. The I term is the accumulated error over time. The d term is the change in error, where you subtract your error from your previous error. Then you multiply all terms by their constants, and add all of the values together, for example
I was simply using the terms P, I and D to represent the calculated values derived from the error terms and constants, so we are on the same page there. I will certainly post my code later today for your review. What I was trying to get at is that I am having a hard time understanding how the motor can continue to run once it reaches target velocity as the P, I and D terms should all go to zero, which would cause the motor to turn offā¦
I didnāt consider thatā¦ I thought that using a small constant for I keeps it from blowing up, or some have mentioned using an integral active zone. So in short, am I just way overthinking all of this?
I doesnāt go to 0. I effectively finds the power needed to maintain a system. It increases power until error is 0 and stays there. If. We had only I the system would reach and maintain target speed fine. It just wouldnāt recover or accelerate very quickly. I takes a while to build up so we need P to help just shove energy into the system when error is too high.
I becomes that power needed to maintain speed all on its own. You probably should limit I to be something around FC power /Ki. Just to ensure I never gets an order of magnitude off but for Velocity like this you rarely need to limit or reset I.
In some PID loop implementations the I term is reset to 0 once error becomes 0. Iām guessing this is not a desired behavior in velocity PID for exactly the reason you mention.
Yup that is exactly the case :). Resetting I to 0 is never even strictly necessary, it is just to stop the motor from trying to eliminate something like 1% of a degree of error forever.
Thank you! So if I am understanding this, there is no need to include a motor hold term because I term essentially figures it out? When tuning the constants, it sounds like you set ki and kd to zero and mess around with kp first. While watching motor power in the debugger window, should I expect to see 127 at any point during spool up from just the P controller or should I not worry about the system utilizing max power during spool up until all three constants have been tuned?
One thing I am finding odd is that my accumulated_error value always starts out at a fairly large negative number (like in the -25,000 range, but it is never the same starting number) when I hit āStartā on the debugger. This is before even giving a command to start the flywheel. Once I start the flywheel, the accumulated_error begins moving in the positive direction as expected but it takes several seconds for it to reach a positive value. Any idea why that would be happening?
A second question I have is when I have ki and kd set to zero, and try to tune kp, I start to get oscillation in the output WAY below the target velocity. For example, if I set a target velocity of 114 rpm, oscillation starts in the range 55 rpm and will never make it to the target velocity unless I use a kp value of around 1.5. I am assuming there is some error in how I am calculating my velocity and associated errors, but I canāt seem to figure it out. Any help would be greatly appreciated!
#include āVex_Competition_Includes.cā //Main competition background codeā¦do not modify!
// Update inteval (in mS) for the flywheel control loop #define FW_LOOP_SPEED 20
// Maximum power we want to send to the flywheel motors #define FW_MAX_POWER 127
// encoder counts per revolution depending on motor #define MOTOR_TPR_269 240.448 #define MOTOR_TPR_393R 261.333 #define MOTOR_TPR_393S 392 #define MOTOR_TPR_393T 627.2 #define MOTOR_TPR_QUAD 360.0
// encoder tick per revolution
float ticks_per_rev; ///< encoder ticks per revolution
// Encoder
long encoder_counts; ///< current encoder count
long encoder_counts_last; ///< current encoder count
// velocity measurement
float motor_velocity; ///< current velocity in rpm
long nSysTime_last; ///< Time of last velocity calculation
// control algorithm variables
long target_velocity; ///< target_velocity velocity
float current_error; ///< error between actual and target_velocity velocities
float last_error; ///< error last time update called
float accumulated_error; ///< total accumulated error
// final motor drive
long motor_drive;
/*Set the flywheel motors */
void FwMotorSet( int value )
{
motor Motor_FWR ] = value;
}
/Get the flywheen motor encoder count RIGHT/
long FwMotorEncoderGet()
{
return( nMotorEncoder Motor_FWR ] );
}
/*Set the controller position RIGHT */
void FwVelocitySet( int velocity )
{
// set target_velocity velocity (motor rpm)
target_velocity = velocity;
/Calculate the current flywheel motor velocity/
void FwCalculateSpeed()
{
int delta_ms;
int delta_enc;
// Get current encoder value
encoder_counts = FwMotorEncoderGet();
// change in system time
delta_ms = nSysTime - nSysTime_last;
nSysTime_last = nSysTime;
// change in encoder counts
delta_enc = (encoder_counts - encoder_counts_last);
// save last position
encoder_counts_last = encoder_counts;
// Calculate velocity in rpm
motor_velocity = (1000.0 / delta_ms) * delta_enc * 60.0 / ticks_per_rev;
// calculate error in velocity
// target_velocity is desired velocity
// current is measured velocity
current_error = target_velocity - motor_velocity;
// Save last error
last_error = current_error;
}
/*Task to control the velocity of the flywheel */
task ShooterControl()
{
float kp = 0.59;
float ki = 0.0;
float kd = 0;
float proportional;
float integral;
float derivative;
//PID control code
while(true)
{
proportional = current_error * kp;
accumulated_error = accumulated_error + current_error;
integral = accumulated_error * ki;
derivative = (current_error - last_error) * kd;
last_error = current_error;
// Set the encoder ticks per revolution
ticks_per_rev = MOTOR_TPR_393S;
// Calculate velocity
FwCalculateSpeed();
// calculate the motor output value
motor_drive = (proportional + integral + derivative);
// limit of motor values
if( motor_drive > 127 ) motor_drive = 127;
if( motor_drive < 0 ) motor_drive = 0;
// set the motor control value
FwMotorSet( motor_drive );
// Run at somewhere between 20 and 50mS
wait1Msec( FW_LOOP_SPEED );
}
}
task usercontrol()
{
// Start the flywheel control task
startTask( ShooterControl );
// Main user control loop
while(1)
{
//Tank control
tankControl( Ch3, Ch2, 10 );
//Hook control
if(vexRT[Btn6DXmtr2] == 1)
{
motor[Hook] = 100;
}
else if(vexRT[Btn6UXmtr2] == 1)
{
motor[Hook] = -40;
}
else
{
Yes, it is true that flywheel speed control is quite different from the classic applications of PID. Exponential shape of the flywheel velocity curve may require asymmetric PID coefficients for stable operation.
This non-linear shape could be handled well with algorithms like TBH, that are somewhat similar to numerical methods for non-linear equation solving. Think about it: flywheel velocity is a function (with an unknown shape) of the motor power and you need to find the input power that will give the required velocity.
However, many teams have PID implementations that work quite well. To understand why, first, lets look at the classic PID terms:
P - stands for āproportionalā and adjusts the power depending on how far you are from the target. The easy analogy is that if the target is far away, you need to throw a ball with a lot of force and, if the target is near, you just need to gently toss it.
D - stands from āderivativeā or how fast are you closing in on the target. It is easy to understand intuitively as well. If you were standing still at armās length from the target you take a step forward, but if you at the same distance and are moving at the high velocity toward the target, you will need a lot of braking power to avoid crashing into it.
I - stands for āintegralā and generally describes how much power losses or resistance are there in the system. This is most widely misunderstood and frequently miscalculated term of PID.
If you read a wikipedia article you will find out that original PID controller was developed to steer ships. In this context āIā could be used to estimate crosswind and indicate that you need less power to sail along the wind than into it.
Another example could be PID controller driving (and stopping) the elevator. Here āIā would correspond to the extra load the elevator is carrying.
You already see that these two examples require different strategies for calculating āIā. While shipās autopilot would need to continuously update āIā based on the wind and wave patterns, elevator needs to reset āIā on the per-trip basis and itās final value, obviously, should not depend on which floor you are going to.
Those who are familiar with PID, understand that I am talking about integral accumulation (sometimes called integral windup) and possible strategies for handling it. Various tutorials describe different methods for dealing with windup, because each application has its own dynamics and meaning for āIā.
In any case, lets look at what is the meaning of PID terms as applicable to the flywheel speed control:
P - corresponds to the difference between current and target velocities. It is obvious that for the shortest recovery time you want the largest available power (as long as it is not going to ruin your gears). However, Kp must not be too large to avoid overshooting with long settling time doing oscillations.
D - corresponds to the notion of Newtonās law that āobject in motion tends to remain in that state of motionā both here and in classic PID application.
However, for flywheel change in process variable (angular velocity) is angular acceleration and represents amount of torque (due to extra power) delivered by the motor and causing that acceleration. You may think that once you reach target speed you just clip the power to the level required to maintain target speed and be done with it.
The problem is that first, you donāt really know the exact value for the target power and, second, your power reduction command could take considerable time to get to the motor controller (6101, 7091, 14878). When you tune your program, the value of Kd is there to protect you from the uncertainty of the command delay vs how much overshoot you could get with the present extra power level.
I - same as in classic PID, it will estimate any extra commanded motor power you will need to overcome any āunforeseenā power losses. I guess that it will mostly estimate the state of PTCs on the way from the battery to the motor, as well as any mechanical issues like loosening screw, bent axle, etcā¦
If you donāt do anything special the decreasing battery voltage will go into āIā, but it is better to compensate for it using battery sensor readings.
I couldnāt tell you for sure what is the best way to compute āIā since we are very much in the middle of debugging it, but my guess is to use moving average with half-life similar to whatever it takes PTCs to cool down and try to ignore any abnormal events like ball launches.
Once again, it could be done either way, but I feel that doing
will yield more stable implementation, because āIā will be estimated independently of the requested power level and will settle faster as you move around the field. It will only need to find correction to the experimentally determined approximate_power.
Note: since velocity curve is nonlinear it may be easier to have separate set of coefficients for each preset shooting location. On the other hand, if you plan to do shooting from any point on the field, you will either need to do more math or accept decreased accuracy with increased recovery time the further you are from the location for which you tuned the coefficients.
This is correct, and is a classical PID+C, where C is some known system constant. It may allow you to come to speed more quickly with less overshoot if it is determined well through a-priori knowledge and/or expirements.
There are a lot of ways to handle specific environments. However, one generic way for a PI+C, as a start, to handle the I windup is to reset it to 0 whenever your system output is maxed out by P+C alone, i.e. 127. If you think about it, your I is not needed at this time since your motor canāt do any more to help, so what is the point of accumulating the error?
Another aspect of this flywheel control over position control is that you probably donāt want to apply any counteracting force. You only want your motor to supply an acting force to keep the flywheel speed constant. So, the forces are asymmetric. Your motor can spin up the flywheel, but only external forces spin it down. This helps you in determining C since it will never change sign. We have a similar issue in temperature control system for things like fryers and ovens (used at McD, fast food, cruise ships, etcā¦) in that we can only apply a heating force, but the environment supplies the cooling force.
This is important since the average will include the area under the curve of the speed decrease pulse you may see when you launch. This is known as a step-input function and is normally how you would tune a loop. However, it could offset your true speed depending upon the I time constant and the duty cycle (% good vs %impulse) of the velocity.
One of the strategies we are testing is to accumulate āIā only when current RPM is withing certain percentage or distance of the target RPM. The cutoff is chosen such that motor power is expected to be well below max. Then you can think of āIā as estimating time it takes to reach from, let say, 95% to 100% of the target velocity and Ki will translate it to extra power over approximate_power needed to maintain the target velocity.
However, you need to be careful. For example, technik jr was too lazy to implement it the right way and just put an if statement:
if( abs(error)<5 ) I=I+error;
This had several problems and algorithm would become unstable if you start getting large measurement noise in your velocity readings. With noisy measurements the algorithm would jump in and out of this condition and compute either underpowered value for āIā when speeding up, or keep overpowered value (by not doing down corrections) if battery was fresh and you could easily overshoot.
The right way of implementing it could be to start accumulating (integrating) āIā after it crosses threshold once and keep doing it for a while as your speed settles. Then when abnormal event happens you stop doing integration and rely on P. On the next cycle you could reset I to 0. Alternatively, you could reuse old value, but then you have to make sure it decays by using some sort of exponential smoothing.
Another important point that @TriDragon reminded by mentioning PI+C is that many PID controller implementations do not use D. The reason for that is that it is often too noisy and just makes entire thing less stable.
If you cannot get D smooth enough you could just drop it and be less aggressive with Kp and Ki to leave yourself some margin for motor power commands to propagate. Alternatively, you can use the difference between your current motor power and approximate_power as a crude estimate for D.
The irony is that the better your build quality is (i.e. less friction) the less stable your flywheel velocity may be if you donāt match it with well tuned PID implementation.
Another note is that C doesnāt necessary need to be a constant. For example, you could run two experiments and determine that with freshly charged battery at 9.0v you need power level of 80 for the full-court shots and when it discharges to 7.5v you need 92. Then you could scale your approximate_power appropriately which should help you to settle on target speed even faster.
This is true, and would be an active feed forward network. I guess, generally, I donāt count a constant C as feedforward since it isnāt active but thatās just me. You could also use this for Kp alterations based upon the difference of the battery voltage and estimated back EMF of the motor.
Yep. in the flywheel case windage should help give a necessary opposing force even without friction.
@tabor473@technik3k@TriDragon ā¦ thank you so much for your detailed advice. I can see why so many teams have given up on PID and went to the TBH algorithm! We started out with TBH due to its simplicity but found that it overshoots the target velocity considerably and takes a long time to settle after a shot (around 2.5 seconds per shot is about the fastest we can push it). I was hoping the PID would give better results, but now I see that it isnāt a magic bullet and is going to take a lot of work to make it work right.
At this stage, I am still struggling to simply find a kp value that gets me close to the target velocity. I was wondering if any of you would be willing to look at my code to see where the errors are? As mentioned previously, if I have the target velocity set for 114, I start to get major oscillation in the velocity range of 55 with a kp of 0.65. This is with ki and kd set to zero. I am wondering if I have an error in the velocity calculation or error calculation or if it is normal to not to be able to achieve target velocity with just the P term active? To me, I donāt see why any oscillation could even occur since the current_error should be positive until target velocity is exceeded.
What make me question my calculations is if I watch accumulated_error in the debugger, it first starts at 0, then within a fraction of a second it jumps to a huge negative number and stays constant until I start the flywheel. As soon as I start the flywheel, accumulated_error begins moving in the positive direction as expected, but it takes several seconds to reach a positive value. The only thing I can think of that would cause this would be if my current_error calculation is messed up. I have attached a copy of my code for your review.
Okay so I am about 50 lines into your program and I have to stop. I am going to say thank you from all of us who help people debug code on the forum. The program is well documented, each function has a header comment describing its goal. The file is organized, has a decent but not obtuse amount of white space and of course is already indented correctly.
I would probably be 2-3 times faster with my programming help and a hell of a lot more willing to help if everyone gave clean organized files like this one.
Thank you for the compliment. I am brand new to programming, VEX and robotics in general, so it is encouraging to hear I am on the right track with the formatting of code. As a coach of a 1st year middle school team, I am trying to learn as much as possible so that I can teach them the correct way to do things so their VEX careers will hopefully be enjoyable and successfulā¦ it is impossible to teach someone something if you donāt fully understand it yourself first.
Please remember that you will always have an error when you only use Kp. This is as it should be, donāt try to have 0 error with just a P algorithm.You tune the P first, the I position will close the error to 0.
Iām surprised you have oscillations with a gain of 0.65 and a target of 114. So when you first start you should have an output power of 74 since you arenāt moving error is (114 - 0).
Out = Kp * error
74 is not a huge value and, unless you have an awesome frictionless system, should spin up over a second or two.
So the code looks really good. In terms of just little things to add, you have support for the loop delay to be modified. Modifying loop delay by definition will change the tuning of the I constant. If you see numbers half as often you need to value them twice as much. If you want to make everything as clean and adjustable as it is currently I would modify
integral = accumulated_error * ki;
to also multiply by loop delay.
Mainly what I want to see is what your velocity is outputting. Try removing where you turn the motor on in your PID setup and just giving the motor a constant power of like 80. After the system gets up to speed what values to you see in the ROBOTC debugger.