飞思卡尔PID程序
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
}
void main(void)
{
DisableInterrupts;
DDRP=0xFF;
DDRB=0xFF;
DDRA=0x00;
PORTB=0x24;
Init_PLL();
PIT_Init();
Init_PWMB();
Init_Event_Count();
EnableInterrupts;
for(;;)
#include <hidef.h> /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#define think_speed 30 // think_speed是用来控制电机轮子的转速的，给定他们不同的值，轮子就不同的转速，
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);//电机前进的控制函数，换成自己的就行了
{
}
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 66 timer0(void) //1ms执行函数程序
{
PITTF_PTF0 = 1; //PIT中断标志位清零
count++;
if(count>9) //10MS一次中断，用来读取编码器脉冲值
PITCFLMT_PITE = 0; //PIT is disabled.
PITCE_PCE0 = 1; //0 Chanel is enabled.
PITMUX_PMUX0 = 0; //使用子定时器0
PITMTLD0 = 200; //子定时器0设置为4us
PITLD0 = 250; // 1ms
{
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
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);
PWMDTY23 = 1800; //占空比1/20
PWMPER23 = 2778; //周期20ms
PWME_PWME3 = 1; //enable pwm3
}
//PIT定时中断
void PIT_Init(void)
{
INT_CFADDR = 0x70;
INT_CFDATA5 = 0x07; //设置为最高级
{
DDRT=0x00;
PERT=0xC0;//编码器脉冲累加器口为输入，上拉有效。其他口没作用。
PACTL = (1<<6);//脉冲累加器启动，外部发生一次下降沿就计数一次。外部编码器连接到IOC7,IOC6.
//在中断里读取PACNT的值就是脉冲个数。读取后清零PACNT。
}
void Start_Moto(uint M_PWM) //第一路电机
PITCFLMT_PFLMT = 1; //使能计数器0
Hale Waihona Puke BaiduPITFLT_PFLT0 = 1; //使能计数器1
PITINTE_PINTE0 = 1; //通道0中断打开
PITTF_PTF0 = 1; //清中断标志位
PITCFLMT_PITE = 1; // PIT is enabled
}
void Init_Event_Count(void)
}
void Init_PWMB(void) //控制后轮电机
{
PWMCTL_CON23 = 1; //级联2，3通道
PWME_PWME3 = 0;//关闭3通道
PWMPRCLK =0x00; //选择时钟B，50MHZ
PWMPOL_PPOL3 = 1; //起始波为高电平
PWMCNT23 = 0; //计数器从零开始计数
{
count=0;
pulse=PACNT; //读取脉冲值
Moto_PID();
//进行电机的PID控制
PACNT=0; //脉冲累加器计数器清零
}
}
#pragma CODE_SEG DEFAULT
_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;
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
void main(void)
{
DisableInterrupts;
DDRP=0xFF;
DDRB=0xFF;
DDRA=0x00;
PORTB=0x24;
Init_PLL();
PIT_Init();
Init_PWMB();
Init_Event_Count();
EnableInterrupts;
for(;;)
#include <hidef.h> /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#define think_speed 30 // think_speed是用来控制电机轮子的转速的，给定他们不同的值，轮子就不同的转速，
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);//电机前进的控制函数，换成自己的就行了
{
}
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 66 timer0(void) //1ms执行函数程序
{
PITTF_PTF0 = 1; //PIT中断标志位清零
count++;
if(count>9) //10MS一次中断，用来读取编码器脉冲值
PITCFLMT_PITE = 0; //PIT is disabled.
PITCE_PCE0 = 1; //0 Chanel is enabled.
PITMUX_PMUX0 = 0; //使用子定时器0
PITMTLD0 = 200; //子定时器0设置为4us
PITLD0 = 250; // 1ms
{
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
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);
PWMDTY23 = 1800; //占空比1/20
PWMPER23 = 2778; //周期20ms
PWME_PWME3 = 1; //enable pwm3
}
//PIT定时中断
void PIT_Init(void)
{
INT_CFADDR = 0x70;
INT_CFDATA5 = 0x07; //设置为最高级
{
DDRT=0x00;
PERT=0xC0;//编码器脉冲累加器口为输入，上拉有效。其他口没作用。
PACTL = (1<<6);//脉冲累加器启动，外部发生一次下降沿就计数一次。外部编码器连接到IOC7,IOC6.
//在中断里读取PACNT的值就是脉冲个数。读取后清零PACNT。
}
void Start_Moto(uint M_PWM) //第一路电机
PITCFLMT_PFLMT = 1; //使能计数器0
Hale Waihona Puke BaiduPITFLT_PFLT0 = 1; //使能计数器1
PITINTE_PINTE0 = 1; //通道0中断打开
PITTF_PTF0 = 1; //清中断标志位
PITCFLMT_PITE = 1; // PIT is enabled
}
void Init_Event_Count(void)
}
void Init_PWMB(void) //控制后轮电机
{
PWMCTL_CON23 = 1; //级联2，3通道
PWME_PWME3 = 0;//关闭3通道
PWMPRCLK =0x00; //选择时钟B，50MHZ
PWMPOL_PPOL3 = 1; //起始波为高电平
PWMCNT23 = 0; //计数器从零开始计数
{
count=0;
pulse=PACNT; //读取脉冲值
Moto_PID();
//进行电机的PID控制
PACNT=0; //脉冲累加器计数器清零
}
}
#pragma CODE_SEG DEFAULT
_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;
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