89 lines
2.4 KiB
C#
89 lines
2.4 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
|
|
namespace DroneClient.utils
|
|
{
|
|
internal class AdaptivePID
|
|
{
|
|
//Initial
|
|
private float P = 1;
|
|
private float I = 0.2f;
|
|
private float D = 0.005f;
|
|
|
|
//Adaptive
|
|
private float aP = 1;
|
|
private float aI = 0.2f;
|
|
private float aD = 0.005f;
|
|
|
|
private float P_min = 0; private float P_max = 10;
|
|
private float I_min = 0; private float I_max = 1;
|
|
private float D_min = 0; private float D_max = 0.1f;
|
|
|
|
private float alpha = 0.001f;
|
|
private float beta = 0.001f;
|
|
private float gamma = 0.0005f;
|
|
|
|
private float prev_error = float.NaN;
|
|
private float integral = 0;
|
|
|
|
public AdaptivePID()
|
|
{
|
|
alpha = 0; beta = 0; gamma = 0;
|
|
}
|
|
|
|
public AdaptivePID(float p, float i, float d, float alpha = 0.001f, float beta = 0.0005f, float gamma = 0.00005f)
|
|
{
|
|
P = p; aP = p;
|
|
I = i; aI = i;
|
|
D = d; aD = d;
|
|
this.alpha = alpha;
|
|
this.beta = beta;
|
|
this.gamma = gamma;
|
|
}
|
|
|
|
public float process(float error, float T)
|
|
{
|
|
if (T <= 0) return 0;
|
|
integral += error * T;
|
|
float derivative = 0;
|
|
if (float.IsFinite(prev_error))
|
|
{
|
|
derivative = (error - prev_error) / T;
|
|
}
|
|
prev_error = error;
|
|
|
|
float dP = -alpha * error * error;
|
|
float dI = -beta * error * integral;
|
|
float dD = -gamma * error * derivative;
|
|
|
|
aP += dP; aI += dI; aD += dD;
|
|
|
|
aP = constrain(aP, P_min, P_max);
|
|
aI = constrain(aI, I_min, I_max);
|
|
aD = constrain(aD, D_min, D_max);
|
|
integral = constrain(integral, -10, 10);
|
|
float output = aP * error + aI * integral + aD * derivative;
|
|
|
|
return output;
|
|
}
|
|
|
|
public void reset()
|
|
{
|
|
integral = 0;
|
|
prev_error = 0;
|
|
aP = P; aI = I; aD = D;
|
|
}
|
|
|
|
static float constrain(float val, float min, float max)
|
|
{
|
|
if (min >= max) return val;
|
|
if(val < min) return min;
|
|
if(val > max) return max;
|
|
return val;
|
|
}
|
|
}
|
|
}
|