diff --git a/PID_AutoTune_v0.cpp b/PID_AutoTune_v0.cpp index e775ffe..4ee2d10 100644 --- a/PID_AutoTune_v0.cpp +++ b/PID_AutoTune_v0.cpp @@ -1,148 +1,154 @@ #if ARDUINO >= 100 - #include "Arduino.h" +#include "Arduino.h" #else - #include "WProgram.h" +#include "WProgram.h" #endif #include "PID_AutoTune_v0.h" - PID_ATune::PID_ATune(double* Input, double* Output) { input = Input; output = Output; - controlType =0 ; //default to PI + controlType = 0; // default to PI noiseBand = 0.5; running = false; oStep = 30; SetLookbackSec(10); lastTime = millis(); - } - - void PID_ATune::Cancel() { running = false; -} - +} + int PID_ATune::Runtime() { - justevaled=false; - if(peakCount>9 && running) + Serial.println(peakCount); + justevaled = false; + if (peakCount > 9 && running) { running = false; FinishUp(); return 1; } unsigned long now = millis(); - - if((now-lastTime) absMax) + absMax = refVal; + if (refVal < absMin) + absMin = refVal; } - else + + // oscillate the output base on the input's relation to the setpoint + + if (refVal > setpoint + noiseBand) + *output = outputStart - oStep; + else if (refVal < setpoint - noiseBand) + *output = outputStart + oStep; + + // bool isMax=true, isMin=true; + isMax = true; + isMin = true; + // id peaks + for (int i = nLookBack - 1; i >= 0; i--) { - if(refVal>absMax)absMax=refVal; - if(refVal val; + if (isMin) + isMin = refVal < val; + lastInputs[i + 1] = lastInputs[i]; } - - //oscillate the output base on the input's relation to the setpoint - - if(refVal>setpoint+noiseBand) *output = outputStart-oStep; - else if (refVal=0;i--) - { - double val = lastInputs[i]; - if(isMax) isMax = refVal>val; - if(isMin) isMin = refVal2) - { //we've transitioned. check if we can autotune based on the last peaks - double avgSeparation = (abs(peaks[peakCount-1]-peaks[peakCount-2])+abs(peaks[peakCount-2]-peaks[peakCount-3]))/2; - if( avgSeparation < 0.05*(absMax-absMin)) - { - FinishUp(); - running = false; - return 1; - - } - } - justchanged=false; + + if (isMax) + { + if (peakType == 0) + peakType = 1; + if (peakType == -1) + { + peakType = 1; + justchanged = true; + peak2 = peak1; + } + peak1 = now; + peaks[peakCount] = refVal; + } else if (isMin) + { + if (peakType == 0) + peakType = -1; + if (peakType == 1) + { + peakType = -1; + peakCount++; + justchanged = true; + } + + if (peakCount < 10) + peaks[peakCount] = refVal; + } else { + Serial.println("nein"); + } + + if (justchanged && peakCount > 2) + { // we've transitioned. check if we can autotune based on the last peaks + double avgSeparation = (abs(peaks[peakCount - 1] - peaks[peakCount - 2]) + abs(peaks[peakCount - 2] - peaks[peakCount - 3])) / 2; + if (avgSeparation < 0.05 * (absMax - absMin)) + { + FinishUp(); + running = false; + return 1; + } + } + justchanged = false; return 0; } void PID_ATune::FinishUp() { - *output = outputStart; - //we can generate tuning parameters! - Ku = 4*(2*oStep)/((absMax-absMin)*3.14159); - Pu = (double)(peak1-peak2) / 1000; + *output = outputStart; + // we can generate tuning parameters! + Ku = 4 * (2 * oStep) / ((absMax - absMin) * 3.14159); + Pu = (double)(peak1 - peak2) / 1000; } double PID_ATune::GetKp() { - return controlType==1 ? 0.6 * Ku : 0.4 * Ku; + return controlType == 1 ? 0.6 * Ku : 0.4 * Ku; } double PID_ATune::GetKi() { - return controlType==1? 1.2*Ku / Pu : 0.48 * Ku / Pu; // Ki = Kc/Ti + return controlType == 1 ? 1.2 * Ku / Pu : 0.48 * Ku / Pu; // Ki = Kc/Ti } double PID_ATune::GetKd() { - return controlType==1? 0.075 * Ku * Pu : 0; //Kd = Kc * Td + return controlType == 1 ? 0.075 * Ku * Pu : 0; // Kd = Kc * Td } void PID_ATune::SetOutputStep(double Step) @@ -155,7 +161,7 @@ double PID_ATune::GetOutputStep() return oStep; } -void PID_ATune::SetControlType(int Type) //0=PI, 1=PID +void PID_ATune::SetControlType(int Type) // 0=PI, 1=PID { controlType = Type; } @@ -163,7 +169,7 @@ int PID_ATune::GetControlType() { return controlType; } - + void PID_ATune::SetNoiseBand(double Band) { noiseBand = Band; @@ -176,17 +182,17 @@ double PID_ATune::GetNoiseBand() void PID_ATune::SetLookbackSec(int value) { - if (value<1) value = 1; - - if(value<25) + if (value < 1) + value = 1; + + if (value < 25) { nLookBack = value * 4; sampleTime = 250; - } - else + } else { nLookBack = 100; - sampleTime = value*10; + sampleTime = value * 10; } } diff --git a/Xiaomi-Scooter-Motion-Control.ino b/Xiaomi-Scooter-Motion-Control.ino index 41263e5..fadbe0d 100644 --- a/Xiaomi-Scooter-Motion-Control.ino +++ b/Xiaomi-Scooter-Motion-Control.ino @@ -129,14 +129,15 @@ void motion_control() { if (autotunerActive) { aTune.Cancel(); Serial.println("BRAKE ~> Cancelled autotuner."); - kpHigh = aTune.GetKp(); - kiHigh = aTune.GetKi(); - kdHigh = aTune.GetKd(); - Serial.println(kpHigh); - Serial.println(kiHigh); - Serial.println(kdHigh); - autotunerActive = false; - State = BREAKINGSTATE; + kpHigh = aTune.GetKp(); + kiHigh = aTune.GetKi(); + kdHigh = aTune.GetKd(); + Serial.println(kpHigh); + Serial.println(kiHigh); + Serial.println(kdHigh); + autotuneProcedureTimer = 0; + autotunerActive = false; + State = BREAKINGSTATE; } } else { if (State == BREAKINGSTATE && speed < startThrottle) State = READYSTATE; @@ -218,7 +219,6 @@ void motion_control() { int result = aTune.Runtime(); if (result == 1) { Serial.println("TUNER ~> Finished."); - targetSpeed = 0; currentThrottle = 0; kpHigh = aTune.GetKp(); kiHigh = aTune.GetKi(); @@ -229,24 +229,27 @@ void motion_control() { autotunerActive = false; State = BREAKINGSTATE; throttleWrite(45); - speedController.SetMode(MANUAL); } else { Serial.println(currentThrottle); throttleWrite((int) currentThrottle); } } else if (speed > startThrottle) { - targetSpeed = 20; - currentThrottle = 224; - throttleWrite(224); - autotunerActive = true; - - aTune.SetControlType(1); - aTune.SetOutputStep(30); - aTune.SetLookbackSec(5); - aTune.SetNoiseBand(5); - - speedController.SetMode(AUTOMATIC); - Serial.println("TUNER ~> The speed has exceeded the minimum throttle speed."); + if (autotuneProcedureTimer == 0) { + autotuneProcedureTimer = currentTime; + currentThrottle = 139; + throttleWrite(139); + + aTune.SetControlType(1); + aTune.SetOutputStep(47); + aTune.SetLookbackSec(3); + aTune.SetNoiseBand(5); + + speedController.SetMode(MANUAL); + Serial.println("TUNER ~> The speed has exceeded the minimum throttle speed."); + } else if (autotuneProcedureTimer + 2000 < currentTime) { + autotunerActive = true; + autotuneProcedureTimer = 0; + } } break;