PID Theory Question

Wow… that is embarrassing! Thank you so much. Your code for improved velocity calculation works awesome!

Just as a followup, with the @jpearman new velocity calculation code running, I was able to increase kp from 0.4 to 0.87 without oscillation! I’ll post some more data tomorrow night after I get a chance to do some tuning. Thanks to all (@TriDragon, @tabor473, @frogs2345, @technik3k, @jpearman) who have given invaluable input on this thread so far. I hope all of this information will be of use to other too!

I know this isn’t ground breaking useage of velocity control, but I think some great information is here for those who (like me) want to fully understand every aspect of the code that goes into doing something as seemingly simple as acting as a cruise control for a flywheel!

@technik3k @tabor473 @TriDragon @jpearman Sorry for the delay in posting some more results…

So we spent some time today tuning kp and ki and are at the max of both values that will give us a fast spool up time without getting any oscillation (we were able to get to a kp of 3.05 and ki of 0.06). Based on the general consensus of the forum members, trying to implement the D term of PID is tough, so we have left that alone for now. Datalogging our first shot was a little disappointing… as you can see from the attached file, we overshot our target velocity significantly after a ball was launched. The target velocity overshoot was actually worse in this case than the TBH logarithm we were using before. Any recommendations? I understand the D term is there to help with overshoot, but I am not sure if I should have the team work on tuning this variable if it is just going to be a lesson in futility…

Actually in our PID the best recovery time seems to be with kp=3.2 and there is a value of kd that kind of works for the specific velocity and loop delay to stop oscillations but it proven elusive to get it to work both for the range of target speeds and battery voltages.

The main source of oscillations is the delay in the feedback loop between you commanding new motor speed and your velocity calculation code actually sensing changes. If you make your measurement delay short, like doing 3/4 exponential smoothing with getMotorVelocity() every 5 ms you will get almost instantaneous result but a lot of noise. And if you use getEncoderAndTimeStamp() over a longer period like 50ms you get very little noise and long delay.

So, before going to a full blown filter implementation, I want to try getMotorVelocity() for fast estimates and then switch to @jpearman getEncoderAndTimeStamp() method for estimating long term values for ki.

I am reluctant to post the code, as I am still not sure if it is the right approach, but I could say that at least one part seems to work well. Once the error is within 5% of the target velocity it switches from the segment with aggressive kp=3.2 to a different set of coefficients with kp=0.55 and only then starts estimating long term ki.

Also there are two ki’s - one for being above target speed and one for being below:


I+=(error<0 ? 0.001 : 0.0004) *error;

Where negative value for error corresponds to overshooting. This helps stability, because it is much easier to increase speed than to wait for flywheel to slow down. Specific values for ki will depend on your measurement loop delay.

Then it should detect if the ball is going through and switch back to aggressive kp, however there are still oscillations there due to discontinuity between segments and it is still work in progress.

After another day of debugging the code seems to be stable enough, so I am going to post it here.

There was just way to much noise to compute D, so it is gone. Instead there are three different ways to compute P depending in which part of the acceleration curve it is.

When power is initially turned on there is a slew rate of 5 motor power units per 5 ms to protect motor gears. Then as the flywheel accelerates aggressively (segment=0) the power is pretty much 100%. As the velocity gets closer to the target the motor power drops fast using P=err^2 function.

Once within ~5% of the target (segment=1) it is using much more conservative kp=0.55 and starts estimating I with two asymmetric coefficients (with larger one corresponding to overshooting).

If the ball is going through the launcher it switches to safe mode (segment=2) and stays there for 80ms before switching back to the aggressive acceleration mode.

There are two turbo motors on ports 1 and 10 for fast response. Speed is monitored with IME and there are LEDs to give driving team clues about flywheel speed. Also, if IME stops working the code will fallback to hardcoded targetPow which could be adjusted with 6U and 6D buttons.

float Batt = 0;
float vel  = 0;
float vel2 = 0;
float targetRPM = 0;
float targetPow = 0;
float mPow=0,P,I=0;
float acc=0, velP=0;

int   segment = 0,  a=0, b=0;
unsigned long segmentEnd = 0;

void FlywheelSpeedControl()   // called every 5ms
{
	if(vexRT[Btn8L] == 1) {targetPow = 63;	targetRPM = 136; I=0; segment=0; } // base
	if(vexRT[Btn8U] == 1) {targetPow = 55;	targetRPM = 118; I=0; segment=0; } // mid-field
	if(vexRT[Btn8R] == 1) {targetPow = 46;	targetRPM =  97; I=0; segment=0; } // at the bar
	if(vexRT[Btn8D] == 1) {targetPow =  0;	targetRPM =   0; I=0; segment=0; }

	if(vexRT[Btn6U]==1 && a==0) {targetRPM++; targetPow+=0.5; a=1;} else if(vexRT[Btn6U]==0) a=0;
	if(vexRT[Btn6D]==1 && b==0) {targetRPM--; targetPow-=0.5; b=1;} else if(vexRT[Btn6D]==0) b=0;

	short RPM = getMotorVelocity(fw1);

	Batt = (Batt * 7 + nImmediateBatteryLevel / 1000.0) / 8;

	if( segment == 2 && nPgmTime > segmentEnd ) segment = 0;
	
	P=0;

	LED(0,0,0,0);

	if( RPM > 0 )
	{
		vel  = ( 3*vel  + RPM)/4;  // smoothed velocity
		vel2 = (15*vel2 + RPM)/16; // even more smoothed velocity
		float err = targetRPM - vel2;
		acc = 10*(vel - velP);
		velP = vel;
		if( abs(err) < 3 ) LED(1,1,0,0); // green
		else if( err > 6 ) LED(0,0,1,1); // red

		if( segment == 0 ) // aggressive acceleration
		{
	  	    if( err > 5 ) P = err*err/2; // undershoot, kp>=3
	  	    else if( err<-3 ) P = err;    // overshoot, kp=1
	  	    else segment=1;
		}

		if( segment == 1 && acc < -22 ) // ball is going through
		{
	  	    segment = 2;
	  	    segmentEnd = nPgmTime + 80; // do not increase motor power for the next 80ms
		}

		if( segment == 1 ) // maintain constant speed
		{
	  	    P = err * 0.55; // kp=0.55
	  	    I += err * (err>0 ? 0.0008 : 0.002); // assymmetric ki, larger when overshooting
		}
	}

	float mp = (7.623/Batt) * (targetPow + P + I); // scaling, it was tuned at 7.623v 

	if( mp<0 ) mp=0;
	if( mp>127 ) mp=127;

	if( mp > mPow+5 )  // limit slew rate to 5 power units per 5ms
	     mPow = mPow+5;
	else mPow = mp;

	motor[fw1] = mPow;
	motor[fw2] = mPow;
}

Hope this helps.

1 Like

@TriDragon - I absolutely love the safety net concept built in if the encoder gets goofed up. We actually had that happen during a match a few weeks ago.

Two questions:
1.Did you include the battery voltage factor to help give the algorithm a better hint at the motor power calculation or did you find a flaw in the way P + I is being calculated at different voltages? My understanding is motor power should be automatically adjusted by the PI controller regardless of battery voltage (at least down to the point where the battery is too weak to get max power to the motors anymore).

  1. Have you found it necessary to tune kp and ki for each velocity setting? With our current setup, we tuned kp and ki at our highest speed setting and are getting occasional oscillation at our lowest speed setting. Maybe we need to tune kp and ki at low speed???

Thanks for the awesome ideas!!!

Great write up @technik3k

On your incrementing buttons, I got a different RPM step per power increment. I got about 2.2 RPM per PWM step versus 0.5

Graphing in Excel I got the function
TARGET_POWER = (2.2949*TARGET_RPM) - 8.4562

Of course this falls down when you get near top PWM signals because the motor does not really move much faster past a certain point. But the flywheel is load. So your mileage may vary.

Very old post here. Did not do so well in the forum cut over… An original jpearman greatness post reference…
https://vexforum.com/t/estimating-269-motor-current-based-on-h-bridge-models/21876/1
https://vexforum.com/index.php/attachment/56464d7f967fa_motor_speed_comparison.jpg

I’m still gradually fixing up old threads.

hey @jpearman , i have a six motor single fly wheel, need help with a pid. i want it to ramp up to 127 until it reaches the rpm, then slow to the reuquired value to keep that rpm. i have a IEM on the flywheel, help

So just a tip from one new forum member to another… there are countless threads on PID control all over this forum. I would recommend you spend some time reading through as much as you can and do your best to come up with some code. If you get stuck, I would suggest you start a new thread and ask more specific questions than what you previously asked. Be sure to include your code in your post… the experts on this forum will be a lot more willing to help you if you show them that you have done some ground work first. FYI… the code posted above would work awesome for a single flywheel if you integrate it properly into the code you are currently using to control your robot. It will take some tuning of all of the variables and the experts will not be able to do that for you… they can only be determined by trial and error on your robot.

Yes, you are correct, in general kp and ki should be configured for the stable operation at the lowest intended speed. The reason is the steeper power (acceleration) gradient that you have when you apply high power level while flywheel is rotating at the lower speeds.

Electric current through the motor (and resulting torque) is proportional to the difference between battery voltage and back EMF. The faster motor rotates the more back EMF it generates and less potential torque it could produce.

However, in the code I posted this shouldn’t really matter, because it tries to exit aggressive acceleration segment before any oscillations could start and “maintain constant velocity” segment has conservative value for kp that should give limited power at any target velocity.

Yes, battery variability could be estimated with “I”. However, if you take care of as many things as possible, like approx targetPow and battery voltage scaling, then it will take less time for “I” to settle on the stable value.

That’s true. The reason I picked 0.5 (instead of 0.45) is that driving team will have more predictable button behavior if IME fails: two pushes per power unit. If IME works we only care about targetRPM and P+I should easily correct any targetPow inaccuracy.

What I intended to do and completely forgot was to show how flywheel could be controlled with fractional power levels both in PID and manual fallback mode. While NbN launcher may not need that much accuracy, it still makes PID operation more precise and stable.

Anyone wants to guess how with just 4 character mod to the code above you can have half-step flywheel power levels: 42, 42.5, 43, 43.5, 44, …

We had a chance to field-test the code from Jan 25 post at the competition and it performed exceptionally well.

While , with our two-motor flywheel, we weren’t the fastest shooter at the competition, we were, probably, the most accurate. In our best robot skills run all the balls went into the high goal with the exception of one scored in the low goal (driving team went a bit conservative on the speed and didn’t have time to use all driver loads).

Funny thing is that the only team that beat us in skills had a two-motor puncher which was shooting faster than us and, despite missing some of the balls, went to the other side of the field and scored a number of driver loads from there.

You definitely need more than two motors for the faster scoring, but simply adding more power will not necessarily help if you cannot control it well with your code.

In fact, we went to a competition in December with exactly the same flywheel, run with three motors and very similar code, but had a disastrous results with destroyed motor gears and quite bad performance.

After a month of research and trying out many variations of PID-like algorithms we literally came back to almost the original code but with the two important exceptions: out programmer now understands importance of handling measurement noise in the flywheel velocity readings and we now wait 80ms before hitting motors with 127 power level to protect the gears.

I just want to underscore this one more time, because, while it was simple to explain a need for a delay with a broken gear in hand, it took a lot of effort to communicate to a 7th grader what measurement noise is and why filtering it is so much important for a control algorithm.

And finally, an answer to the question from the previous post:

motor[fw1] = mPow + 0.5;
motor[fw2] = mPow;

This will double power resolution for controlling flywheel, by running one of the motors a power unit higher if mPow fractional part is >= 0.5. This is not a game changer all by its own but makes it easier to maintain constant speed without extra fluctuations.