//MCU：Mega16；晶振：8MHz；

//PWM:4KHz;濾波器頻率:100Hz;系統(tǒng)頻率：100Hz；10ms;

//二輪平衡機(jī)器人項(xiàng)目

#include

#include

#include

//#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<

TCCR1B|=(1<

}

//-------------------------------------------------------

//定時(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<

TCCR2|=(1<

}

//-------------------------------------------------------

//外部中斷初始化

void INT_initial(void)

{

MCUCR|=(1<

GICR|=(1<

}

//-------------------------------------------------------

//串口初始化；

void USART_initial( void )

{        

UBRRH = 0X00;

UBRRL = 51;        //f=8MHz;設(shè)置波特率:9600:51;19200:25;

UCSRB = (1<

UCSRC = (1<

UCSRB|=(1<

} 

//-------------------------------------------------------

//串口發(fā)送數(shù)據(jù)；

void USART_Transmit( unsigned char data )

{

while ( !( UCSRA & (1<

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<

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'