from: https://eroro.tistory.com/500
\Drivers\CMSIS\DSP_Lib\Source\ControllerFunctions
안에 arm_pid_init_f32.c 파일을 Merge 시키거나 arm_math 파일만을 묶어서 라이브러리로 만들어 추가해도 됩니다.
실제 사용시에는 다양한 값을 받아서 PID 제어를 하면 될 듯 싶습니다.
조금 더 부드럽게 값을 만져줄 수 있어 몇개의 Factor을 받아 2중 PID 구조를 사용하였습니다.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 |
#include "arm_math.h" /* Private typedef -----------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/ /* Private macro ------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ arm_pid_instance_f32 PID_Acc; arm_pid_instance_f32 PID_Gyro; /* ACC */ #define PID_PARAM_ACC_P 2 /* Proporcional */ #define PID_PARAM_ACC_I 0.005 /* Integral */ #define PID_PARAM_ACC_D 0.02 /* Derivative */ /* Private function prototypes ------------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ void PID_ACCInit(void); /******************************************************************************* * Function Name : * Parameters : None * Return : None * Description : *******************************************************************************/ void PID_Init(void) { PID_ACCInit(); } /******************************************************************************* * Function Name : * Description : * Parameters : None * Return : None * Description : Gyroscope Sensor *******************************************************************************/ void PID_ACCInit(void) { /* Set PID parameters */ /* Set this for your needs */ PID_Acc.Kp = PID_PARAM_ACC_P; /* Proporcional */ PID_Acc.Ki = PID_PARAM_ACC_I; /* Integral */ PID_Acc.Kd = PID_PARAM_ACC_D; /* Derivative */ /* Initialize PID system, float32_t format */ arm_pid_init_f32(&PID_Acc, 1); } /******************************************************************************* * Function Name : * Parameters : None * Return : None * Description : *******************************************************************************/ void PID_ACCControl(const float a_fTargetValue, const float a_fCurrentValue) { float pid_error = 0; /* Duty cycle for PWM */ float duty; pid_error = a_fTargetValue - a_fCurrentValue; /* Calculate PID here, argument is error */ /* Output data will be returned, we will use it as duty cycle parameter */ duty = 0; duty = arm_pid_f32(&PID_Acc, pid_error); /* Check overflow, duty cycle in percent */ if (duty >= ACCHIGHLIMIT) {duty = ACCHIGHLIMIT;} else if (duty <= ACCLOWLIMIT) {duty = ACCLOWLIMIT;} PWM(duty); } |