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 } }