?? pi.c
字號:
/*
***********************************************************************
* For test BLDC
* 2008-1-7
***********************************************************************
*/
//=============================================//
//Include File
//=============================================//
#include "Spmc75f2313a.h"
#include "Spmc_typedef.h"
#include "config.h"
static PID sPID;
PID *sptr = &sPID;
static CONTROLSM sCtrlSM;
static CONTROLSM *sCSptr = &sCtrlSM;
UInt16 cnt[200];
UInt16 *cntptr=cnt;
UInt8 step;
UInt16 CountEn,Count;
static unsigned int uiFilter[FILTERLEN]={0};
static unsigned int *ptr = uiFilter;
static long summation = 0;
extern UInt16 adcnt;
extern UInt16 iav;
int motorposition = 1;
//=============================================//
//120度方波控制信號PWM加入方式選擇及正反轉選擇
//=============================================//
#define Turnaround 6
/*
0 －－ 正轉、上相PWM120度方波
1 －－ 反轉、上相PWM120度方波
2 －－ 正轉、下相PWM120度方波
3 －－ 反轉、下相PWM120度方波
4 －－ 正轉、前半相PWM120度方波
5 －－ 反轉、前半相PWM120度方波
6 －－ 正轉、后半相PWM120度方波
7 －－ 反轉、后半相PWM120度方波
*/
//=============================================//
//=============================================//
/*
**********************************************************************
*
* PI init
*
**********************************************************************
*/
void DK_motor_init(void)
{
sCSptr->B._direction = CCW;
CountEn = DISABLE;
}
/*
**********************************************************************
*
* start pro
*
**********************************************************************
*/
void DK_start()
{
IncPIDInit();
sptr->SetPoint = 256;
sptr->LowRange = 200;
sptr->HighRange = 500;
sCSptr->B._startup = ENABLE;
/*if(sCSptr->B._direction)
BLDC_Test4_NU120FullPWM(P_POS1_DectData->W);
else
BLDC_Test4_PU120FullPWM(P_POS1_DectData->W);*/
//P_TMR_Start->B.TMR4ST |= CB_TMR_TMR4ST_Start;
}
void DK_stop()
{
sCSptr->B._startup = DISABLE;
//P_TMR_Start->B.TMR4ST &= (~CB_TMR_TMR4ST_Start);
P_TMR4_OutputCtrl->W = CW_TMR4_POLP_Active_High | \
CW_TMR4_UPWM_Out_HL | CW_TMR4_UOC_Mode1 |\
CW_TMR4_VPWM_Out_HL | CW_TMR4_VOC_Mode1 |\
CW_TMR4_WPWM_Out_HL | CW_TMR4_WOC_Mode1;
F_Delay(10);
P_TMR4_OutputCtrl->W = CW_TMR4_POLP_Active_High | \
CW_TMR4_UPWM_Out_HL | CW_TMR4_UOC_Mode0 |\
CW_TMR4_VPWM_Out_HL | CW_TMR4_VOC_Mode0 |\
CW_TMR4_WPWM_Out_HL | CW_TMR4_WOC_Mode0;
}
/**********************************************************************
*
* change direction
*
**********************************************************************
*/
void DK_change(UInt16 speed)
{
sptr->SetPoint=speed;
}
/**********************************************************************
*
* change direction
*
**********************************************************************
*/
void DK_move(UInt16 speed)
{
UInt8 count,i,tmp1,tmp2;
count=0;
IncPIDInit();
sptr->SetPoint = 670;
sptr->LowRange = 600;
sptr->HighRange = 850;
CountEn=ENABLE;
Count=0;
sCSptr->B._startup = ENABLE;
while(Count<=11);
sCSptr->B._startup = DISABLE;
P_TMR4_OutputCtrl->W = CW_TMR4_POLP_Active_High | \
CW_TMR4_UPWM_Out_HL | CW_TMR4_UOC_Mode1 |\
CW_TMR4_VPWM_Out_HL | CW_TMR4_VOC_Mode1 |\
CW_TMR4_WPWM_Out_HL | CW_TMR4_WOC_Mode1;
F_Delay(2);
P_TMR4_OutputCtrl->W = CW_TMR4_POLP_Active_High | \
CW_TMR4_UPWM_Out_HL | CW_TMR4_UOC_Mode0 |\
CW_TMR4_VPWM_Out_HL | CW_TMR4_VOC_Mode0 |\
CW_TMR4_WPWM_Out_HL | CW_TMR4_WOC_Mode0;
CountEn=DISABLE;
/* BLDC_Test4_PU120FullPWM(3);
P_TMR4_TGRA->W = MID2PWM;
P_TMR4_TGRD->W = MID2PWM >>1;
P_TMR_LDOK->W = CW_TMR_LDOK1;
while((HALL>>5)!=2);
P_TMR4_TGRA->W = MINPWM;
P_TMR4_TGRD->W = MINPWM >>1;
P_TMR_LDOK->W = CW_TMR_LDOK1;
P_TMR4_OutputCtrl->W = CW_TMR4_POLP_Active_High | \
CW_TMR4_UPWM_Out_HL | CW_TMR4_UOC_Mode0 |\
CW_TMR4_VPWM_Out_HL | CW_TMR4_VOC_Mode0 |\
CW_TMR4_WPWM_Out_HL | CW_TMR4_WOC_Mode0;*/
}
/*****************************************************************************/
/* TPM4_ISR(): TPM4 period interrupt service routine */
/*****************************************************************************/
void TPM4_ISR(UInt16 hall)
{
if(sCSptr->B._startup == ENABLE)
BLDC_Test4_NU120FullPWM(hall>>5);
}
/*=================================================================*/
/*BLDC 啟動 */
/*=================================================================*/
void BLDC_Motor_Startup(void)
{
P_TMR0_Status->B.TCVIF = 1;
if(sCSptr->B._startup==ENABLE)
{
if(sCSptr->B._direction)
BLDC_Test4_NU120FullPWM(P_POS1_DectData->W);
else
BLDC_Test4_PU120FullPWM(P_POS1_DectData->W);
}
}
/*=================================================================*/
/*BLDC 正常運轉 */
/*=================================================================*/
void BLDC_Motor_Normalrun(UInt8 port)
{ UInt8 i;
// P_TMR0_Status->B.PDCIF = 1;
if(sCSptr->B._startup==ENABLE)
{
Speedcalc();
if(port == 3)
{
/*if(sCSptr->B._direction)
BLDC_Test3_NU120FullPWM(P_POS0_DectData->W);
else
BLDC_Test3_PU120FullPWM(P_POS0_DectData->W);*/
}
else if(port == 4)
{
/*if(sCSptr->B._direction)
BLDC_Test4_NU120FullPWM(P_POS1_DectData->W);
else
BLDC_Test4_PU120FullPWM(P_POS1_DectData->W);*/
}
}
}
/*=================================================================*/
/*BLDC 調速 */
/*=================================================================*/
void BLDC_Motor_Actiyator(void)
{
unsigned int uiSpeed = 0;
if(sCSptr->B._startup)
{
//uiSpeed = 1875000L/(summation>>SHIFTDIV); //計算速度
if(iav<sptr->LowRange)
{
sptr->IncPoint += IncPIDCalc(iav);
}
else
{
if(iav>sptr->HighRange)
{
sptr->IncPoint += DecPIDCalc(iav);
}
}
if(sptr->IncPoint > MAXPWM)
sptr->IncPoint = MAXPWM;
if(sptr->IncPoint < MINPWM)
sptr->IncPoint = MINPWM;
P_TMR4_TGRA->W = sptr->IncPoint;
P_TMR4_TGRD->W = sptr->IncPoint >>1;
P_TMR_LDOK->W = CW_TMR_LDOK1;
}
}
/*=================================================================*/
/* */
/*=================================================================*/
void Speedcalc(void)
{
unsigned int original;
original = P_TMR1_TGRA->W;
//original=800;
if(original > 469) //最高轉速限制在4000rpm（RADIX/4000）
{
summation -= *ptr; //滑動濾波
*ptr = original;
summation += *ptr;
if((++ptr) > (uiFilter+CAPBSIZE-1)) ptr = uiFilter;
}
}
void IncPIDInit(void)
{ UInt8 i;
sptr->IncPoint = MID2PWM;
sptr->LastError = 0;
sptr->PrevError = 0;
sptr->ProportionDec = KPP;
sptr->ProportionInc= KPP/2;
sptr->Integral = KPI;
sptr->Derivative = KPD;
sptr->SetPoint = SETSPD;
sptr->LowRange = SETSPD-100;
sptr->HighRange = SETSPD+100;
for(i=0;i<CAPBSIZE;i++) uiFilter[i] = 0;
ptr= uiFilter;
summation = 0;
sCSptr->B._startup = DISABLE;
}
/*=================================================================*/
/* Increment PID計算 */
/*=================================================================*/
static int IncPIDCalc(unsigned int NextPoint)
{
register int dError, incpid;
dError = sptr->SetPoint - NextPoint;
incpid = sptr->ProportionInc* dError;
//- sptr->Integral * sptr->LastError;
// + sptr->Derivative * sptr->PrevError;
sptr->PrevError = sptr->LastError;
sptr->LastError = dError;
return(incpid);
}
UInt8 ReadHall()
{ UInt8 hall;
hall = ((P_IOC_Data->W)&0x00e0)>>5;
return (hall);
}
UInt8 ReadHallState()
{ UInt8 tmp1,tmp2,tmp3;
UInt8 i,j;
j=5;
tmp1=ReadHall();
tmp2=ReadHall();
if( tmp1 != tmp2)
return (0x01);
else
return (0xff);
}
static int DecPIDCalc(unsigned int NextPoint)
{
register int dError, incpid;
dError = sptr->SetPoint - NextPoint;
incpid = sptr->ProportionDec* dError;
sptr->LastError = dError;
return(incpid);
}
/*=================================================================*/
/* *END* */
/*=================================================================*/
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -