57 lines
2.0 KiB
C
57 lines
2.0 KiB
C
|
/*
|
|||
|
|
* <EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD>: pid.c
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD>debug<EFBFBD>汾<EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>: MichaelKing
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>: 2024-06-26
|
|||
|
|
* <EFBFBD>汾: 1.0
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>뻷<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>pid<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*
|
|||
|
|
* <EFBFBD><EFBFBD>ʷ<EFBFBD><EFBFBD>¼:
|
|||
|
|
* <EFBFBD>汾 1.0 - 2024-06-26 - MichaelKing
|
|||
|
|
* <EFBFBD><EFBFBD>ʼ<EFBFBD>汾<EFBFBD><EFBFBD>
|
|||
|
|
*/
|
|||
|
|
#include "pid.h"
|
|||
|
|
|
|||
|
|
pid pid_angle; // <20><><EFBFBD><EFBFBD><EFBFBD>ǶȻ<C7B6>pid
|
|||
|
|
pid pid_speed; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶȻ<D9B6>pid
|
|||
|
|
pid pid_current; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>pid
|
|||
|
|
|
|||
|
|
/**********************
|
|||
|
|
PID<EFBFBD><EFBFBD><EFBFBD>ú<EFBFBD><EFBFBD><EFBFBD>:
|
|||
|
|
**********************/
|
|||
|
|
void PID_init() // <20><>ʼ<EFBFBD><CABC>pid<69><64><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
// λ<>û<EFBFBD>pid
|
|||
|
|
pid_angle.Set = 0.0f; // <20><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>ֵ<EFBFBD><D6B5>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_angle.Actual = 0.0f; // ʵ<><CAB5>ֵ<EFBFBD><D6B5>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_angle.err = 0.0f; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_angle.err_last = 0.0f; // <20>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_angle.voltage = 0.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_angle.integral = 0.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_angle.Kp = 0.01f; // λ<>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0.043
|
|||
|
|
pid_angle.Ki = 0.0005f; // λ<>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0.0005
|
|||
|
|
pid_angle.Kd = 0.25f; // λ<>û<EFBFBD><EFBFBD><CEA2>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0.22
|
|||
|
|
|
|||
|
|
// <20>ٶȻ<D9B6>pid
|
|||
|
|
pid_speed.Set = 0.0f; // <20><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>ֵ<EFBFBD><D6B5>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_speed.Actual = 0.0f; // ʵ<><CAB5>ֵ<EFBFBD><D6B5>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_speed.err = 0.0f; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_speed.err_last = 0.0f; // <20>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_speed.voltage = 0.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_speed.integral = 0.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_speed.Kp = 0.01f; // <20>ٶȻ<D9B6><C8BB><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0.11
|
|||
|
|
pid_speed.Ki = 0.0001f; // <20>ٶȻ<D9B6><C8BB><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0.005
|
|||
|
|
pid_speed.Kd = 0.00001f; // <20>ٶȻ<D9B6><EFBFBD><CEA2>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0.0008
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>pid
|
|||
|
|
pid_current.Set = 0.0f; // <20><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>ֵ<EFBFBD><D6B5>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_current.Actual = 0.0f; // ʵ<><CAB5>ֵ<EFBFBD><D6B5>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_current.err = 0.0f; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_current.err_last = 0.0f;// <20>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_current.voltage = 0.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_current.integral = 0.0f;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0
|
|||
|
|
pid_current.Kp = 0.045f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0.03
|
|||
|
|
pid_current.Ki = 0.005f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0.001
|
|||
|
|
pid_current.Kd = 0.000f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><CEA2>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ0.0008
|
|||
|
|
}
|