二輪平衡機(jī)器人控制器代碼

ID:80436 · 發(fā)表于 2015-5-22 01:06

//MCU：Mega16；晶振：8MHz；
//PWM:4KHz;濾波器頻率:100Hz;系統(tǒng)頻率：100Hz；10ms;
//二輪平衡機(jī)器人項(xiàng)目
#include <iom16v.h>
#include <macros.h>
#include <math.h>
//#define checkbit(var,bit) (var&(0x01<<(bit))) /*定義查詢位函數(shù)*/
//#define setbit(var,bit) (var|=(0x01<<(bit))) /*定義置位函數(shù)*/
//#define clrbit(var,bit) (var&=(~(0x01<<(bit)))) /*定義清零位函數(shù)*/
//-------------------------------------------------------
//輸出端口初始化
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;
}
//-------------------------------------------------------
//定時(shí)器1初始化
void T1_initial(void)
{
TCCR1A|=(1<<COM1A1)|(1<<WGM10)|(1<<COM1B1); //T1：8位快速PWM模式、匹配時(shí)清0，TOP時(shí)置位
TCCR1B|=(1<<WGM12)|(1<<CS11); //PWM：8分頻：8M/8/256=4KHz;
}
//-------------------------------------------------------
//定時(shí)器2初始化
void T2_initial(void) //T2：計(jì)數(shù)至OCR2時(shí)產(chǎn)生中斷
{
OCR2=0X4E; //T2：計(jì)數(shù)20ms(0X9C)10ms(0X4E)時(shí)產(chǎn)生中斷;
TIMSK|=(1<<OCIE2);
TCCR2|=(1<<WGM21)|(1<<CS22)|(1<<CS21)|(1<<CS20); //CTC模式，1024分頻
}
//-------------------------------------------------------
//外部中斷初始化
void INT_initial(void)
{
MCUCR|=(1<<ISC01)|(1<<ISC00)|(1<<ISC11)|(1<<ISC10); //INT0、INT1上升沿有效
GICR|=(1<<INT0)|(1<<INT1); //外部中斷使能
}
//-------------------------------------------------------
//串口初始化；
void USART_initial( void )
{
UBRRH = 0X00;
UBRRL = 51; //f=8MHz;設(shè)置波特率:9600:51;19200:25;
UCSRB = (1<<RXEN)|(1<<TXEN); //接收器與發(fā)送器使能；
UCSRC = (1<<URSEL)|(1<<UCSZ0)|(1<<UCSZ1); //設(shè)置幀格式: 8 個(gè)數(shù)據(jù)位, 1 個(gè)停止位；
UCSRB|=(1<<RXCIE); //USART接收中斷使能
}
//-------------------------------------------------------
//串口發(fā)送數(shù)據(jù)；
void USART_Transmit( unsigned char data )
{
while ( !( UCSRA & (1<<UDRE))); //等待發(fā)送緩沖器為空；
UDR = data; //將數(shù)據(jù)放入緩沖器，發(fā)送數(shù)據(jù)；
}
//-------------------------------------------------------
//串口接收數(shù)據(jù)中斷，確定數(shù)據(jù)輸出的狀態(tài)；
#pragma interrupt_handler USART_Receive_Int:12
static char USART_State;
void USART_Receive_Int(void)
{
USART_State=UDR;//USART_Receive();
}
//-------------------------------------------------------
//計(jì)算LH側(cè)輪速：INT0中斷；
//-------------------------------------------------------
static int speed_real_LH;
//-------------------------------------------------------
#pragma interrupt_handler SPEEDLHINT_fun:2
void SPEEDLHINT_fun(void)
{
if (0==(PINB&BIT(0)))
{
speed_real_LH-=1;
}
else
{
speed_real_LH+=1;
}
}
//-------------------------------------------------------
//計(jì)算RH側(cè)輪速，：INT1中斷；
//同時(shí)將輪速信號(hào)統(tǒng)一成前進(jìn)方向了；
//-------------------------------------------------------
static int speed_real_RH;
//-------------------------------------------------------
#pragma interrupt_handler SPEEDRHINT_fun:3
void SPEEDRHINT_fun(void)
{
if (0==(PINB&BIT(1)))
{
speed_real_RH+=1;
}
else
{
speed_real_RH-=1;
}
}
//-------------------------------------------------------
//ADport采樣：10位，采樣基準(zhǔn)電壓Aref
//-------------------------------------------------------
static int AD_data;
//-------------------------------------------------------
int ADport(unsigned char port)
{
ADMUX=port;
ADCSRA|=(1<<ADEN)|(1<<ADSC)|(1<<ADPS1)|(1<<ADPS0); //采樣頻率為8分頻；
while(!(ADCSRA&(BIT(ADIF))));
AD_data=ADCL;
AD_data+=ADCH*256;
AD_data-=512;
return (AD_data);
}
//*
//-------------------------------------------------------
//Kalman濾波，8MHz的處理時(shí)間約1.8ms；
//-------------------------------------------------------
static float angle, angle_dot; //外部需要引用的變量
//-------------------------------------------------------
static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;
//注意：dt的取值為kalman濾波器采樣時(shí)間;
static float P[2][2] = {
{ 1, 0 },
{ 0, 1 }
};
static float Pdot[4] ={0,0,0,0};
static const char C_0 = 1;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
angle+=(gyro_m-q_bias) * dt;
Pdot[0]=Q_angle - P[0][1] - P[1][0];
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
angle_err = angle_m - angle;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;
}
//*/
/*
//-------------------------------------------------------
//互補(bǔ)濾波
//-------------------------------------------------------
static float angle,angle_dot; //外部需要引用的變量
//-------------------------------------------------------
static float bias_cf;
static const float dt=0.01;
//-------------------------------------------------------
void complement_filter(float angle_m_cf,float gyro_m_cf)
{
bias_cf*=0.998; //陀螺儀零飄低通濾波；500次均值；
bias_cf+=gyro_m_cf*0.002;
angle_dot=gyro_m_cf-bias_cf;
angle=(angle+angle_dot*dt)*0.90+angle_m_cf*0.05;
//加速度低通濾波；20次均值；按100次每秒計(jì)算，低通5Hz；
}
*/
//-------------------------------------------------------
//AD采樣；
//以角度表示；
//加速度計(jì)：1.2V=1g=90°；滿量程：1.3V~3.7V；
//陀螺儀：0.5V~4.5V=-80°~+80°；滿量程5V=200°=256=200°；
//-------------------------------------------------------
static float gyro,acceler;
//-------------------------------------------------------
void AD_calculate(void)
{
acceler=ADport(2)+28; //角度校正
gyro=ADport(3);
acceler*=0.004069; //系數(shù)換算：2.5/(1.2*512); // 5/(1.2*1024);5為參考電壓5V;1.2V靈敏度對(duì)應(yīng)加速度1g;1024為AD精度
acceler=asin(acceler); //反正弦求角度
gyro*=0.00341; //角速度系數(shù)：(3.14/180)* 100/512=0.01364；//(3.14/180)* (200*0.025)/1024*0.025既5/1024*0.025
//求得角速度單位角度/秒
Kalman_Filter(acceler,gyro); //卡爾曼濾波帶入角度。角速度
//complement_filter(acceler,gyro);
}
//-------------------------------------------------------
//PWM輸出
//-------------------------------------------------------
void PWM_output (int PWM_LH,int PWM_RH)
{
if (PWM_LH<0)
{
PORTD|=BIT(6);
PWM_LH*=-1;
}
else
{
PORTD&=~BIT(6);
}
if (PWM_LH>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輸出；
}
//-------------------------------------------------------
//計(jì)算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)) ) //左轉(zhuǎn)
{
Turn_Need=-40;
}
else if ( 0==(~PINB&BIT(2)) ) //右轉(zhuǎn)
{
Turn_Need=40;
}
else //不轉(zhuǎn)
{
Turn_Need=0;
}
if ( 0==(~PINC&BIT(0)) ) //前進(jìn)
{
Speed_Need=-2;
}
else if ( 0==(~PINC&BIT(1)) ) //后退
{
Speed_Need=2;
}
else //不動(dòng)
{
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=PWM*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) //防止位置誤差過大導(dǎo)致的不穩(wěn)定
{
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);
}
//-------------------------------------------------------
//定時(shí)器2中斷處理
//-------------------------------------------------------
static unsigned char temp;
//-------------------------------------------------------
#pragma interrupt_handler T2INT_fun:4
void T2INT_fun(void)
{
AD_calculate();
PWM_calculate();
if(temp>=4) //10ms即中斷；每秒計(jì)算：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; //換算系數(shù)：(256/10) * (2*pi/(64*12))=0.20944；//256/10:電壓換算至PWM，256對(duì)應(yīng)10V；
K_angle=34 * 25.6; //換算系數(shù)：256/10 =25.6；
K_position_dot=1.09 * 20.9; //換算系數(shù)：(256/10) * (25*2*pi/(64*12))=20.944；
K_angle_dot=2.2 * 25.6; //換算系數(shù)：256/10 =25.6；
for (i=1;i<=500;i++) //延時(shí)啟動(dòng)PWM，等待卡爾曼濾波器穩(wěn)定
{
for (j=1;j<=300;j++);;
}
T1_initial();
while(1)
{
;
}
}