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.