mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-05-01 19:56:44 +00:00
47 lines
1.2 KiB
C
47 lines
1.2 KiB
C
#include "common.h"
|
|
|
|
void diffTime(const struct timespec* debut, const struct timespec* fin, struct timespec* ecoule)
|
|
{
|
|
if ((fin->tv_nsec - debut->tv_nsec) < 0) {
|
|
ecoule->tv_sec = fin->tv_sec - debut->tv_sec - 1;
|
|
ecoule->tv_nsec = fin->tv_nsec - debut->tv_nsec + 1000000000UL;
|
|
} else {
|
|
ecoule->tv_sec = fin->tv_sec - debut->tv_sec;
|
|
ecoule->tv_nsec = fin->tv_nsec - debut->tv_nsec;
|
|
}
|
|
}
|
|
|
|
float diffTimeSec(const struct timespec* debut, const struct timespec* fin)
|
|
{
|
|
struct timespec ecoule;
|
|
diffTime(debut, fin, &ecoule);
|
|
return ecoule.tv_sec + ecoule.tv_nsec * 1E-9;
|
|
}
|
|
|
|
void resetPID(struct PID *pid)
|
|
{
|
|
clock_gettime(CLOCK_REALTIME, &pid->lastCalc);
|
|
pid->prevErr = 0;
|
|
pid->integErr = 0;
|
|
}
|
|
|
|
void initPID(struct PID *pid, float KP, float KI, float KD)
|
|
{
|
|
pid->KP = KP;
|
|
pid->KI = KI;
|
|
pid->KD = KD;
|
|
resetPID(pid);
|
|
}
|
|
|
|
float updatePID(struct PID *pid, float err)
|
|
{
|
|
struct timespec now;
|
|
clock_gettime(CLOCK_REALTIME, &now);
|
|
float dt = diffTimeSec(&pid->lastCalc, &now);
|
|
pid->integErr += (err + pid->prevErr) / 2 * dt;
|
|
float derivErr = (err - pid->prevErr) / dt;
|
|
pid->prevErr = err;
|
|
return pid->KP * err + pid->KI * pid->integErr + pid->KP * derivErr;
|
|
}
|
|
|