@tabor473@TriDragon Ok guys, I had some time to mess around tonight and have attached some data files. I started with kp =1.0 and the flywheel went into crazy oscillation, so I didn’t bother trying to get data for kp = 2 or 4 as requested. Sorry… don’t want to wreck my teams motors!
I found that a value for kp that gave little to no oscillation (audible or visual in excel) was at 0.4. I have attached that data as well. As suggested by @TriDragon, I then took 63% of kp which gave me 0.252 and set that as my new kp value. Dividing that by 100 gave me 0.00252 for ki. The system seemed stable but took almost 9 seconds to achieve the target velocity, so clearly I have a ways to go! The next question is what do I start modifying to improve the time it takes to reach the setpoint?
I wish I could share some more insights about PID tuning, but we are still debugging our own code. However, I could share a graph that I made to explain a simplified version of TBH to my team.
Let say you need to run flywheel at the speed (wf). If you already know the power level (uf) that would give you (wf) you can set it right away and have flywheel accelerate as shown with curve (1) until you are satisfied with your speed at a time t10. But this is slow and likely you don’t know required power level (uf) or it depends on some external variables?
Our simplified TBH will initially set power to 100% (u0). (You really shouldn’t do that in order to protect motor gears and prevent PTC from overheating) .
When velocity reaches your target (wf) at a time (t1) you take back half of the power and set it to (u1) and when it crosses target again you set it to u2 = (u0+u1)/2, then when it crosses again you set u3=(u1+u2)/2, and so forth until you are happy with the result at a time t9.
Notice that power curves (u1), (u2), (u3), and (u4) are not horizontal lines due to gain accumulation. This is “I” component that brings velocity curve (6) last few Hz to the target without crossing it.
The first thing you might ask is why would it overshoot if you reduce power as soon as velocity reaches the target? To answer this question lets look at the graph with more details:
First of all, you need to know that depending on which motor port you are using there might be delay in changing power of the motor. Let say we don’t know that ports 1 and 10 are fast and are using port2 that gives us 30ms delay on average.
So when you apply full power (u0) the motor only starts turning at 30ms with velocity following exponential curve (2).
At a time (t1) flywheel reaches desired speed, but our main loop runs on 20ms or even larger interval (to reduce measurement noise) and we only become aware of it at a time (t2). Algorithm immediately reacts by changing motor power to (u1), but by the time (t3) when the command reaches the motor controller we already overshoot.
Similar delays happen with the next segments of the curve and you end up doing several bounces before you accept the final speed at a time t9. The delays could easily add up to 0.1 sec or more on each bounce.
Clearly the cause of overshoot is accumulation of the various delays. So is there anything that could be done to reduce it?
First of all, you should run your flywheel motors on ports 1 and 10 to reduce command delay. Then you could reduce your main loop delay to 1ms. Finally, you can switch to IME where getMotorVelocity() could give you almost instantaneous velocity value. However, be aware that IME may introduce additional sources of measurement noise.
This all will reduce overshoot but not eliminate it. The real solution may be not to wait for velocity to cross target threshold, but start reducing the power preemptively, thus requiring the “D” term of PID.
Do you think the oscillations are coming from slop in the drive train? I can move the flywheel a little over 1/3 of a revolution before the motor shaft rotates… I would think that this would cause a lot of bouncing around in the encoder as the motor tries to control the speed.
That is an amazing post @jpearman ! I integrated James’ code into mine and it appears as if it is working perfectly… with one minor problem that I have been trying to figure out for the past two hours and I highly doubt that it has anything to do with James’ code! For some reason my error calculations stopped automatically updating. The error calc runs once and stops. If I press any button on the controller that is designated to set the target velocities, the error calculation runs with each button press. I am afraid I am going to wear out the buttons if I leave the code this way because I need to hit them as fast as I can to keep updating the error calculation! Is there any chance someone would be willing to take a look at the new code and see where I messed up?
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!
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 @jpearmangetEncoderAndTimeStamp() 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;
}
@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).
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???
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.
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.
