PID 제어

\Drivers\CMSIS\DSP_Lib\Source\ControllerFunctions

안에 arm_pid_init_f32.c 파일을 Merge 시키거나 arm_math 파일만을 묶어서 라이브러리로 만들어 추가해도 됩니다.

실제 사용시에는 다양한 값을 받아서 PID 제어를 하면 될 듯 싶습니다.

조금 더 부드럽게 값을 만져줄 수 있어 몇개의 Factor을 받아 2중 PID 구조를 사용하였습니다. 

#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);

}