Nikita Klimchuk
/
Jannes_PID
PID based on Janne's code
jannes_pid.cpp@0:79200cd7801e, 2020-12-14 (annotated)
- Committer:
- nikitakl
- Date:
- Mon Dec 14 14:28:02 2020 +0000
- Revision:
- 0:79200cd7801e
PID based on Janne's code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nikitakl | 0:79200cd7801e | 1 | #include "mbed.h" |
nikitakl | 0:79200cd7801e | 2 | #include "PID.h" |
nikitakl | 0:79200cd7801e | 3 | #define threshold 0.003 // 1/330 |
nikitakl | 0:79200cd7801e | 4 | |
nikitakl | 0:79200cd7801e | 5 | AnalogIn temperature(p20); |
nikitakl | 0:79200cd7801e | 6 | //static BufferedSerial pc(USBTX, USBRX); |
nikitakl | 0:79200cd7801e | 7 | |
nikitakl | 0:79200cd7801e | 8 | Ticker temperatureTicker; |
nikitakl | 0:79200cd7801e | 9 | |
nikitakl | 0:79200cd7801e | 10 | int readTemperatureFlag=0; |
nikitakl | 0:79200cd7801e | 11 | |
nikitakl | 0:79200cd7801e | 12 | // ??? https://www.keil.com/pack/doc/CMSIS/DSP/html/group__PID.html |
nikitakl | 0:79200cd7801e | 13 | // ??? https://os.mbed.com/questions/1904/mbed-DSP-Library-PID-Controller/ |
nikitakl | 0:79200cd7801e | 14 | // https://os.mbed.com/cookbook/PID |
nikitakl | 0:79200cd7801e | 15 | // http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ |
nikitakl | 0:79200cd7801e | 16 | // PID library https://os.mbed.com/users/aberk/code/PID/docs/tip/classPID.html |
nikitakl | 0:79200cd7801e | 17 | // https://en.wikipedia.org/wiki/PID_controller |
nikitakl | 0:79200cd7801e | 18 | |
nikitakl | 0:79200cd7801e | 19 | //static BufferedSerial pc(USBTX, USBRX); |
nikitakl | 0:79200cd7801e | 20 | |
nikitakl | 0:79200cd7801e | 21 | Ticker PIDticker; |
nikitakl | 0:79200cd7801e | 22 | |
nikitakl | 0:79200cd7801e | 23 | //PID controller(DEFAULT_Kp , DEFAULT_Ki , DEFAULT_Kd , 5); |
nikitakl | 0:79200cd7801e | 24 | #define INTERVAL 0.1 // seconds |
nikitakl | 0:79200cd7801e | 25 | #define INTERVAL_SAME 100ms |
nikitakl | 0:79200cd7801e | 26 | |
nikitakl | 0:79200cd7801e | 27 | // Tip: Check error |
nikitakl | 0:79200cd7801e | 28 | #define Kp 1.0 |
nikitakl | 0:79200cd7801e | 29 | #define Ki 0.0 |
nikitakl | 0:79200cd7801e | 30 | #define Kd 0.0 |
nikitakl | 0:79200cd7801e | 31 | |
nikitakl | 0:79200cd7801e | 32 | PID controller(Kp, Ki, Kd, INTERVAL); |
nikitakl | 0:79200cd7801e | 33 | |
nikitakl | 0:79200cd7801e | 34 | float co=0; // controller output |
nikitakl | 0:79200cd7801e | 35 | float movingAverage=0; |
nikitakl | 0:79200cd7801e | 36 | void computePID() |
nikitakl | 0:79200cd7801e | 37 | { |
nikitakl | 0:79200cd7801e | 38 | //Update the process variable. |
nikitakl | 0:79200cd7801e | 39 | controller.setProcessValue(movingAverage); // Setpoint |
nikitakl | 0:79200cd7801e | 40 | //Set the new output. |
nikitakl | 0:79200cd7801e | 41 | co = controller.compute(); |
nikitakl | 0:79200cd7801e | 42 | } |
nikitakl | 0:79200cd7801e | 43 | |
nikitakl | 0:79200cd7801e | 44 | int main() |
nikitakl | 0:79200cd7801e | 45 | { |
nikitakl | 0:79200cd7801e | 46 | float oldTemperature = temperature.read(); |
nikitakl | 0:79200cd7801e | 47 | float newTemperature= oldTemperature; |
nikitakl | 0:79200cd7801e | 48 | temperatureTicker.attach(&tickerFlagOn, 2); // the address of the function to be attached (tickerFlagOn) and the interval (0.2 seconds) |
nikitakl | 0:79200cd7801e | 49 | float movingAverageArray[10]; |
nikitakl | 0:79200cd7801e | 50 | int movingAverageIndex=0; |
nikitakl | 0:79200cd7801e | 51 | for (int i=0; i < 10; i++) |
nikitakl | 0:79200cd7801e | 52 | movingAverageArray[i]=newTemperature; // fill in the array |
nikitakl | 0:79200cd7801e | 53 | movingAverage=newTemperature; |
nikitakl | 0:79200cd7801e | 54 | |
nikitakl | 0:79200cd7801e | 55 | |
nikitakl | 0:79200cd7801e | 56 | PIDticker.attach(&computePID, INTERVAL_SAME); // the address of the function to be attached (computePID) and the interval |
nikitakl | 0:79200cd7801e | 57 | |
nikitakl | 0:79200cd7801e | 58 | //Analog input from 0.0 to 1.0 |
nikitakl | 0:79200cd7801e | 59 | controller.setInputLimits(0.0, 1.0); |
nikitakl | 0:79200cd7801e | 60 | //Pwm output from 0.0 to 1.0 |
nikitakl | 0:79200cd7801e | 61 | controller.setOutputLimits(0.0, 1.0); |
nikitakl | 0:79200cd7801e | 62 | //If there's a bias. |
nikitakl | 0:79200cd7801e | 63 | //controller.setBias(0.3); |
nikitakl | 0:79200cd7801e | 64 | // ??? controller.setMode(AUTO_MODE); |
nikitakl | 0:79200cd7801e | 65 | //We want the process variable to be |
nikitakl | 0:79200cd7801e | 66 | controller.setSetPoint(0.0757575757575758); // 25 degrees in 0-1 range is 0,0757575757575758 |
nikitakl | 0:79200cd7801e | 67 | |
nikitakl | 0:79200cd7801e | 68 | while (1) |
nikitakl | 0:79200cd7801e | 69 | { |
nikitakl | 0:79200cd7801e | 70 | printf("Driver(PWM)=%.2f\n\r", co); |
nikitakl | 0:79200cd7801e | 71 | |
nikitakl | 0:79200cd7801e | 72 | if (readTemperatureFlag) |
nikitakl | 0:79200cd7801e | 73 | { |
nikitakl | 0:79200cd7801e | 74 | readTemperatureFlag=0; |
nikitakl | 0:79200cd7801e | 75 | newTemperature = temperature.read(); |
nikitakl | 0:79200cd7801e | 76 | movingAverage-=movingAverageArray[movingAverageIndex]/10; // Remove the oldest value from the moving average |
nikitakl | 0:79200cd7801e | 77 | movingAverageArray[movingAverageIndex]=newTemperature; // Add new value to the array, overwrite the oldest value |
nikitakl | 0:79200cd7801e | 78 | movingAverage+=movingAverageArray[movingAverageIndex]/10; // Add the newest value to the moving average |
nikitakl | 0:79200cd7801e | 79 | if (movingAverageIndex < 9) |
nikitakl | 0:79200cd7801e | 80 | movingAverageIndex++; // Increase the array index |
nikitakl | 0:79200cd7801e | 81 | else |
nikitakl | 0:79200cd7801e | 82 | movingAverageIndex=0; // Rollover the array index |
nikitakl | 0:79200cd7801e | 83 | if ((newTemperature < (oldTemperature-threshold)) || (newTemperature > (oldTemperature+threshold))) |
nikitakl | 0:79200cd7801e | 84 | { |
nikitakl | 0:79200cd7801e | 85 | oldTemperature = newTemperature; // These are for the threshold |
nikitakl | 0:79200cd7801e | 86 | printf("AnalogIn=%f, Temperature= %.1f degrees, Average= %.1f degrees\n\r", oldTemperature, 3.3*oldTemperature*100, 3.3*movingAverage*100); |
nikitakl | 0:79200cd7801e | 87 | } |
nikitakl | 0:79200cd7801e | 88 | } |
nikitakl | 0:79200cd7801e | 89 | } |
nikitakl | 0:79200cd7801e | 90 | } |