不做则已做则至全时不待我切勿止步

1、不做则已,做则至全;时不待我,切勿止步! #include "DSP28_Device.h"#include "DSP28_GlobalPrototypes.h"#define INA1 GpioDataRegs.GPADAT.bit.GPIOA11#define INA2 GpioDataRegs.GPADAT.bit.GPIOA12#define INB1 GpioDataRegs.GPADAT.bit.GPIOA13#define INB2 GpioDataRegs.GPADAT.bit.GPIOA14/PIDint Len,Len_1,Ren,R

2、en_1; / 左右轮误差量int Lyn,Lyn_1,Lyn_2,Ryn,Ryn_1,Ryn_2; / 左右轮反馈值int Lun,Lun_1,Run,Run_1; / 左右轮输出值int D_Lunp,D_Luni,D_Lund,D_Runp,D_Runi,D_Rund; int Lkp,Rkp; / 左右轮比例系数int Lki,Rki; / 左右轮积分系数int Lkd,Rkd;int ski,hki;int vl=0;int vr=0;unsigned int speed_measure; /采样完成标志,通讯完成标unsigned int comm_old2;int maxv=25

3、5;int speedrun3000;void Sys_int();void Sysclk_int();void IO_int();void Eva_int();void wat_int();void PID();void PWM_out();interrupt void T3pint_isr(void);/PWM_outvoid main()unsigned int k=0;Sys_int();/系统初始化/PID参0数值Lkp =80;/左轮速初使给定PRkp =60;/右轮速初使给定PLkd =30;/右轮速初使给定DRkd =100;/右轮速初使给定Dhki=10;ski=2;spee

4、d_measure=0;Len=0;Len_1=0;Ren=0;Ren_1=0;Lun=0;Lun_1=0;Run=0;Run_1=0;Lyn=0;Lyn_1=0;Lyn_2=0;Ryn=0;Ryn_1=0;Ryn_2=0; comm_old0=10;comm_old1=0;EvaRegs.T1CON.bit.TENABLE=1;/使能定时器14EvaRegs.T2CON.bit.TENABLE=1;EvbRegs.T3CON.bit.TENABLE=1;EvbRegs.T4CON.bit.TENABLE=1;DINT;IER = 0x0000;IFR = 0x0000;/*设置中断服务程序入

5、口地址*/EALLOW; PieVectTable.T3PINT = &T3pint_isr; EDIS; /*开中断*/IER |= M_INT4;/为何如此设置，哪个中断使能哪？PieCtrl.PIEIER4.bit.INTx4 = 1;EINT; ERTM;while(1) EALLOW;SysCtrlRegs.WDCNTR=0x0055;/看门狗计数器清零SysCtrlRegs.WDCNTR=0x00AA;EDIS;if(speed_measure=1) if (k<1000)speedrunk=vr;k+;speed_measure=0;DINT;/关中断PID();

6、PWM_out();EINT;/开中断 /END MAINvoid Sys_int()DINT;SysCtrlRegs.WDCR= 0x0068; /屏蔽看门狗,使用晶振时钟OSCCLK/512(WDCR.20)Sysclk_int();InitPieCtrl();InitPieVectTable();IO_int();Eva_int();wat_int();SysCtrlRegs.WDCR=0x0028;/使能看门狗(WDCR.6)EINT;/开放全局中断/晶振初始化；外接30MHz晶体,初始化锁相环：外部晶体4倍频120MHz。void Sysclk_int() unsigned int

7、i;EALLOW;SysCtrlRegs.WDCR= 0x0068; SysCtrlRegs.PLLCR=0xA; /SYSCLKOUT=5*30MHz=150MHz/等待锁相环稳定asm ("NOP");for(i= 0; i< 5000; i+)SysCtrlRegs.HISPCP.all=0x0001;/HSPCLK=SYSCLKOUT=75M SysCtrlRegs.LOSPCP.all=0x00 02;/LSPCLK=SYSCLKOUT/4=37.5MSysCtrlRegs.PCLKCR.bit.EVAENCLK=1;/使能EVA外设中的高速时钟HSPCLK

8、SysCtrlRegs.PCLKCR.bit.EVBENCLK=1;EDIS; /看门狗初始化：void wat_int()EALLOW;SysCtrlRegs.WDCNTR=0x0055;/看门狗计数器清零SysCtrlRegs.WDCNTR=0x00AA;EDIS; /IO口初始化：void IO_int()EALLOW;/pwm1,3GpioMuxRegs.GPAMUX.bit.PWM1_GPIOA0 =1;GpioMuxRegs.GPAMUX.bit.PWM3_GPIOA2 =1;/QEP1,2,3,4GpioMuxRegs.GPAMUX.bit.CAP1Q1_GPIOA8 =1;Gp

9、ioMuxRegs.GPAMUX.bit.CAP2Q2_GPIOA9 =1;GpioMuxRegs.GPBMUX.bit.CAP4Q1_GPIOB8 =1;GpioMuxRegs.GPBMUX.bit.CAP5Q2_GPIOB9 =1;/INA1GpioMuxRegs.GPAMUX.bit.TDIRA_GPIOA11 =0;/配置成I/O口GpioMuxRegs.GPADIR.bit.GPIOA11 =1;/配置成输出 /INA2GpioMuxRegs.GPAMUX.bit.TCLKINA_GPIOA12 =0;GpioMuxRegs.GPADIR.bit.GPIOA12 =1;/INB1Gp

10、ioMuxRegs.GPAMUX.bit.C1TRIP_GPIOA13 = 0;GpioMuxRegs.GPADIR.bit.GPIOA13 =1;/INB2GpioMuxRegs.GPAMUX.bit.C2TRIP_GPIOA14 =0;GpioMuxRegs.GPADIR.bit.GPIOA14 =1;EDIS;/EVA初始化：（1）通用定时器1初始化：连续增模式，128分频。通用定时器2，4初始化：定向增减模式，不分频，QEP电路作为时钟/ 通用定时器3用作采样周期，周期中断，连续增模式，128分频/（2）比较单元1,2初始化：比较器输出方式是高有效。/（3）QEP单元初始化：检测上升沿

11、。void Eva_int()/T1 PWMEvaRegs.GPTCONA.all = 0x0000;EvaRegs.T1CNT=0x0000;EvaRegs.T1PR=0x00FF;/PWM周期值EvaRegs.T1CON.all=0x1100;/连续增模式T1CON（12，11），2分频T1CON（108)/T3采样周期EvbRegs.GPTCONB.all= 0x0000;EvbRegs.T3PR=0x7C03;/采样周期值0.4233msEvbRegs.T3CNT=0x0000;/计数初值EvbRegs.T3CON.all=0x1000;/连增模式，/QEP通用定时器2,4初始化Eva

12、Regs.T2CNT=0x0BB8;EvaRegs.T2PR=0x0FF0;EvaRegs.T2CON.all=0x1830;/内部高频时钟不分频（108），定向增减模式，定时器不使能EvbRegs.T4CNT=0x0BB8;EvbRegs.T4PR=0x0FF0;EvbRegs.T4CON.all=0x1830;/EVA完全比较单元1，2初始化EvaRegs.COMCONA.all=0x8260;/使能比较器COMCONA（15），使能全比较器COMCONA（9），使能全比较器1，2COMCONA（75）.EvaRegs.CMPR1 = 0x0000;EvaRegs.CMPR2 = 0x00

13、00;EvaRegs.ACTR.all = 0x00AA;/比较器输出引脚41（ACTRA70）输出方式为高电平有效。 /QEP1,2,3,4初始化EvaRegs.CAPCON.all=0x8000; /禁止捕获单元EvbRegs.CAPCONB.all=0x8000; /定时器3周期中断；EvbRegs.EVBIFRA.bit.T3PINT = 1;/中断标志置零EvbRegs.EVBIMRA.bit.T3PINT = 1;/使能通用定时器3周期中断/PIE定时器3周期中断INT4.4使能PieCtrl.PIEIER4.bit.INTx4=1; interrupt void T3pint_i

14、sr(void)DINT;/ EvaRegs.T2CON.bit.TENABLE=0;/关闭定时器2，4/EvbRegs.T4CON.bit.TENABLE=0;sp eed_measure=1;vl=EvbRegs.T4CNT-3000;vr=EvaRegs.T2CNT-3000;EvaRegs.T2CNT=3000;/计数值清零EvbRegs.T4CNT=3000;/ EvaRegs.T2CON.bit.TENABLE=1;/打开定时器2，4/EvbRegs.T4CON.bit.TENABLE=1;EvbRegs.EVBIFRA.bit.T3PINT = 1;/中断标志置零PieCtrl.

15、PIEACK.bit.ACK4 = 1;EINT;return; void PWM_out() / Run=10;/ Lun=0; if (Run < 0) / 左轮EvaRegs.CMPR2=-Run;INA1 = 0;INA2 = 1;else EvaRegs.CMPR2=Run;INA1 = 1;INA2 = 0;if (Lun < 0) / 右轮EvaRegs.CMPR1=-Lun;INB1 = 1;INB2 = 0;else EvaRegs.CMPR1=Lun;INB1 = 0;INB2 = 1;void PID()/PID算法 Lyn = vl;Ryn = vr;Len

16、 = comm_old0 - Lyn;Ren = comm_old1 - Ryn;if ( (Len >= 5) | (Len <= -5) ) Lki = ski; /积分分离else Lki = hki;if ( (Ren >= 5) | (Ren <= -5) ) Rki = ski;else Rki = hki;D_Lunp = Lkp * (Len - Len_1); /左轮PID计算D_Luni = (Lki * Len)/10;D_Lund = Lkd * (Lyn_2-2 * Lyn_1 + Lyn);Lun = Lun_1 + (D_Lunp + D_Luni + D_Lund)/2;D_Runp = Rkp * (Ren - Ren_1); /右轮PID计算D_Runi = (Rki * Ren)/10;D_Rund = Rkd * (Ryn_2-2 * Ryn_1 + Ryn);Run = Run_1 + (D_R