飞思卡尔光电组程序Word格式.docx
《飞思卡尔光电组程序Word格式.docx》由会员分享，可在线阅读，更多相关《飞思卡尔光电组程序Word格式.docx（18页珍藏版）》请在冰豆网上搜索。
//e（t）
floatset_ser=0;
floatinc_ser;
floatservo_kp;
///////////////////////////PID变量定义//////////////////////
typedefstruct
{
charsp_p;
charsp_d;
charsp_i;
}sp_pid;
sp_pidpid={11,3,42};
/////////////////////////////主函数//////////////////////////////////
voidmain（void）{
DDRB=0XFF;
DDRD=0X00;
DDRC=0X00;
EnableInterrupts;
base_Init（）;
core_Car_speed_set（750）;
for（;
;
）
turn（）;
}
_FEED_COP（）;
///////////////////////定时测脉冲/////////////////////////////
voidTime0_Init（）
{
TIOS=0x01;
//定时器通道0设置为输出比较
TC0=100000;
//赋初值，当TCNT从0计数到此值时第一次进入中断
TCTL2=0x01;
TSCR2=0x07;
//计一个数用5.33微秒
TSCR1=0x80;
TIE=0x01;
}
//中断：
#pragmaCODE_SEG__NEAR_SEGNON_BANKED
interrupt8TIM0（void）
{
TFLG1_C0F=1;
//清中断标志位
TIE=0x00;
sudu=PACNT;
//读取脉冲数
//setspeed（）;
PACNT=0;
#pragmaCODE_SEGDEFAULT
///////////////////////////////////////////////////////////
////////////////////////初始化/////////////////////////////
voidbase_Init（void）
base_Pll_init（）;
base_Pwm_init（）;
base_Pulse_count_init（）;
Time0_Init（）;
}
/////////////////////锁相环设置//////////////////////////////
intbase_Pll_init（void）{
CPMUCLKS=0x00;
CPMUSYNR=0x00|0x02;
CPMUREFDIV=0x80|0x01;
CPMUPOSTDIV=0x00;
CPMUOSC_OSCE=1;
while（!
（CPMUFLG_LOCK==1））;
CPMUCLKS_PLLSEL=1;
_DISABLE_COP（）;
}
///////////////////////pulse初始化////////////////////////////
intbase_Pulse_count_init（）
PACTL=0x50;
PACNT=0x0000;
//清0计数器
//////////////////////////////pwm设置//////////////////////////////
intbase_Pwm_init（void）
PWME=0x00;
PWMCLK=0xff;
PWMPRCLK=0x30;
PWMSCLA=0x01;
PWMSCLB=0x03;
PWMPOL=0x00;
PWMCAE=0x00;
PWMCTL=0xf0;
intbase_Pwm_set（Pwm_Channelpchannel,intpper,intpdty）{
int*bpwm=（int*）0x00ac;
ucharp_start=0x01;
p_start=~（p_start<
<
（pchannel*2+1））;
//PWME&
=p_start;
bpwm+=pchannel;
//*bpwm=0x0000;
bpwm+=4;
*bpwm=pper;
*bpwm=pdty;
if（pper!
=0）{
p_start=0x01;
p_start=p_start<
（pchannel*2+1）;
PWME|=p_start;
//////////////////////////速度设置///////////////////////////////////
intcore_car_speed_setting=0;
intcore_Car_speed_start（）{
base_Pwm_set（PWM_CHANNEL0,1200,1200）;
base_Pwm_set（PWM_CHANNEL2,1200,1200）;
intcore_Car_speed_set（intspeed）{
base_Pwm_set（PWM_CHANNEL2,1200,speed）;
intcore_Car_speed_close（）{
base_Pwm_set（PWM_CHANNEL0,0,0）;
base_Pwm_set（PWM_CHANNEL2,0,0）;
/////////////////////////方向设置/////////////////////////
intcore_Car_direction_set（intsize）{
if（size>
=0）{
base_Pwm_set（PWM_CHANNEL3,10000,9250+250*size/45）;
}else{
base_Pwm_set（PWM_CHANNEL3,10000,9250-250*abs（size）/45）;
voidsleep（intms）
{inti,j;
for（i=0;
i<
ms;
i++）
for（j=0;
j<
2003;
j++）
{}
////////////////////////////调整速度PID///////////////////////////
voidsetspeed（）
intidealspeed=17750;
staticintspeedpwm=700;
staticinterror_sp=0,errorlast_sp=0,errord_sp=0,errordlast_sp=0,errordd_sp=0;
intrealspeed;
realspeed=sudu;
//实际速度
error_sp=idealspeed-realspeed;
//当前误差计算积分项I
errord_sp=error_sp-errorlast_sp;
//计算比例项P
errordd_sp=errord_sp-errordlast_sp;
//计算微分项D
errorlast_sp=error_sp;
//前一次误差
errordlast_sp=errord_sp;
//前两次误差
speedpwm=speedpwm+pid.sp_p*errord_sp+pid.sp_d*errordd_sp/10+pid.sp_i*error_sp/10;
if（speedpwm>
=800）
speedpwm=800;
elseif（speedpwm<
=700）
speedpwm=700;
core_Car_speed_set（speedpwm）;
voidsetspeed1（）
intidealspeed=7600;
staticintspeedpwm=950;
=1000）
speedpwm=1000;
=900）
speedpwm=900;
//////////////////////激光头点亮流水灯/////////////////////
voidlight（）
PORTB=0x00;
sleep
（1）;
dushu4=PORTD_PD4;
dushu9=PORTC_PC0;
PORTB=0x11;
dushu1=PORTD_PD1;
PORTB=0x22;
dushu5=PORTD_PD5;
dushu2=PORTD_PD2;
PORTB=0x33;
PORTB=0x44;
dushu6=PORTD_PD6;
dushu3=PORTD_PD3;
PORTB=0x55;
PORTB=0x66;
dushu7=PORTD_PD7;
dushu8=PORTD_PD0;
///////////////////////////转弯判断////////////////////////////
voidturn（）
{light（）;
///////////////////////////直道/////////////////////////////////
if（dushu8==1&
&
dushu7==1）
{setspeed（）;
CarControl（0）;
//左转
elseif（dushu4==1&
dushu7==1&
dushu1==0&
dushu2==0&
dushu3==0&
dushu5==0&
dushu6==0&
dushu8==0）
//eft_direction（-11）;
CarControl（-9）;
dushu5==1&
{setspeed1（）;
//left_direction（-13）;
CarControl（-12）;
dushu6==1&
//left_direction（-14）;
CarControl（-14）;
//右转
elseif（dushu8==1&
dushu1==1&
dushu4==0&
dushu7==0）
//right_direction（8）;
CarControl（7）;
elseif（dushu1==1&
dushu2==1&
dushu8==1&
//right_direction（9）;
CarControl（8）;
dushu3==1&
//right_direction（11）;
CarControl（11）;
//////////////////////////弯道///////////////////////////
//右转
elseif（dushu3==1&
dushu8==1）
//left_direction（-16）;
CarControl（20）;
dushu6==1）
//left_direction（-30）;
CarControl（30）;
dushu5==1）
{//setspeed1（）;
core_Car_speed_set（1000）;
dushu1==1）
//left_direction（-11）;
//左转
//right_direction（12）;
dushu3==1）
//right_direction（22）;
CarControl（-22）;
dushu2==1）
CarControl（-25）;
CarControl（-28）;
elseif（dushu6==1&
//right_direction（12）;
//
elseif（dushu1==0&
dushu8==0&
voidCarControl（floatmeasure）
//舵机输出增量
floatinc_ser;
//舵机上一次的输出值，初始化为1500
staticfloatout_pwm=9250;
//舵机变增益控制系数
floatservo_kp=0.4;
floatservo_kd=0.2;
n_ser_error=measure-set_ser;
//这里是测量值-设定值
//变增益控制
inc_ser=servo_kp*SERVO_KP*（n_ser_error-l_ser_error）
+servo_kd*SERVO_KD*（n_ser_error-2*l_ser_error+
p_ser_error）;
out_pwm+=inc_ser;
//初始就为9250
p_ser_error=l_ser_error;
l_ser_error=n_ser_error;
//防止超限,阈值到时要重新测一测
if（out_pwm<
=9000）
out_pwm=9000;
if（out_pwm>
=9500）
out_pwm=9500;
base_Pwm_set（PWM_CHANNEL3,10000,out_pwm）;
升级会员 