|
|
@ -13,8 +13,8 @@ static void clampOutput(struct PID *self) |
|
|
|
|
|
|
|
|
|
|
|
static void clampIterm(struct PID *self) |
|
|
|
static void clampIterm(struct PID *self) |
|
|
|
{ |
|
|
|
{ |
|
|
|
if (self->ITerm > self->outMax) { self->ITerm = self->outMax; } |
|
|
|
if (self->ITerm > self->iTermMax) { self->ITerm = self->iTermMax; } |
|
|
|
else if (self->ITerm < self->outMin) { self->ITerm = self->outMin; } |
|
|
|
else if (self->ITerm < self->iTermMin) { self->ITerm = self->iTermMin; } |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void PID_Compute(struct PID *self, float input) |
|
|
|
void PID_Compute(struct PID *self, float input) |
|
|
@ -32,6 +32,11 @@ void PID_Compute(struct PID *self, float input) |
|
|
|
|
|
|
|
|
|
|
|
clampIterm(self); |
|
|
|
clampIterm(self); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Shortcut to reduce overshoot
|
|
|
|
|
|
|
|
if (error < 0 && self->ITerm > 0) { |
|
|
|
|
|
|
|
self->ITerm = 0; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
float dInput = (input - self->lastInput); |
|
|
|
float dInput = (input - self->lastInput); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
@ -91,6 +96,14 @@ void PID_SetOutputLimits(struct PID *self, float min, float max) |
|
|
|
self->outMax = max; |
|
|
|
self->outMax = max; |
|
|
|
|
|
|
|
|
|
|
|
clampOutput(self); |
|
|
|
clampOutput(self); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void PID_SetITermLimits(struct PID *self, float min, float max) |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
if (min > max) { return; } |
|
|
|
|
|
|
|
self->iTermMin = min; |
|
|
|
|
|
|
|
self->iTermMax = max; |
|
|
|
|
|
|
|
|
|
|
|
clampIterm(self); |
|
|
|
clampIterm(self); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|