M451 Series:DSP PID Controller

只看该作者 · 2021-6-6 15:27

M451 Series:DSP PID Controller

Proportional--integral--derivative (PID) controllers have been the backbone of motion and
process control for decades. The document shows the performance difference between CPU
calling software PID function and DSP PID library.
The main system configuration of the M451 MCU is as follows:
 System clock frequency is 72 MHz
 Clock source is from PLL, and PLL source is from HXT
 Using Timer0’s counter to compare the difference of performance

A Proportional Integral Derivative (PID) controller is a generic feedback control loop
mechanism widely used in industrial control systems. A PID controller is the most commonly
used type of feedback controller.
Algorithm:
y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]
A0 = Kp + Ki + Kd
A1 = (-Kp ) - (2 * Kd )
A2 = Kd
where Kp is proportional constant, Ki is Integral constant and Kd is Derivative constant

只看该作者 · 2021-6-6 15:28

只看该作者 · 2021-6-6 15:28

只看该作者 · 2021-6-6 15:29

/* Variables for DSP PID */
arm_pid_instance_f32 PIDS;

/* Variables for software PID */
float A0, A1, A2, state[3], Kp = 0.4, Ki = 0.4, Kd = 0, target, ival;

/* Variables for DSP and Software PID */
float output[100], ee;

float PID(float in)
{
float out;

A0 = Kp + Ki;
A1 = -Kp - (2 * Kd);
A2 = Kd;

/* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2] */
out = (A0 * in) + (A1 * state[0]) + (A2 * state[1]) + (state[2]);

/* Update state */
state[1] = state[0];
state[0] = in;
state[2] = out;

/* return to application */
return (out);
}

只看该作者 · 2021-6-6 15:31

void SYS_Init(void)
{
/*---------------------------------------------------------------------------------------------------------*/
/* Init System Clock */
/*---------------------------------------------------------------------------------------------------------*/

/* Enable HIRC clock (Internal RC 22.1184MHz) */
CLK_EnableXtalRC(CLK_PWRCTL_HIRCEN_Msk);

/* Wait for HIRC clock ready */
CLK_WaitClockReady(CLK_STATUS_HIRCSTB_Msk);

/* Select HCLK clock source as HIRC and and HCLK source divider as 1 */
CLK_SetHCLK(CLK_CLKSEL0_HCLKSEL_HIRC, CLK_CLKDIV0_HCLK(1));

/* Set PLL to Power-down mode and PLLSTB bit in CLK_STATUS register will be cleared by hardware.*/
CLK_DisablePLL();

/* Enable HXT clock (external XTAL 12MHz) */
CLK_EnableXtalRC(CLK_PWRCTL_HXTEN_Msk);

/* Wait for HXT clock ready */
CLK_WaitClockReady(CLK_STATUS_HXTSTB_Msk);

/* Set core clock as PLL_CLOCK from PLL */
CLK_SetCoreClock(PLL_CLOCK);

/* Enable UART module clock */
CLK_EnableModuleClock(UART0_MODULE);

CLK_EnableModuleClock(TMR0_MODULE);

/* Peripheral clock source */

/* Select UART module clock source as HXT and UART module clock divider as 1 */
CLK_SetModuleClock(UART0_MODULE, CLK_CLKSEL1_UARTSEL_HXT, CLK_CLKDIV0_UART(1));

CLK_SetModuleClock(TMR0_MODULE, CLK_CLKSEL1_TMR0SEL_HXT, 0);

/*---------------------------------------------------------------------------------------------------------*/
/* Init I/O Multi-function */
/*---------------------------------------------------------------------------------------------------------*/

/* Set PD multi-function pins for UART0 RXD and TXD */
SYS->GPD_MFPL &= ~(SYS_GPD_MFPL_PD0MFP_Msk | SYS_GPD_MFPL_PD1MFP_Msk);
SYS->GPD_MFPL |= (SYS_GPD_MFPL_PD0MFP_UART0_RXD | SYS_GPD_MFPL_PD1MFP_UART0_TXD);

}

只看该作者 · 2021-6-6 15:31

void UART_Init(void)
{
/*---------------------------------------------------------------------------------------------------------*/
/* Init UART */
/*---------------------------------------------------------------------------------------------------------*/
UART_Open(UART0, 115200);
}

只看该作者 · 2021-6-6 15:32

/*---------------------------------------------------------------------------------------------------------*/
/*  Main Function                                                                                        */
/*---------------------------------------------------------------------------------------------------------*/
int32_t main(void)
{
uint32_t i, CalTime;

PIDS.Kp = 0.4;
PIDS.Ki = 0.4;
PIDS.Kd = 0;
/* Target value*/
target = 500;
/* Inital value */
ival = 0;
/* Initial value and target value error */
ee = target - ival;

/* Unlock protected registers */
SYS_UnlockReg();

/* Init System, peripheral clock and multi-function I/O */
SYS_Init();

/* Lock protected registers */
SYS_LockReg();

/* Init UART for printf */
UART_Init();

/* Init TIMER0 for performance comparing */
TIMER_Open(TIMER0, TIMER_CONTINUOUS_MODE, 1);

TIMER_Start(TIMER0);

/************************* DSP PID ******************************/
/* Initial DSP PID controller function*/
arm_pid_init_f32(&PIDS,0);

/* Calculate PID controller function 100 times*/
for(i = 1; i < 100; i++)
{
      output[i] = arm_pid_f32(&PIDS,ee);
      ee = target-output[i-1];
}

TIMER_Close(TIMER0);
CalTime = TIMER_GetCounter(TIMER0);
printf("\nDSP PID: It took %d HXT clocks\n", CalTime);

/********************** Software PID ****************************/
/* Re-Initialization TIMER0 for performance comparing */
TIMER_Open(TIMER0, TIMER_CONTINUOUS_MODE, 1);

TIMER_Start(TIMER0);

/* Calculate PID controller function 100 times*/
for(i = 1; i < 100; i++)
{
      output[i] = PID(ee);
      ee = target-output[i-1];
}

TIMER_Close(TIMER0);
CalTime = TIMER_GetCounter(TIMER0);
printf("Software PID: It took %d HXT clocks\n", CalTime);

while(1);
}

只看该作者 · 2021-6-6 15:32

对于PID 计算方式，应该是有增量式和位置式两种方式。

看这个例程，也不是十分清楚。。。

1万 · 2021-6-6 21:00

其实串口的初始化，最好是把时钟配置相关的也放到对应的初始化函数内。

只看该作者 · 2021-6-6 23:04

PID一般用于电机控制方面吧，除了电机控制方面还会用在哪儿呀？

只看该作者 · 2021-6-8 07:56

734774645 发表于 2021-6-6 21:00
其实串口的初始化，最好是把时钟配置相关的也放到对应的初始化函数内。

/* Unlock protected registers */
SYS_UnlockReg();

/* Init System, peripheral clock and multi-function I/O */
SYS_Init();

/* Lock protected registers */
SYS_LockReg();

关键是这样的话，岂不是要写多次 LOCK REG？

会不会有问题？