}
void setspeed1() {
int idealspeed=7600; static int speedpwm=950; static error_sp=0,errorlast_sp=0,errord_sp=0,errordlast_sp=0,errordd_sp=0; int realspeed; 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; //前两次误差
/////////////////////锁相环设置////////////////////////////// int base_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(); EnableInterrupts; } ///////////////////////pulse初始化////////////////////////////
base_Pwm_set(PWM_CHANNEL0,1200,1200); base_Pwm_set(PWM_CHANNEL2,1200,speed);
} int core_Car_speed_close(){ base_Pwm_set(PWM_CHANNEL0,0,0); base_Pwm_set(PWM_CHANNEL2,0,0); } /////////////////////////方向设置///////////////////////// int core_Car_direction_set(int size){ 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); } }
DDRB=0XFF; DDRD=0X00; DDRC=0X00; EnableInterrupts; base_Init();
core_Car_speed_set(750);
for(;;) { turn();
}
_FEED_COP(); }
///////////////////////定时测脉冲///////////////////////////// void Time0_Init() { TIOS=0x01; //定时器通道0设置为输出比较 TC0=100000; //赋初值，当TCNT从0计数到此值时第一次进入中断 TCTL2=0x01; TSCR2=0x07; //计一个数用5.33微秒 TSCR1=0x80; TIE=0x01; } //中断： #pragma CODE_SEG __NEAR_SEG NON_BANKED interrupt 8 TIM0(void) { TFLG1_C0F=1;//清中断标志位 TIE=0x00; TC0=100000; sudu=PACNT ; //读取脉冲数 //setspeed(); PACNT =0; TIE=0x01;
} } //////////////////////////速度设置/////////////////////////////////// int core_car_speed_setting=0; int core_Car_speed_start(){ base_Pwm_set(PWM_CHANNEL0,1200,1200); base_Pwm_set(PWM_CHANNEL2,1200,1200); } int core_Car_speed_set(int speed){
int idealspeed=17750; static int speedpwm=700; static error_sp=0,errorlast_sp=0,errord_sp=0,errordlast_sp=0,errordd_sp=0; int realspeed; 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; //前两次误差
int base_Pulse_count_init() { PACTL=0x50; PACNT = 0x0000; //清0计数器 } //////////////////////////////pwm设置////////////////////////////// int base_Pwm_init(void) { PWME=0x00; PWMCLK=0xff; PWMPRCLK=0x30; PWMSCLA=0x01; PWMSCLB=0x03; PWMPOL=0x00; PWMCAE=0x00; PWMCTL=0xf0; } int base_Pwm_set(Pwm_Channel pchannel,int pper,int pdty){ int *bpwm=(int*)0x00ac; uchar p_start=0x01; p_start = ~(p_start<<(pchannel*2+1)); //PWME &=p_start; bpwm+=pchannel; //*bpwm=0x0000; bpwm+=4; *bpwm=pper; bpwm+=4; *bpwm=pdty; if(pper!=0){ p_start=0x01; p_start=p_start<<(pchannel*2+1); PWME |=p_start;
} #pragma CODE_SEG DEFAULT ///////////////////////////////////////////////////////////
////////////////////////初始化///////////////////////////// void base_Init(void) { base_Pll_init(); base_Pwm_init(); base_Pulse_count_init(); Time0_Init(); }
void sleep(int ms) { int i,j; for(i=0;i<ms;i++) for(j=0;j<2003;j++) {} } ////////////////////////////调整速度PID/////////////////////////// void setspeed() {
///////////////////////////PID变量定义////////////////////// typedef struct { char sp_p; char sp_d; char sp_i; }sp_pid; sp_pid pid={11,3,42}; /////////////////////////////主函数////////////////////////////////// void main(void) {
}
//////////////////////激光头点亮 流水灯///////////////////// void light()
{ PORTB=0x00; sleep(1); dushu4=PORTD_PD4; dushu9=PORTC_PC0; PORTB=0x11; sleep(1); dushu4=PORTD_PD4; dushu1=PORTD_PD1; PORTB=0x22; sleep(1); dushu5=PORTD_PD5; dushu2=PORTD_PD2; PORTB=0x33; sleep(1); dushu5=PORTD_PD5; dushu2=PORTD_PD2; PORTB=0x44; sleep(1); dushu6=PORTD_PD6; dushu3=PORTD_PD3; PORTB=0x55; sleep(1); dushu6=PORTD_PD6; dushu3=PORTD_PD3; PORTB=0x66; sleep(1); dushu7=PORTD_PD7; dushu8=PORTD_PD0; } ///////////////////////////转弯判断////////////////////////////
////////////////////////////函数声明/////////////////////// void base_Init(void); void sleep(int ms); void Time0_Init(); void setspeed(); void light(); void turn(); void CarControl(float measure); void setspeed1(); ////////////////////////////变量定义//////////////////////////////// int k,sudu; int jiaodu=0; int m=0,i=0; int dushu6,dushu5,dushu4,dushu3,dushu2,dushu1,dushu7,dushu8,dushu9; float l_ser_error = 0; float p_ser_error = 0; float n_ser_error = 0; float set_ser = 0; float inc_ser ; float servo_kp; // e(t - 1) // e(t - 2) // e(t)