PID Theory Question

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


output = (kP*P) + (kI*I) + (kD*D)

So that constant you are using would be called feed forward.

The other thing to consider is if error is 0, what is I? How might the I that you built up compare to your “hold motor power”?

Thanks for your quick responses everyone!

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?

Below is my code as requested.

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!

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port2, LDrive, tmotorVex393_MC29, openLoop, driveLeft)
#pragma config(Motor, port3, RDrive, tmotorVex393_MC29, openLoop, reversed, driveRight)
#pragma config(Motor, port4, Hook, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, Motor_FWL, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port8, Motor_FWR, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor, port9, Intake, tmotorVex393HighSpeed_MC29, openLoop, reversed)
//!!Code automatically generated by ‘ROBOTC’ configuration wizard !!//

#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 velocity errors
current_error = target_velocity - motor_velocity;
last_error    = current_error;

}

/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
{

motor[Hook] = 0;
}

	// Flywheel speed settings
	if( vexRT Btn8UXmtr2 ] == 1 )
	{
		FwVelocitySet( 114 );

	}
	if( vexRT Btn8LXmtr2 ] == 1 )
	{
		FwVelocitySet( 110 );

	}
	if( vexRT Btn8DXmtr2 ] == 1 )
	{
		FwVelocitySet( 60 );

	}
	if( vexRT Btn8RXmtr2 ] == 1 )
	{
		FwVelocitySet( 0.0 );

	}

	if( vexRT Btn7UXmtr2 ] == 1 )
		motor[Intake] = 55;
	else if( vexRT Btn7DXmtr2 ] == 1 )
		motor[Intake] = -127;
	else
		motor[Intake] = 0;


	// Don't hog the cpu 
	wait1Msec(20);
}

}

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


motor_power = approximate_power + Kp*P + Ki*I + Kd*D

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.

Thank you again for your help!

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.

How many motors do you have?

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.

Sample time changes will affect Kd as well.