[DemoCode下载]

M4利用DSP内核实现PID算法

[复制链接]
2063|7
手机看帖
扫描二维码
随时随地手机跟帖
xinpian101|  楼主 | 2018-3-8 09:25 | 显示全部楼层 |阅读模式
例程序演示如何使用PID控制器运算。首先用arm_pid_instance_f32宣告矩阵变量,再来对PID初始化设定。目标参考值为in输入arm_pid_f32()进行运算,得到进行PID后所输出值,再把第一次输出值与目标参考值相减为误差,将其误差输入PID进行第二次运算,以此进行下去输出值即可与目标参考值相近,其执行流程如下图。
用户可以直接使用这些函式,来实现自己的数学方程式运算。程序内也比较了有无使用DSP计算时间的差异,使用者把#define USE_DSP给取消即可不使用DSP来运算这些方程式。
QQ截图20180308092514.png

xinpian101|  楼主 | 2018-3-8 09:26 | 显示全部楼层
/****************************************************************************
* [url=home.php?mod=space&uid=288409]@file[/url]     main.c
* [url=home.php?mod=space&uid=895143]@version[/url]  V1.0
* $Date: 15/09/02 10:04a $
* @brief
*         Display how to use M4 DSP PID Controller
*                      and compare with calcultion without DSP
* @note
* Copyright (C) 2016 Nuvoton Technology Corp. All rights reserved.
*
******************************************************************************/
#include <stdio.h>
#include "M451Series.h"
#include "arm_math.h"
#define USE_DSP
int i,j,CalTime;
arm_pid_instance_f32 PIDS;
float32_t output[100],ee;
float A0,A1,A2,state[3],Kp=0.4,Ki=0.4,Kd=0,target,ival;
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);
}
void SYS_Init(void)
{
    /*---------------------------------------------------------------------------------------------------------*/
    /* Init System Clock                                                                                       */
    /*---------------------------------------------------------------------------------------------------------*/
    /* Enable HIRC clock */
    CLK_EnableXtalRC(CLK_PWRCTL_HIRCEN_Msk);

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

    /* Switch HCLK clock source to HIRC */
    CLK_SetHCLK(CLK_CLKSEL0_HCLKSEL_HIRC, CLK_CLKDIV0_HCLK(1));

    /* Enable HXT */
    CLK_EnableXtalRC(CLK_PWRCTL_HXTEN_Msk);

    /* Waiting for clock ready */
    CLK_WaitClockReady(CLK_STATUS_HXTSTB_Msk);

    /* Set core clock as PLL_CLOCK from PLL and SysTick source to HCLK/2*/
    CLK_SetCoreClock(72000000);
    CLK_SetSysTickClockSrc(CLK_CLKSEL0_STCLKSEL_HCLK_DIV2);

    /* Enable peripheral clock */
    CLK_EnableModuleClock(UART0_MODULE);
    CLK_EnableModuleClock(TMR0_MODULE);

    /* Peripheral clock source */
    CLK_SetModuleClock(UART0_MODULE, CLK_CLKSEL1_TMR0SEL_HXT, CLK_CLKDIV0_UART(1));
    CLK_SetModuleClock(TMR0_MODULE, CLK_CLKSEL1_TMR0SEL_HXT, 0);

}
void UART_Init(void)
{
    /* Set PD multi-function pins for UART0 RXD, TXD */
    SYS->GPD_MFPL = SYS_GPD_MFPL_PD0MFP_UART0_RXD | SYS_GPD_MFPL_PD1MFP_UART0_TXD;
    /* Reset UART module */
    SYS_ResetModule(UART0_RST);

    /* Configure UART0 and set UART0 Baudrate */
    UART_Open(UART0, 115200);
}

/*---------------------------------------------------------------------------------------------------------*/
/*  Main Function                                                                                          */
/*---------------------------------------------------------------------------------------------------------*/
int32_t main(void)
{               
          uint32_t i;
                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();
               
                TIMER_Open(TIMER0, TIMER_CONTINUOUS_MODE, 1);
                TIMER_Start(TIMER0);
#ifdef USE_DSP
                /* 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);
                        //printf("%0.2f\n",output[i]);
                        /* Update error */
                        ee=target-output[i-1];
                }
#else
               
                for(i=1;i<100;i++)
                {
                        output[i]=PID(ee);
                        ee=target-output[i-1];
                }
               
#endif
          TIMER_Close(TIMER0);
                CalTime=TIMER_GetCounter(TIMER0);       
                printf("time is %d \n",CalTime);

    while(1);                             /* main function does not return */
}

使用特权

评论回复
xinpian101|  楼主 | 2018-3-8 10:50 | 显示全部楼层
是不是很好用

使用特权

评论回复
dongnanxibei| | 2018-3-8 15:21 | 显示全部楼层
这个参数有默认值吗

使用特权

评论回复
doit888| | 2018-3-9 13:27 | 显示全部楼层
请问 如果实现一个 多通道的 PID

比如 8个通道的 该怎么做呢?

谢谢!

使用特权

评论回复
doit888| | 2018-3-9 13:28 | 显示全部楼层
一般的 PID 采样周期 是多少?

计算周期又是多少呢?

使用特权

评论回复
doit888| | 2018-3-9 13:37 | 显示全部楼层
如果说 自平衡小车 一类的 是200HZ 的运算速度,那么采集MPU6050的速度 1KHZ 足够了吧?

使用特权

评论回复
yiyigirl2014| | 2018-3-10 08:44 | 显示全部楼层
doit888 发表于 2018-3-9 13:37
如果说 自平衡小车 一类的 是200HZ 的运算速度,那么采集MPU6050的速度 1KHZ 足够了吧? ...

肯定够了

使用特权

评论回复
发新帖 我要提问
您需要登录后才可以回帖 登录 | 注册

本版积分规则

93

主题

1394

帖子

1

粉丝