/* filename: intpid.tea */ #include <aDig.tea> #include <aServo.tea> #include <aCore.tea> #include <aPad.tea> //#include <aPrint.tea> // the scratch PAD index to store RPS // (revolutions per second) #define PADIndexRPS 3 /* less-than and greater-than functions put */ /* in reusable subroutines to save code bytes */ char lt(int a, int b) { return (a < b); } char gt(int a, int b) { return (a > b); } /* * set the ESC output. * PWRlevel is a number between -100 and 100 */ void setESC(int PWRlevel) { int output1 = 0; int neutral = 128; int brake = 120; int FWDRange = 127; // 255 - 128 int REVRange = 120; // 120 - 0 if (lt(PWRlevel,0)) { /* negative for reverse */ output1 = brake + PWRlevel; } else if (gt(PWRlevel,0)) { /* default is positive */ output1 = neutral + PWRlevel; } else { output1 = brake; } // aPrint_String("PID: "); // aPrint_IntDec(PWRlevel); // aPrint_String(" ESC: "); // aPrint_CharUdec((unsigned char)output1); aServo_SetAbsolute(1, (unsigned char)output1 ); } /* * Get the encoder revolutions-per-second * uses a pin_timer with edge detection * (max time per pulse is 104ms) */ unsigned int getRPS(void) { unsigned int PPS = 0; // pulses per second unsigned int RPS = 0; // revolutions per second int TMR; TMR = aDig_ReadTmr(1); // look out for jitter if (TMR < 100) TMR = 100; // original floating point code: // PPS = (1000000/(PPS * 1.6F)); // RPS = PPS / 128; // integer scaled version // convert to uSeconds (1 counter tick = 1.6uS) PPS = (TMR/10) * 16; // convert to pulses per second if (PPS > 0){ PPS = (10000/PPS) * 100; RPS = PPS / 128; // 64 black encoder segments * 2 = 128 } else { PPS = 0; RPS = 0; } // aPrint_String(" RPS: "); // aPrint_IntUdec(RPS); // aPrint_String(" TMR: "); // aPrint_IntDec(TMR); // aPrint_String(" PPS: "); // aPrint_IntUdec(PPS); // aPrint_String("\n"); return (RPS); } /* read the desired RPS from the scratch pad */ char getDesRPS(void) { return(aPad_ReadChar(PADIndexRPS)); } /* save RPS to scratch pad */ void saveRPS(char desRPS) { aPad_WriteChar(PADIndexRPS, desRPS); } /* * a simple pid loop * PID tuning constants are integers between 1 and 100 * setPoint is held in scratch pad * so that it may be altered externally */ void main() { // init vars int processValue = 0; // holds the actual speed (RPS) int setPoint; // holds the desired speed (RPS) // the tuning constants ----------------------------------- int PFactor = 3; // the Proportional tuning constant int IFactor = 5; // the Integral tuning constant int DFactor = 1; // the Derivative tuning constant //---------------------------------------------------------- int error = 0; int Pterm = 0; int Iterm = 0; int Dterm = 0; int lastProcessValue = 0; int lastSetPoint = 0; int sumError = 0; int DprocessValue = 0; int firstExecution = 0; int initialError = 0; int output; saveRPS(0); setESC(0); // configure pin 1 to be input edge timer aDig_Config(1, ADIG_INPUT); // main loop ------------------------------- while (1){ setPoint = (int)getDesRPS(); processValue = (int)getRPS(); error = setPoint - processValue; sumError = sumError + error; // >>> calculate Iterm // -------------------- Iterm = IFactor * sumError; // <---- scaled // first time through if (firstExecution < 2){ if (firstExecution == 0){ //sumError = Manual_value / Ifactor firstExecution = 1; initialError = error; } // wait for error to over correct via Iterm // before going on to normal operation if ( (gt(initialError,0) && lt(error,0)) || (lt(initialError,0) && gt(error,0)) ) { firstExecution = 2; lastProcessValue = processValue; } lastSetPoint = setPoint; // normal operation } else if (processValue >= 0) { // >>> calculate Dterm // ------------------- DprocessValue = lastProcessValue - processValue; lastProcessValue = processValue; Dterm = DFactor * DprocessValue; // <--- scaled if (setPoint == lastSetPoint) { // setpoint has not changed // >>> calculate Pterm // ------------------- Pterm = PFactor * error; // <--- scaled } else { // setPoint has changed // reset PD terms Pterm = 0; Dterm = 0; if (gt(setPoint,lastSetPoint) && gt(processValue,setPoint)){ lastSetPoint = setPoint; lastProcessValue = processValue; } if (lt(setPoint,lastSetPoint) && lt(processValue,setPoint)){ lastSetPoint = setPoint; lastProcessValue = processValue; } } // close (setpoint == lastsetpoint) else }// close else if (precessValue >= 0) output = ((Pterm + Iterm + Dterm) / 100); // <--- scaled back down // make it stop faster if (setPoint==0){ setESC(0); } else { setESC((int)output); } aCore_Sleep(1000); } // close while loop } // close main