int main()
{
	char buffer[20], float_[10];
	float Xa,Ya,Za,t;
	float Xg=0,Yg=0,Zg=0, kalPitch, kalRoll, kalYaw;
	I2C_Init();											/* Initialize I2C */
	Gyro_Init();										/* Initialize Gyro */
	USART_Init(9600);									/* Initialize USART with 9600 baud rate */
	calman_init_roll();
	calman_init_pitch();
	timer_init();
	servo_angle(90);
	servo_angle_2(90);
	
	DDRD = 0xff;
	
	while(1)
	{
		Read_RawValue();

		Xa = Acc_x/16384.0;								/* Divide raw value by sensitivity scale factor to get real values */
		Ya = Acc_y/16384.0;
		Za = Acc_z/16384.0;
		
		Xg = Gyro_x/16.4;
		Yg = Gyro_y/16.4;
		Zg = Gyro_z/16.4;

		t = (Temperature/340.00)+36.53;					/* Convert temperature in °/c using formula */

		arx = (180/3.141592) * atan(Xa / sqrt(square(Ya) + square(Za)));  //pitch 
		ary = (180/3.141592) * atan(Ya / sqrt(square(Xa) + square(Za)));	//roll
		arz = (180/3.141592) * atan(sqrt(square(Ya) + square(Xa)) / Za);    //yaw
		 
		grx = grx + (0.047 * Xg);
		gry = gry + (0.047 * Yg);
		grz = grz + (0.047 * Zg);
		
		kalPitch = calman_filtr_pitch(arx, gry);
		kalRoll = calman_filtr_roll(ary, grx);
		
		
		if(kalRoll < 3 && kalRoll > -3) {
			servo_angle(90);
			servo_angle_2(-90);
		} else if(kalRoll > 3) {
			servo_angle(90-kalRoll);
			servo_angle_2(-90+kalRoll);
		} else if(kalRoll < -3) {
			servo_angle(90-(kalRoll));
			servo_angle_2(-90+(kalRoll));
		}
		
		
		/*dtostrf( arx, 3, 2, float_ );
		sprintf(buffer," AP = %s o\t",float_);
		USART_SendString(buffer);

		dtostrf( ary, 3, 2, float_ );
		sprintf(buffer," AR = %s o\t",float_);
		USART_SendString(buffer);
		
		dtostrf( grx, 3, 2, float_ );
		sprintf(buffer," Gx = %s o\t",float_);
		USART_SendString(buffer);

		dtostrf( gry, 3, 2, float_ );
		sprintf(buffer," Gy = %s o\t",float_,0xF8);
		USART_SendString(buffer);

		dtostrf( kalPitch, 3, 2, float_ );
		sprintf(buffer," kP = %s o\t",float_,0xF8);
		USART_SendString(buffer);

		dtostrf( kalRoll, 3, 2, float_ );
		sprintf(buffer," kR = %s 0\t\n",float_,0xF8);
		USART_SendString(buffer); */

		/*dtostrf( Xa, 3, 2, float_ );					
		sprintf(buffer," Ax = %s g\t",float_);
		USART_SendString(buffer);

		dtostrf( Ya, 3, 2, float_ );
		sprintf(buffer," Ay = %s g\t",float_);
		USART_SendString(buffer);
		
		dtostrf( Za, 3, 2, float_ );
		sprintf(buffer," Az = %s g\t",float_);
		USART_SendString(buffer);

		dtostrf( t, 3, 2, float_ );
		sprintf(buffer," T = %s%cC\t",float_,0xF8);           
		USART_SendString(buffer);

		dtostrf( Xg, 3, 2, float_ );
		sprintf(buffer," Gx = %s%c/s\t",float_,0xF8);
		USART_SendString(buffer);

		dtostrf( Yg, 3, 2, float_ );
		sprintf(buffer," Gy = %s%c/s\t",float_,0xF8);
		USART_SendString(buffer);
		
		dtostrf( Zg, 3, 2, float_ );
		sprintf(buffer," Gz = %s%c/s\r\n",float_,0xF8);
		USART_SendString(buffer); */
		
		/*dtostrf( grx, 3, 2, float_ );
		sprintf(buffer," Grx = %s%c/s\t",float_,0xF8);
		USART_SendString(buffer);

		dtostrf( gry, 3, 2, float_ );
		sprintf(buffer," Gry = %s%c/s\t",float_,0xF8);
		USART_SendString(buffer);
		
		dtostrf( grz, 3, 2, float_ );
		sprintf(buffer," Grz = %s%c/s\r",float_,0xF8);
		USART_SendString(buffer);
		
		dtostrf( arx, 3, 2, float_ );
		sprintf(buffer," Arx = %s%c/s\t",float_,0xF8);
		USART_SendString(buffer);

		dtostrf( ary, 3, 2, float_ );
		sprintf(buffer," Ary = %s%c/s\t",float_,0xF8);
		USART_SendString(buffer);
		
		dtostrf( arz, 3, 2, float_ );
		sprintf(buffer," Arz = %s%c/s\r\n",float_,0xF8);
		USART_SendString(buffer); */
	}
}