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

//PWM:4KHz;滤波器频率:100Hz;系统频率：100Hz；10ms;

//二轮平衡机器人项目

#include

#include

#include

//#define checkbit(var,bit)     (var&(0x01<

//#define setbit(var,bit)     (var|=(0x01<

//#define clrbit(var,bit)     (var&=(~(0x01<

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

//输出端口初始化

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<

TCCR1B|=(1<

}

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

//定时器2初始化

void T2_initial(void)        //T2：计数至OCR2时产生中断

{

OCR2=0X4E;        //T2：计数20ms(0X9C)10ms(0X4E)时产生中断;

TIMSK|=(1<

TCCR2|=(1<

}

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

//外部中断初始化

void INT_initial(void)

{

MCUCR|=(1<

GICR|=(1<

}

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

//串口初始化；

void USART_initial( void )

{        

UBRRH = 0X00;

UBRRL = 51;        //f=8MHz;设置波特率:9600:51;19200:25;

UCSRB = (1<

UCSRC = (1<

UCSRB|=(1<

} 

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

//串口发送数据；

void USART_Transmit( unsigned char data )

{

while ( !( UCSRA & (1<

UDR = data;        //将数据放入缓冲器，发送数据；

} 

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

//串口接收数据中断，确定数据输出的状态；

#pragma interrupt_handler USART_Receive_Int:12

static char USART_State;

void USART_Receive_Int(void)

{

USART_State=UDR;//USART_Receive();

}

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

//计算LH侧轮速：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;

} 

}

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

//计算RH侧轮速，：INT1中断；

//同时将轮速信号统一成前进方向了；

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

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位，采样基准电压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的处理时间约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滤波器采样时间;

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;

}

//*/

/*

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

//互补滤波

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

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次每秒计算，低通5Hz；

}

*/        

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

//AD采样；

//以角度表示；

//加速度计：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;        //系数换算：2.5/(1.2*512); // 5/(1.2*1024);5为参考电压5V;1.2V灵敏度对应加速度1g;1024为AD精度

acceler=asin(acceler);        //反正弦求角度

gyro*=0.00341;        //角速度系数：(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输出；

}

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

//计算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=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

{

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_di