飞思卡尔PID程序

#include /* common defines and macros */

#include "derivative.h" /* derivative-specific definitions */

#define think_speed 30 // think_speed是用来控制电机轮子的转速的，给定他们不同的值，轮子就不同的转速，

volatile int count;

volatile int pulse;

//时钟初始化

void Init_PLL(void)

{

CLKSEL = 0X00; //disengage PLL to system

PLLCTL_PLLON = 1; //turn on PLL

SYNR = (0xc0|0x18); //SYDIV=0X18=24

REFDV = (0x40|0x07); //REFDIV=0X07=7

POSTDIV = 0x00; //pllclock=2*osc*(1+SYDIV)/(1+REFDIV)=100MHz;

_asm(nop); //BUS CLOCK=50M

_asm(nop);

_asm(nop);

_asm(nop);

_asm(nop);

_asm(nop);

_asm(nop);

_asm(nop);

_asm(nop);

while(!(CRGFLG_LOCK==1)); //when pll is steady ,then use it;

CLKSEL_PLLSEL = 1; //engage PLL to system;

}

void Init_PWMB(void) //控制后轮电机

{

PWMCTL_CON23 = 1; //级联2，3通道

PWME_PWME3 = 0; //关闭3通道

PWMPRCLK =0x00; //选择时钟B，50MHZ

PWMPOL_PPOL3 = 1; //起始波为高电平

PWMCNT23 = 0; //计数器从零开始计数

PWMDTY23 = 1800; //占空比1/20

PWMPER23 = 2778; //周期20ms

PWME_PWME3 = 1; //enable pwm3

}

//PIT定时中断

void PIT_Init(void)

{

INT_CFADDR = 0x70;

INT_CFDA TA5 = 0x07; //设置为最高级

PITCFLMT_PITE = 0; //PIT is disabled.

PITCE_PCE0 = 1; //0 Chanel is enabled.

PITMUX_PMUX0 = 0; //使用子定时器0

PITMTLD0 = 200; // 子定时器0设置为4us

PITLD0 = 250; // 1ms

PITCFLMT_PFLMT = 1; // 使能计数器0

PITFLT_PFLT0 = 1; // 使能计数器1

PITINTE_PINTE0 = 1; //通道0中断打开

PITTF_PTF0 = 1; //清中断标志位

PITCFLMT_PITE = 1; // PIT is enabled

}

void Init_Event_Count(void)

{

DDRT=0x00;

PERT=0xC0;//编码器脉冲累加器口为输入，上拉有效。其他口没作用。

PACTL = (1<<6);//脉冲累加器启动，外部发生一次下降沿就计数一次。外部编码器连接到IOC7,IOC6.

//在中断里读取PACNT的值就是脉冲个数。读取后清零PACNT。

}

void Start_Moto(uint M_PWM) //第一路电机

{

PORTB |=(1<<5);

PORTB &= ~(1<<4);

PWMDTY23 = M_PWM;

void Moto_PID()

{

volatile static int err=0,last_err=0,derr=0;

volatile static int M_PWM=0;

err=think_speed-pulse;// err

derr=err-last_err;

M_PWM+=err*10+derr*15; //15 5

if(M_PWM<=0)

M_PWM=0;

if(M_PWM>=2777)

M_PWM=2777;

last_err=err; //记录上次偏差值

Start_Moto(M_PWM);//电机前进的控制函数，换成自己的就行了}

void main(void)

{

DisableInterrupts;

DDRP=0xFF;

DDRB=0xFF;

DDRA=0x00;

PORTB=0x24;

Init_PLL();

PIT_Init();

Init_PWMB();

Init_Event_Count();

EnableInterrupts;

for(;;)

{

}

}

#pragma CODE_SEG __NEAR_SEG NON_BANKED

void interrupt 66 timer0(void) //1ms执行函数程序

{

PITTF_PTF0 = 1; //PIT中断标志位清零count++;

if(count>9) //10MS一次中断，用来读取编码器脉冲值

{

count=0;

pulse=PACNT; //读取脉冲值

Moto_PID();

//进行电机的PID控制

PACNT=0; //脉冲累加器计数器清零

}

}

#pragma CODE_SEG DEFAULT