Radzhab Bisultanov
0faafbf089
*Чтение IMU и обработка его данных выполняется в точности как в рабочей прошивке. *Определение вращения работает корректно.
23 lines
453 B
C++
23 lines
453 B
C++
#include "PID.h"
|
|
|
|
float pid_update(pid_t* pid, float error, float gyro_rate, float dt)
|
|
{
|
|
float p = pid->kp * error;
|
|
|
|
pid->integral += error * dt;
|
|
if (pid->integral > 100) pid->integral = 100;
|
|
if (pid->integral < -100) pid->integral = -100;
|
|
|
|
float i = pid->ki * pid->integral;
|
|
|
|
float d = - pid->kd * gyro_rate;
|
|
|
|
pid->prev_error = error;
|
|
|
|
float spid = p + i + d;
|
|
|
|
if (spid > 300) spid = 300;
|
|
if (spid < -300) spid = -300;
|
|
|
|
return spid;
|
|
} |