有关舵机和直流电机的PID控制算法.doc

有关舵机和直流电机的PID控制算法 #include hidef.h /* common defines and macros */ #include mc9s12dg128.h /* derivative information */ #pragma LINK_INFO DERIVATIVE mc9s12dg128b /********************CLOCK**************************/ //函数名 : initial_pll(void) //功能 : 初始化时钟 //入口参数: 无 //全局变量: 无 //返回参数: 无 //说明 : 总线时钟为 24MHz ,片内系统时钟为 48MHz //说明 PLLCLK=2*OSCCLK*(SYNR+1)/(REFDV+1) //说明 =2*16M*(2+1)/(1+1) //说明 =24M /***************************************************/ void initial_pll (void) { REFDV=0x01; SYNR =0x02; while (!(CRGFLG 0x08)) /*till VCO run stable*/ { } CLKSEL=0x80; /*PLL Clock Enable*/ } /**************************PORT*****************************/ //函数名 : init_port(void) //功能 : 初始化端口 PORTA PORTB PORTH //入口参数: 无 //全局变量: 无 //返回参数: 无 //说明 : PORTA PORTB 用于采集传感器的数据 PTH_PTH1为开关 /***********************************************************/ void init_port(void) { DDRB =0; /*PORTB 为输入 */ DDRA =0xF0; /*PORTA 的前4 个BIT0~BIT3 为输入*/ DDRH_DDRH1=0; /*PTH的端口1为输入*/ PTH_PTH1 =0; } /***************************SERVO*****************************/ //函数名 : init_pwm1(void) //功能 : 初始化pwm1,产生频率为 200Hz的用于控制舵机转向的信号 //入口参数: 无 //全局变量: 无 //返回参数: 无 //说明 : 无 /*************************************************************/ void init_pwm1(void) { PWME =0XFC; /*Disable PWM1*/ PWMPRCLK =0xF0; /*BUS /1*/ PWMCLK |=0X02; /*选 CLOCKA*/ PWMSCLA =0X03; /*CLOCKA=BUS /(2 *3)=4000000*/ PWMPOL |=0X03; /*PWMchannel 1 output is high at the beginning of the period*/ PWMCAE =0XFC; /* 左对齐*/ PWMCTL |=0x10; /*选 PWM1 为16位*/ PWMPER0 =0x4E; PWMPER1 =0x20; /*设定周期为 1000 再结合上面就可得到200HZ 的脉冲*/ PWMDTY0 =0x17; PWMDTY1 =0x70; /*1.5MS 默认车不转向*/ PWME_PWME1 =1; /*Enable PWM1*/ } /************************ DC MOTOR_2 **************************/ //函数名 : init_pwm2(void) //功能 : 初始化pwm2,产生频率为 1.5625KHz的用于控制电机正转的信号 //入口参数: 无 //全局变量: 无 //返回参数: 无 //说明 : 无 /****************************************************************/ void init_pwm2 (void) { PWME_PWME2 = 0; /*Disable PWM2*/ PWMCLK_PCLK2 = 0; /*选CLOCKB*/ PWMPRCLK |