mirror of https://github.com/rusefi/autopid.git
172 lines
5.2 KiB
C++
172 lines
5.2 KiB
C++
/*
|
|
* @file pid_sim.h
|
|
*
|
|
* PID Controller simulator.
|
|
* Used
|
|
*
|
|
* @date Oct 10, 2019
|
|
* @author andreika, (c) 2019
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "global.h"
|
|
|
|
|
|
typedef enum {
|
|
PID_SIM_REGULATOR = 0, // imitate load disturbance
|
|
PID_SIM_SERVO, // imitate setpoint change
|
|
} pid_sim_type_e;
|
|
|
|
template <int maxPoints>
|
|
class PidSimulator {
|
|
public:
|
|
PidSimulator(pid_sim_type_e simType_, int order, double target1_, double target2_, double dTime_, double modelBias, const char *outputFile_) :
|
|
simType(simType_), target1(target1_), target2(target2_), dTime(dTime_), methodOrder(order), outputFile(outputFile_),
|
|
plantInput(1.0 / dTime_), plant1(&plantInput, nullptr, 0, modelBias), plant2(&plantInput, nullptr, 0, modelBias) {
|
|
}
|
|
|
|
PidAccuracyMetric simulate(int numPoints, const pid_s & pidParams, const double *params) {
|
|
LMSFunction<4> *plant;
|
|
if (methodOrder == 1)
|
|
plant = (LMSFunction<4> *)&plant1;
|
|
else
|
|
plant = &plant2;
|
|
|
|
PidParallelController pid(pidParams);
|
|
//PidDerivativeFilterController pid(pidParams, 10);
|
|
|
|
plantInput.reset();
|
|
// guess a previous state to minimize the system "shock"
|
|
plantInput.addDataPoint((float)(getSetpoint(0) / params[PARAM_K]) - pidParams.offset);
|
|
// "calm down" the PID controller to avoid huge spikes at the beginning
|
|
pid.getOutput(getSetpoint(0), plant->getEstimatedValueAtPoint(0, params), dTime);
|
|
|
|
stepPoint = maxPoints / 2;
|
|
|
|
metric.reset();
|
|
|
|
// simulate over time
|
|
for (int i = 0; i < numPoints; i++) {
|
|
// make a step in the middle
|
|
double target = getSetpoint(i);
|
|
// "measure" the current value of the plant
|
|
double pidInput = plant->getEstimatedValueAtPoint(i, params) + getLoadDisturbance(i);
|
|
// wait for the controller reaction
|
|
double pidOutput = pid.getOutput(target, pidInput, dTime);
|
|
|
|
// apply the reaction to the plant's pidInput
|
|
plantInput.addDataPoint((float)pidOutput);
|
|
// don't take into account any start-up transients, we're interested only in our step response!
|
|
if (i >= stepPoint)
|
|
metric.addPoint((double)i / (double)numPoints, pidInput, target);
|
|
#ifdef PID_DEBUG
|
|
if (outputFile != nullptr)
|
|
output_csv(outputFile, (double)i, pidOutput, target, pidInput);
|
|
#endif
|
|
}
|
|
|
|
return metric;
|
|
}
|
|
|
|
double getSetpoint(int i) const {
|
|
switch (simType) {
|
|
case PID_SIM_SERVO:
|
|
return (i > stepPoint) ? target2 : target1;
|
|
default:
|
|
// we want to be in the middle for safety
|
|
return (target1 + target2) / 2.0;
|
|
}
|
|
}
|
|
|
|
double getLoadDisturbance(int i) const {
|
|
static const double disturb = 0.10;
|
|
static const double ampl = 0.25, period = 0.37;
|
|
double d = 0;
|
|
switch (simType) {
|
|
case PID_SIM_REGULATOR:
|
|
// add or subtract 10% to imitate the "load"
|
|
d += target1 * ((i > stepPoint) ? -disturb : disturb);
|
|
// add periodic noise
|
|
d += sin(2.0 * 3.14159265 * (double)i * dTime / period) * ampl;
|
|
return d;
|
|
default:
|
|
return 0.0;
|
|
}
|
|
}
|
|
|
|
protected:
|
|
int methodOrder;
|
|
int stepPoint;
|
|
const char *outputFile;
|
|
StoredDataInputFunction<maxPoints> plantInput;
|
|
FirstOrderPlusDelayLineFunction plant1;
|
|
SecondOrderPlusDelayLineOverdampedFunction plant2;
|
|
PidAccuracyMetric metric;
|
|
double target1, target2, dTime;
|
|
pid_sim_type_e simType;
|
|
};
|
|
|
|
|
|
// A working copy of simulator and PID controller params. Used by PidAutoTune::solveModel().
|
|
// We don't want to create a simulator instance each time we call getEstimatedValueAtPoint() from PidCoefsFinderFunction.
|
|
// So we create an instance of this class and store temporary allocated data.
|
|
template <int numPoints>
|
|
class PidSimulatorFactory {
|
|
public:
|
|
PidSimulatorFactory(pid_sim_type_e simType, int methodOrder_, double target1_, double target2_, double dTime, double modelBias, const pid_s & pid_) :
|
|
sim(simType, methodOrder_, target1_, target2_, dTime, modelBias, nullptr), pid(pid_) {
|
|
}
|
|
|
|
public:
|
|
PidSimulator<numPoints> sim;
|
|
pid_s pid;
|
|
};
|
|
|
|
|
|
// A special function used for PID controller analytic simulation
|
|
// This method doesn't use any "magic" formulas, instead it uses Levenberg-Marquardt solver as a "brute-force".
|
|
template <int numPoints>
|
|
class PidCoefsFinderFunction : public LMSFunction<numParamsForPid> {
|
|
public:
|
|
PidCoefsFinderFunction(PidSimulatorFactory<numPoints> *simFactory_, const double *modelParams_) :
|
|
simFactory(simFactory_), modelParams(modelParams_) {
|
|
}
|
|
|
|
virtual void justifyParams(double *params) const {
|
|
// todo: limit PID coefs somehow?
|
|
}
|
|
|
|
// Get the total number of data points
|
|
virtual int getNumPoints() const {
|
|
return numPoints;
|
|
}
|
|
|
|
// Calculate the sum of the squares of the residuals
|
|
virtual double calcMerit(double *params) const {
|
|
return simulate(numPoints - 1, params).getItae();
|
|
}
|
|
|
|
virtual double getResidual(int i, const double *params) const {
|
|
return simFactory->sim.getSetpoint(i) - getEstimatedValueAtPoint(i, params);
|
|
}
|
|
|
|
virtual double getEstimatedValueAtPoint(int i, const double *params) const {
|
|
return simulate(i, params).getLastValue();
|
|
}
|
|
|
|
PidAccuracyMetric simulate(int i, const double *params) const {
|
|
// update params
|
|
simFactory->pid.pFactor = (float)params[PARAM_Kp];
|
|
simFactory->pid.iFactor = (float)params[PARAM_Ki];
|
|
simFactory->pid.dFactor = (float)params[PARAM_Kd];
|
|
// simulate PID controller response
|
|
return simFactory->sim.simulate(i + 1, simFactory->pid, modelParams);
|
|
}
|
|
|
|
protected:
|
|
const double *modelParams;
|
|
PidSimulatorFactory<numPoints> *simFactory;
|
|
};
|
|
|