//MCU:Mega16;晶振:8MHz; //PWM:4KHz;滤波器频率:100Hz;系统频率:100Hz;10ms; //赵国霖:10.05.10; #include #include #include //#define checkbit(var,bit) (var&(0x01<<(bit))) /*定义查询位函数*/ //#define setbit(var,bit) (var|=(0x01<<(bit))) /*定义置位函数*/ //#define clrbit(var,bit) (var&=(~(0x01<<(bit)))) /*定义清零位函数*/ //------------------------------------------------------- //输出端口初始化 void PORT_initial(void) { DDRA=0B00000000; PINA=0X00; PORTA=0X00; DDRB=0B00000000; PINB=0X00; PORTB=0X00; DDRC=0B00010000; PINC=0X00; PORTC=0X00; DDRD=0B11110010; PIND=0X00; PORTD=0X00; } //------------------------------------------------------- //定时器1初始化 void T1_initial(void) { TCCR1A|=(1<252) { PWM_LH=252; } if (PWM_RH<0) { PORTD|=BIT(7); PWM_RH*=-1; } else { PORTD&=~BIT(7); } if (PWM_RH>252) { PWM_RH=252; } OCR1AH=0; OCR1AL=PWM_LH; //OC1A输出; OCR1BH=0; OCR1BL=PWM_RH; //OC1B输出; } //------------------------------------------------------- //计算PWM输出值 //车辆直径:76mm; 12*64pulse/rev; 1m=3216pulses; //------------------------------------------------------- //static int speed_diff,speed_diff_all,speed_diff_adjust; //static float K_speed_P,K_speed_I; static float K_voltage,K_angle,K_angle_dot,K_position,K_position_dot; static float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD; static float position,position_dot; static float position_dot_filter; static float PWM; static int speed_output_LH,speed_output_RH; static int Turn_Need,Speed_Need; //------------------------------------------------------- void PWM_calculate(void) { if ( 0==(~PINA&BIT(1)) ) //左转 { Turn_Need=-40; } else if ( 0==(~PINB&BIT(2)) ) //右转 { Turn_Need=40; } else //不转 { Turn_Need=0; } if ( 0==(~PINC&BIT(0)) ) //前进 { Speed_Need=-2; } else if ( 0==(~PINC&BIT(1)) ) //后退 { Speed_Need=2; } else //不动 { Speed_Need=0; } K_angle_AD=ADport(4)*0.007; K_angle_dot_AD=ADport(5)*0.007; K_position_AD=ADport(6)*0.007; K_position_dot_AD=ADport(7)*0.007; position_dot=speed_output_RH*0.04; position_dot_filter*=0.9; //车轮速度滤波 position_dot_filter+=position_dot*0.1; position+=position_dot_filter; //position+=position_dot; position+=Speed_Need; if (position<-768) //防止位置误差过大导致的不稳定 { position=-768; } else if (position>768) { position=768; } PWM = K_angle*angle *K_angle_AD + K_angle_dot*angle_dot *K_angle_dot_AD + K_position*position *K_position_AD + K_position_dot*position_dot_filter *K_position_dot_AD; speed_output_RH = PWM - Turn_Need; speed_output_LH = - PWM - Turn_Need ; /* speed_diff=speed_real_RH-speed_real_LH; //左右轮速差PI控制; speed_diff_all+=speed_diff; speed_diff_adjust=(K_speed_P*speed_diff+K_speed_I*speed_diff_all)/2; */ PWM_output (speed_output_LH,speed_output_RH); } //------------------------------------------------------- //定时器2中断处理 //------------------------------------------------------- static unsigned char temp; //------------------------------------------------------- #pragma interrupt_handler T2INT_fun:4 void T2INT_fun(void) { AD_calculate(); PWM_calculate(); if(temp>=4) //10ms即中断;每秒计算:100/4=25次; { if (USART_State==0X30) //ASCII码:0X30代表字符'0' { USART_Transmit(angle*57.3+128); USART_Transmit(angle_dot*57.3+128); USART_Transmit(128); } else if(USART_State==0X31) //ASCII码:0X30代表字符'1' { USART_Transmit(speed_output_LH+128); USART_Transmit(speed_output_RH+128); USART_Transmit(128); } else if(USART_State==0X32) //ASCII码:0X30代表字符'2' { USART_Transmit(speed_real_LH+128); USART_Transmit(speed_real_RH+128); USART_Transmit(128); } else if(USART_State==0X33) //ASCII码:0X30代表字符'3' { USART_Transmit(K_angle+128); USART_Transmit(K_angle_dot+128); USART_Transmit(K_position_dot+128); } temp=0; } speed_real_LH=0; speed_real_RH=0; temp+=1; } //------------------------------------------------------- int i,j; //------------------------------------------------------- void main(void) { PORT_initial(); T2_initial(); INT_initial(); USART_initial (); SEI(); K_position=0.8 * 0.209; //换算系数:(256/10) * (2*pi/(64*12))=0.20944;//256/10:电压换算至PWM,256对应10V; K_angle=34 * 25.6; //换算系数:256/10 =25.6; K_position_dot=1.09 * 20.9; //换算系数:(256/10) * (25*2*pi/(64*12))=20.944; K_angle_dot=2 * 25.6; //换算系数:256/10 =25.6; for (i=1;i<=500;i++) //延时启动PWM,等待卡尔曼滤波器稳定 { for (j=1;j<=300;j++);; } T1_initial(); while(1) { ; } }