/****************************************************************************
* [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 */
}
| 
楼主
标题置顶
标题高亮
