template <class dataType, class floatType> void Pid<dataType, floatType>::Init(
dataType kp,
dataType ki,
dataType kd,
ctrlDir_t controllerDir,
outputMode_t outputMode,
floatType samplePeriodMs,
dataType minOutput,
dataType maxOutput,
dataType setPoint)
{
SetOutputLimits(minOutput, maxOutput);
this->samplePeriodMs = samplePeriodMs;
SetControllerDirection(controllerDir);
this->outputMode = outputMode;
// Set tunings with provided constants
SetTunings(kp, ki, kd);
this->setPoint = setPoint;
prevInput = 0;
prevOutput = 0;
pTerm = floatType(0.0);
iTerm = floatType(0.0);
dTerm = floatType(0.0);
}
void setup() {
pinMode(4, INPUT); //set the second encoder channel up
attachInterrupt(0, integrate, RISING);
Serial.begin(9600);
digitalWrite(motorEnablePin, LOW); //safety
digitalWrite(motorDirectionPin, LOW); //safety
digitalWrite(motorDrivePin, HIGH); //safety
//pid setup
SetMode(AUTOMATIC); //auto PID mode
SetTunings(1, 0, 0);
SetOutputLimits(-3, 3); //+-24V maximum
SetSampleTime(5); //something smaller than the loop delay
Setpoint = 12.56;
digitalWrite(motorEnablePin, HIGH); //let it go
}
QuadPID::QuadPID(float kp, float ki, float kd, float *input, float *output, float *setpoint, MilliSec_t sample_period)
{
// pointeurs, entrées, sorties consignes
Input = input;
Output = output;
Setpoint = setpoint;
// période de calcule par défaut 50Hz : 20ms
DefaultSamplePeriod = SamplePeriod = sample_period;
// mode automatique (par défaut)
Mode = QUADPID_AUTOMATIC;
// mise en place des paramètres
SetTunings(kp, ki, kd);
SetDirection(QUADPID_DIRECT);
SetOutputLimits(0, 255);
}