void main(void)
{
InitSysCtrl();
DINT;
IER = 0x0000;
IFR = 0x0000;
InitPieCtrl();
InitPieVectTable();
InitGpio();
InitEv();
ConfigCpuTimer(&CpuTimer0, 150, 10000);//10ms
EALLOW;
PieVectTable.TINT0 = &ISRTimer0;
PieVectTable.T3PINT =&T3PINT_isr;
EDIS;
IER |= M_INT4; //使能CPU中断
PieCtrl.PIEIER4.bit.INTx4=1;//使能PIE中断
IER |= M_INT1;
PieCtrl.PIEIER1.bit.INTx7 = 1;
EvbRegs.T3CON.bit.TENABLE=1;
EINT;
ERTM;
}
interrupt void T3PINT_isr(void)
{
EvbRegs.T4CON.bit.TENABLE = 1; //使能定时器4
EvbRegs.T4PR=0xffff; //周期寄存器初 值
EvbRegs.T4CNT=0x7fff;//计数器初值为0
EvbRegs.CAPCONB.bit.CAPQEPN=3;//使能QEP电路CAP4_QEP3,CAP5_QEP4
EvbRegs.CAPCONB.bit.CAP45TSEL=0;//用定时器4提供时基
EvbRegs.CAPCONB.bit.CAP4EDGE=3;//
EvbRegs.CAPCONB.bit.CAP5EDGE=3;
EvbRegs.T4CON.bit.TCLKS10=3;//选择时钟源为QEP
EvbRegs.T4CON.bit.TMODE=3;//使定时器4工作在定向增减计数模式
EvbRegs.T4CON.bit.T4SWT3=0;//自己的使能位
data2 = EvbRegs.T4CNT;
StartCpuTimer0();
//EvbRegs.T4CON.bit.TENABLE = 0; //停止定时器4
CpuTimer0Regs.TCR.bit.TSS = 0;
}
interrupt void ISRTimer0(void)
{
PieCtrl.PIEACK.all = 0x0001;
CpuTimer0Regs.TCR.bit.TSS = 1; //禁止CPU定时器0
//EvbRegs.T4CON.bit.TENABLE = 0; //停止定时器4
data1 = EvbRegs.T4CNT;
if(data1>=data2)
Speed_L=44.91*(data1-data2);
else if(data1<data2)
Speed_L=44.91*(data2-data1);
EvbRegs.T4CNT=0x7fff;
iError_L = ExcSpeed_L - Speed_L; //增量计算 E[k]项(期望值-实际值)
iIncpid_L = Kp* (iError_L - LastError_L)+ Ki * iError_L+ Kd * (iError_L - 2*LastError_L + PrevError_L);
//存储误差，用于下次计算,等待新的误差
PrevError_L = LastError_L; //E[k-2]项
LastError_L = iError_L; //E[k-1]项
Speed_Out_L += iIncpid_L;
M=Speed_Out_L;
if(M > 65535) //输出限幅
M=65535;
else if(M<0)
M=0;
M=M*0.2818;
EvbRegs.CMPR4=M;
}
|
楼主
