├── PID_AutoTune_v0 ├── Examples │ └── AutoTune_Example │ │ └── AutoTune_Example.pde ├── PID_AutoTune_v0.cpp └── PID_AutoTune_v0.h └── README.txt /PID_AutoTune_v0/Examples/AutoTune_Example/AutoTune_Example.pde: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | byte ATuneModeRemember=2; 5 | double input=80, output=50, setpoint=180; 6 | double kp=2,ki=0.5,kd=2; 7 | 8 | double kpmodel=1.5, taup=100, theta[50]; 9 | double outputStart=5; 10 | double aTuneStep=50, aTuneNoise=1, aTuneStartValue=100; 11 | unsigned int aTuneLookBack=20; 12 | 13 | boolean tuning = false; 14 | unsigned long modelTime, serialTime; 15 | 16 | PID myPID(&input, &output, &setpoint,kp,ki,kd, DIRECT); 17 | PID_ATune aTune(&input, &output); 18 | 19 | //set to false to connect to the real world 20 | boolean useSimulation = true; 21 | 22 | void setup() 23 | { 24 | if(useSimulation) 25 | { 26 | for(byte i=0;i<50;i++) 27 | { 28 | theta[i]=outputStart; 29 | } 30 | modelTime = 0; 31 | } 32 | //Setup the pid 33 | myPID.SetMode(AUTOMATIC); 34 | 35 | if(tuning) 36 | { 37 | tuning=false; 38 | changeAutoTune(); 39 | tuning=true; 40 | } 41 | 42 | serialTime = 0; 43 | Serial.begin(9600); 44 | 45 | } 46 | 47 | void loop() 48 | { 49 | 50 | unsigned long now = millis(); 51 | 52 | if(!useSimulation) 53 | { //pull the input in from the real world 54 | input = analogRead(0); 55 | } 56 | 57 | if(tuning) 58 | { 59 | byte val = (aTune.Runtime()); 60 | if (val!=0) 61 | { 62 | tuning = false; 63 | } 64 | if(!tuning) 65 | { //we're done, set the tuning parameters 66 | kp = aTune.GetKp(); 67 | ki = aTune.GetKi(); 68 | kd = aTune.GetKd(); 69 | myPID.SetTunings(kp,ki,kd); 70 | AutoTuneHelper(false); 71 | } 72 | } 73 | else myPID.Compute(); 74 | 75 | if(useSimulation) 76 | { 77 | theta[30]=output; 78 | if(now>=modelTime) 79 | { 80 | modelTime +=100; 81 | DoModel(); 82 | } 83 | } 84 | else 85 | { 86 | analogWrite(0,output); 87 | } 88 | 89 | //send-receive with processing if it's time 90 | if(millis()>serialTime) 91 | { 92 | SerialReceive(); 93 | SerialSend(); 94 | serialTime+=500; 95 | } 96 | } 97 | 98 | void changeAutoTune() 99 | { 100 | if(!tuning) 101 | { 102 | //Set the output to the desired starting frequency. 103 | output=aTuneStartValue; 104 | aTune.SetNoiseBand(aTuneNoise); 105 | aTune.SetOutputStep(aTuneStep); 106 | aTune.SetLookbackSec((int)aTuneLookBack); 107 | AutoTuneHelper(true); 108 | tuning = true; 109 | } 110 | else 111 | { //cancel autotune 112 | aTune.Cancel(); 113 | tuning = false; 114 | AutoTuneHelper(false); 115 | } 116 | } 117 | 118 | void AutoTuneHelper(boolean start) 119 | { 120 | if(start) 121 | ATuneModeRemember = myPID.GetMode(); 122 | else 123 | myPID.SetMode(ATuneModeRemember); 124 | } 125 | 126 | 127 | void SerialSend() 128 | { 129 | Serial.print("setpoint: ");Serial.print(setpoint); Serial.print(" "); 130 | Serial.print("input: ");Serial.print(input); Serial.print(" "); 131 | Serial.print("output: ");Serial.print(output); Serial.print(" "); 132 | if(tuning){ 133 | Serial.println("tuning mode"); 134 | } else { 135 | Serial.print("kp: ");Serial.print(myPID.GetKp());Serial.print(" "); 136 | Serial.print("ki: ");Serial.print(myPID.GetKi());Serial.print(" "); 137 | Serial.print("kd: ");Serial.print(myPID.GetKd());Serial.println(); 138 | } 139 | } 140 | 141 | void SerialReceive() 142 | { 143 | if(Serial.available()) 144 | { 145 | char b = Serial.read(); 146 | Serial.flush(); 147 | if((b=='1' && !tuning) || (b!='1' && tuning))changeAutoTune(); 148 | } 149 | } 150 | 151 | void DoModel() 152 | { 153 | //cycle the dead time 154 | for(byte i=0;i<49;i++) 155 | { 156 | theta[i] = theta[i+1]; 157 | } 158 | //compute the input 159 | input = (kpmodel / taup) *(theta[0]-outputStart) + input*(1-1/taup) + ((float)random(-10,10))/100; 160 | 161 | } 162 | -------------------------------------------------------------------------------- /PID_AutoTune_v0/PID_AutoTune_v0.cpp: -------------------------------------------------------------------------------- 1 | #if ARDUINO >= 100 2 | #include "Arduino.h" 3 | #else 4 | #include "WProgram.h" 5 | #endif 6 | 7 | #include 8 | 9 | 10 | PID_ATune::PID_ATune(double* Input, double* Output) 11 | { 12 | input = Input; 13 | output = Output; 14 | controlType =0 ; //default to PI 15 | noiseBand = 0.5; 16 | running = false; 17 | oStep = 30; 18 | SetLookbackSec(10); 19 | lastTime = millis(); 20 | 21 | } 22 | 23 | 24 | 25 | void PID_ATune::Cancel() 26 | { 27 | running = false; 28 | } 29 | 30 | int PID_ATune::Runtime() 31 | { 32 | justevaled=false; 33 | if(peakCount>9 && running) 34 | { 35 | running = false; 36 | FinishUp(); 37 | return 1; 38 | } 39 | unsigned long now = millis(); 40 | 41 | if((now-lastTime)absMax)absMax=refVal; 60 | if(refValsetpoint+noiseBand) *output = outputStart-oStep; 66 | else if (refVal=0;i--) 73 | { 74 | double val = lastInputs[i]; 75 | if(isMax) isMax = refVal>val; 76 | if(isMin) isMin = refVal2) 112 | { //we've transitioned. check if we can autotune based on the last peaks 113 | double avgSeparation = (abs(peaks[peakCount-1]-peaks[peakCount-2])+abs(peaks[peakCount-2]-peaks[peakCount-3]))/2; 114 | if( avgSeparation < 0.05*(absMax-absMin)) 115 | { 116 | FinishUp(); 117 | running = false; 118 | return 1; 119 | 120 | } 121 | } 122 | justchanged=false; 123 | return 0; 124 | } 125 | void PID_ATune::FinishUp() 126 | { 127 | *output = outputStart; 128 | //we can generate tuning parameters! 129 | Ku = 4*(2*oStep)/((absMax-absMin)*3.14159); 130 | Pu = (double)(peak1-peak2) / 1000; 131 | } 132 | 133 | double PID_ATune::GetKp() 134 | { 135 | return controlType==1 ? 0.6 * Ku : 0.4 * Ku; 136 | } 137 | 138 | double PID_ATune::GetKi() 139 | { 140 | return controlType==1? 1.2*Ku / Pu : 0.48 * Ku / Pu; // Ki = Kc/Ti 141 | } 142 | 143 | double PID_ATune::GetKd() 144 | { 145 | return controlType==1? 0.075 * Ku * Pu : 0; //Kd = Kc * Td 146 | } 147 | 148 | void PID_ATune::SetOutputStep(double Step) 149 | { 150 | oStep = Step; 151 | } 152 | 153 | double PID_ATune::GetOutputStep() 154 | { 155 | return oStep; 156 | } 157 | 158 | void PID_ATune::SetControlType(int Type) //0=PI, 1=PID 159 | { 160 | controlType = Type; 161 | } 162 | int PID_ATune::GetControlType() 163 | { 164 | return controlType; 165 | } 166 | 167 | void PID_ATune::SetNoiseBand(double Band) 168 | { 169 | noiseBand = Band; 170 | } 171 | 172 | double PID_ATune::GetNoiseBand() 173 | { 174 | return noiseBand; 175 | } 176 | 177 | void PID_ATune::SetLookbackSec(int value) 178 | { 179 | if (value<1) value = 1; 180 | 181 | if(value<25) 182 | { 183 | nLookBack = value * 4; 184 | sampleTime = 250; 185 | } 186 | else 187 | { 188 | nLookBack = 100; 189 | sampleTime = value*10; 190 | } 191 | } 192 | 193 | int PID_ATune::GetLookbackSec() 194 | { 195 | return nLookBack * sampleTime / 1000; 196 | } 197 | -------------------------------------------------------------------------------- /PID_AutoTune_v0/PID_AutoTune_v0.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_AutoTune_v0 2 | #define PID_AutoTune_v0 3 | #define LIBRARY_VERSION 0.0.1 4 | 5 | class PID_ATune 6 | { 7 | 8 | 9 | public: 10 | //commonly used functions ************************************************************************** 11 | PID_ATune(double*, double*); // * Constructor. links the Autotune to a given PID 12 | int Runtime(); // * Similar to the PID Compue function, returns non 0 when done 13 | void Cancel(); // * Stops the AutoTune 14 | 15 | void SetOutputStep(double); // * how far above and below the starting value will the output step? 16 | double GetOutputStep(); // 17 | 18 | void SetControlType(int); // * Determies if the tuning parameters returned will be PI (D=0) 19 | int GetControlType(); // or PID. (0=PI, 1=PID) 20 | 21 | void SetLookbackSec(int); // * how far back are we looking to identify peaks 22 | int GetLookbackSec(); // 23 | 24 | void SetNoiseBand(double); // * the autotune will ignore signal chatter smaller than this value 25 | double GetNoiseBand(); // this should be acurately set 26 | 27 | double GetKp(); // * once autotune is complete, these functions contain the 28 | double GetKi(); // computed tuning parameters. 29 | double GetKd(); // 30 | 31 | private: 32 | void FinishUp(); 33 | bool isMax, isMin; 34 | double *input, *output; 35 | double setpoint; 36 | double noiseBand; 37 | int controlType; 38 | bool running; 39 | unsigned long peak1, peak2, lastTime; 40 | int sampleTime; 41 | int nLookBack; 42 | int peakType; 43 | double lastInputs[101]; 44 | double peaks[10]; 45 | int peakCount; 46 | bool justchanged; 47 | bool justevaled; 48 | double absMax, absMin; 49 | double oStep; 50 | double outputStart; 51 | double Ku, Pu; 52 | 53 | }; 54 | #endif 55 | 56 | -------------------------------------------------------------------------------- /README.txt: -------------------------------------------------------------------------------- 1 | /********************************************************************************************** 2 | * Arduino PID AutoTune Library - Version 0.0.1 3 | * by Brett Beauregard brettbeauregard.com 4 | * 5 | * This Library is ported from the AutotunerPID Toolkit by William Spinelli 6 | * (http://www.mathworks.com/matlabcentral/fileexchange/4652) 7 | * Copyright (c) 2004 8 | * 9 | * This Library is licensed under the BSD License: 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are 12 | * met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in 18 | * the documentation and/or other materials provided with the distribution 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | **********************************************************************************************/ 32 | 33 | Note: I'd really hoped to have this more polished before release, but with the 34 | osPID coming out I felt that this needed to be out there NOW. if you 35 | encounter any issues please contact me, or post to the diy-pid-control 36 | google group. 37 | --------------------------------------------------------------------------------