Skip to content

Commit

Permalink
Did some bug patching and some tuning.
Browse files Browse the repository at this point in the history
  • Loading branch information
@kearfy
kearfy committed Feb 20, 2022
1 parent e7dd116 commit 16f4b42
Show file tree
Hide file tree
Showing 2 changed files with 130 additions and 121 deletions.
204 changes: 105 additions & 99 deletions PID_AutoTune_v0.cpp
View file
Original file line number Diff line number Diff line change
@@ -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)<sampleTime) return false;

if ((now - lastTime) < sampleTime)
return false;
lastTime = now;
double refVal = *input;
justevaled=true;
if(!running)
{ //initialize working variables the first time around
justevaled = true;
if (!running)
{ // initialize working variables the first time around
peakType = 0;
peakCount=0;
justchanged=false;
absMax=refVal;
absMin=refVal;
peakCount = 0;
justchanged = false;
absMax = refVal;
absMin = refVal;
setpoint = refVal;
running = true;
outputStart = *output;
*output = outputStart+oStep;
*output = outputStart + oStep;
} else
{
if (refVal > 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<absMin)absMin=refVal;
double val = lastInputs[i];
if (isMax)
isMax = 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<setpoint-noiseBand) *output = outputStart+oStep;


//bool isMax=true, isMin=true;
isMax=true;isMin=true;
//id peaks
for(int i=nLookBack-1;i>=0;i--)
{
double val = lastInputs[i];
if(isMax) isMax = refVal>val;
if(isMin) isMin = refVal<val;
lastInputs[i+1] = lastInputs[i];
}
lastInputs[0] = refVal;
if(nLookBack<9)
{ //we don't want to trust the maxes or mins until the inputs array has been filled
return 0;
lastInputs[0] = refVal;
if (nLookBack < 9)
{ // we don't want to trust the maxes or mins until the inputs array has been filled
Serial.println("no u");
return 0;
}

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

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;

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)
Expand All @@ -155,15 +161,15 @@ 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;
}
int PID_ATune::GetControlType()
{
return controlType;
}

void PID_ATune::SetNoiseBand(double Band)
{
noiseBand = Band;
Expand All @@ -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;
}
}

Expand Down
47 changes: 25 additions & 22 deletions Xiaomi-Scooter-Motion-Control.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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();
Expand All @@ -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;
Expand Down

0 comments on commit 16f4b42

Please sign in to comment.