kopia lustrzana https://github.com/Schildkroet/GRBL-Advanced
162 wiersze
2.9 KiB
C
162 wiersze
2.9 KiB
C
#include "PID.h"
|
|
#include "System32.h"
|
|
|
|
#define TICK_SECOND 1000
|
|
|
|
|
|
extern uint32_t millis(void);
|
|
|
|
|
|
PID_t *PID_Create(PID_t *pid, float* in, float* out, float* set, float kp, float ki, float kd)
|
|
{
|
|
pid->input = in;
|
|
pid->output = out;
|
|
pid->setpoint = set;
|
|
pid->automode = false;
|
|
|
|
PID_Limits(pid, -255, 255);
|
|
|
|
// Set default sample time to 25 ms
|
|
pid->sampletime = 16 * (TICK_SECOND / 1000);
|
|
|
|
PID_Direction(pid, E_PID_DIRECT);
|
|
PID_Tune(pid, kp, ki, kd);
|
|
|
|
pid->lasttime = millis() - pid->sampletime;
|
|
|
|
return pid;
|
|
}
|
|
|
|
|
|
void PID_Compute(PID_t *pid)
|
|
{
|
|
// Check if control is enabled
|
|
if (!pid->automode)
|
|
return;
|
|
|
|
float in = *(pid->input);
|
|
|
|
// Compute error
|
|
float error = (*(pid->setpoint)) - in;
|
|
|
|
// Compute integral
|
|
pid->iterm += (pid->Ki * error);
|
|
if (pid->iterm > pid->omax)
|
|
pid->iterm = pid->omax;
|
|
else if (pid->iterm < pid->omin)
|
|
pid->iterm = pid->omin;
|
|
|
|
// Compute differential on input
|
|
float dinput = in - pid->lastin;
|
|
|
|
// Compute PID output
|
|
float out = pid->Kp * error + pid->iterm - pid->Kd * dinput;
|
|
|
|
// Apply limit to output value
|
|
if (out > pid->omax)
|
|
out = pid->omax;
|
|
else if (out < pid->omin)
|
|
out = pid->omin;
|
|
|
|
// Output to pointed variable
|
|
(*pid->output) = out;
|
|
|
|
// Keep track of some variables for next execution
|
|
pid->lastin = in;
|
|
pid->lasttime = millis();;
|
|
}
|
|
|
|
|
|
void PID_Tune(PID_t *pid, float kp, float ki, float kd)
|
|
{
|
|
// Check for validity
|
|
if (kp < 0 || ki < 0 || kd < 0)
|
|
return;
|
|
|
|
//Compute sample time in seconds
|
|
float ssec = ((float) pid->sampletime) / ((float) TICK_SECOND);
|
|
|
|
pid->Kp = kp;
|
|
pid->Ki = ki * ssec;
|
|
pid->Kd = kd / ssec;
|
|
|
|
if (pid->direction == E_PID_REVERSE)
|
|
{
|
|
pid->Kp = 0 - pid->Kp;
|
|
pid->Ki = 0 - pid->Ki;
|
|
pid->Kd = 0 - pid->Kd;
|
|
}
|
|
}
|
|
|
|
|
|
void PID_SampleTime(PID_t *pid, uint32_t time)
|
|
{
|
|
if (time > 0)
|
|
{
|
|
float ratio = (float) (time * (TICK_SECOND / 1000)) / (float) pid->sampletime;
|
|
pid->Ki *= ratio;
|
|
pid->Kd /= ratio;
|
|
pid->sampletime = time * (TICK_SECOND / 1000);
|
|
}
|
|
}
|
|
|
|
|
|
void PID_Limits(PID_t *pid, float min, float max)
|
|
{
|
|
if (min >= max)
|
|
return;
|
|
|
|
pid->omin = min;
|
|
pid->omax = max;
|
|
|
|
//Adjust output to new limits
|
|
if (pid->automode)
|
|
{
|
|
if (*(pid->output) > pid->omax)
|
|
*(pid->output) = pid->omax;
|
|
else if (*(pid->output) < pid->omin)
|
|
*(pid->output) = pid->omin;
|
|
|
|
if (pid->iterm > pid->omax)
|
|
pid->iterm = pid->omax;
|
|
else if (pid->iterm < pid->omin)
|
|
pid->iterm = pid->omin;
|
|
}
|
|
}
|
|
|
|
|
|
void PID_EnableAuto(PID_t *pid)
|
|
{
|
|
// If going from manual to auto
|
|
if (!pid->automode)
|
|
{
|
|
pid->iterm = *(pid->output);
|
|
pid->lastin = *(pid->input);
|
|
|
|
if (pid->iterm > pid->omax)
|
|
pid->iterm = pid->omax;
|
|
else if (pid->iterm < pid->omin)
|
|
pid->iterm = pid->omin;
|
|
|
|
pid->automode = true;
|
|
}
|
|
}
|
|
|
|
|
|
void PID_Manual(PID_t *pid)
|
|
{
|
|
pid->automode = false;
|
|
}
|
|
|
|
|
|
void PID_Direction(PID_t *pid, enum pid_control_directions dir)
|
|
{
|
|
if (pid->automode && pid->direction != dir)
|
|
{
|
|
pid->Kp = (0 - pid->Kp);
|
|
pid->Ki = (0 - pid->Ki);
|
|
pid->Kd = (0 - pid->Kd);
|
|
}
|
|
pid->direction = dir;
|
|
}
|