//MCU：Mega16；晶振：8MHz；
//PWM:4KHz;滤波器频率:100Hz;系统频率：100Hz；10ms;
//赵国霖：10.05.10；
#include <iom16v.h>
#include <macros.h>
#include <math.h>




//-------------------------------------------------------
//输出端口初始化
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<<COM1A1)|(1<<WGM10)|(1<<COM1B1);				//T1：8位快速PWM模式、匹配时清0，TOP时置位
	TCCR1B|=(1<<WGM12)|(1<<CS11);			//PWM：8分频：8M/8/256=4KHz;
}


//-------------------------------------------------------
//定时器2初始化
void T2_initial(void)			//T2：计数至OCR2时产生中断
{
	OCR2=0X4E;			//T2：计数20ms(0X9C)10ms(0X4E)时产生中断;
	TIMSK|=(1<<OCIE2);
	TCCR2|=(1<<WGM21)|(1<<CS22)|(1<<CS21)|(1<<CS20);			//CTC模式，1024分频
}

//-------------------------------------------------------
//ADport采样：10位，采样基准电压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的处理时间约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;
}
//*/



//-------------------------------------------------------
//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);			//角度校正
	gyro=ADport(3);	
	
	acceler*=0.004069;		//系数换算：2.5/(1.2*512);
	acceler=asin(acceler);
	gyro*=0.00341;			//角速度系数：(3.14/180)* 100/512=0.01364；	
	
	Kalman_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)	
{

	
	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;		//利用PWM值代替轮速传感器的信号
	
	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 ;
	speed_output_LH = - PWM ;
	
	
	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();
	
	


}



//-------------------------------------------------------
int i,j;
//-------------------------------------------------------
void main(void)
{
	PORT_initial();
	T2_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<=500;j++);;
		
	}
	T1_initial();
		
	while(1)
	{
		;
	}
}