task log () {
while (1) {
datalogAddValue(0, SensorValue[catapultPot]);
datalogAddValue(1, SensorValue[catapultUp]);
datalogAddValue(2, SensorValue[platformAttached]);
datalogAddValue(3, SensorValue[platformSolenoid]);
}
}
task PIDControl()
{
while(true)
{
SensorValue[FlyWheel] = 0;//Measure difference
releaseCPU();//Oinkless: Do other stuff
wait1Msec(PI*20);//TimeSample to end all time samples
hogCPU();//Oink: Do nothing else
Error = TargetSpeed[n] - SensorValue[FlyWheel];//How much I'm wrong
datalogAddValue(0, Error);//DATADATADATADATADATADATADATADATADATADATADATADATA
KpError = Kp[n]*Error;//Debuggery & Tuning
Integral[n] += (Error + PrevError)/2;//How wrong I've been
datalogAddValue(1, Integral[n]);//DATADATADATADATADATADATADATADATADATADATADATADATA
KiIntegral = Ki[n]*Integral[n];//Debuggery & Tuning
DeltaE = PrevError - Error;//How much less wrong I am
datalogAddValue(2, DeltaE);//DATADATADATADATADATADATADATADATADATADATADATADATA
KdDeltaE = Kd[n]*DeltaE;//Debuggery & Tuning
PrevError = Error;//What it says on the tin
PIDPower = KpError + KdDeltaE + KiIntegral + stillspeed[n];//PID Equation
BangBang = Error > 0 ? 127 : 0;//On or Off, Pure Binary
PIDBang = abs(Error)>AccError[n] ? BangBang : PIDPower;//Axiom of choice
Flyspeed = PIDBang<0?0:PIDBang*(n==0?0:1);//Limit the flywheel to whole numbers and keep 0 range from doing anything
}
}