#include <16F877A.

h>
#device ADC=10
#FUSES NOWDT //No Watch Dog Timer
#FUSES NOBROWNOUT //No brownout reset
#FUSES NOLVP //No low voltage prgming, B3(PIC16) or B5(PIC18)
used FOR I/O
#use delay(crystal=12MHz)
//#use i2c(master, sda=PIN_C4, scl=PIN_C3,FORCE_HW)
//#use i2c(master, sda=PIN_E0, scl=PIN_E1)
#define MPU_SDA PIN_E0
#define MPU_SCL PIN_E1
#use I2C(master, sda=MPU_SDA, scl=MPU_SCL)
//#use TIMER(TIMER=1, TICK=1ms, BITS=16, NOISR)
#define LCD_RS_PIN PIN_D0
#define LCD_RW_PIN PIN_D1
#define LCD_ENABLE_PIN PIN_D2
#define LCD_DATA4 PIN_D4
#define LCD_DATA5 PIN_D5
#define LCD_DATA6 PIN_D6

#define LCD_DATA7 PIN_D7

#include <lcd.c>
#include "MPU6050.c"
#include "math.h"
#include "Kalman.h"
#define RAD_TO_DEG 180/PI

void main()
{
SIGNED int16 accX,accY, accZ;
SIGNED int16 gyroX,gyroY,gyroZ;
DOUBLE accXangle,accYangle;
DOUBLE gyroXangle,gyroYangle;
DOUBLE kalAngleX,kalAngleY;
UNSIGNED int16 timer;
lcd_init ();
printf (LCD_PUTC, "\f MuaLinhKien.Vn
PIC 16/18 Basic Kit");
delay_ms (1000);
printf (LCD_PUTC, "\f");
Mpu6050_Init () ;
delay_ms (500) ;
INT8 x;
x = Mpu6050_Read(MPU6050_RA_WHO_AM_I);

IF (x != 0x68)
{
LCD_Gotoxy (2, 0) ;
printf (LCD_PUTC, "Connection ERR!!!");
return;
}

WHILE (true)
{
accX = Mpu6050_GetData (MPU6050_RA_ACCEL_XOUT_H);
accY = Mpu6050_GetData (MPU6050_RA_ACCEL_YOUT_H);
accZ = Mpu6050_GetData (MPU6050_RA_ACCEL_ZOUT_H);
 gyroX = Mpu6050_GetData(MPU6050_RA_GYRO_XOUT_H);
gyroY = Mpu6050_GetData(MPU6050_RA_GYRO_YOUT_H);
gyroZ = Mpu6050_GetData(MPU6050_RA_GYRO_ZOUT_H);
//! lcd_gotoxy(1,1);
//! printf(LCD_PUTC,"G:X%5lu",gyroX);
//! lcd_gotoxy(10,1);
//! printf(LCD_PUTC,"Y%5lu",gyroY);
//! lcd_gotoxy(1,2);
//! printf(LCD_PUTC,"Z%5lu",gyroZ);
//! delay_ms(100);

//! lcd_gotoxy(1,1);
//! printf(LCD_PUTC,"A:X%5lu",accX);
//! lcd_gotoxy(10,1);
//! printf(LCD_PUTC,"Y%5lu",accY);
//! lcd_gotoxy(1,2);
//! printf(LCD_PUTC,"Z%5lu",accZ);
//! Delay_ms( 100 );

set_ticks(0);
accXangle = (atan2 (accY, accZ) + PI) * RAD_TO_DEG;
DOUBLE gyroXrate = (double) gyroX / 131.0;
gyroXangle += gyroXrate * ( (DOUBLE) (get_ticks () - timer) / 1000);
kalAngleX = kalmanCalculate (accXangle, gyroXrate, (get_ticks()-timer));
timer = get_ticks ();
timer=0;
set_ticks(0);
accYangle = (atan2 (accX, accZ) + PI) * RAD_TO_DEG;
DOUBLE gyroYrate = (double) gyroY / 131.0;

gyroYangle += gyroYrate * ( (DOUBLE) (get_ticks () - timer) / 1000);

kalAngleY = kalmanCalculate (accYangle, gyroYrate, (get_ticks()-timer));
timer = get_ticks ();

LCD_Gotoxy (1, 1) ;
printf (LCD_PUTC, "X=%f", kalAngleX);

LCD_Gotoxy (10, 1) ;
printf (LCD_PUTC, " " );
if(kalAngleX>200)
{
LCD_Gotoxy (10, 1) ;
printf (LCD_PUTC, "back " );
}
if((150<kalAngleX)&&(kalAngleX<170))
{
LCD_Gotoxy (10, 1) ;
printf (LCD_PUTC, "forward" );
}
if(kalAngleX<150)
{
LCD_Gotoxy (10, 1) ;
printf (LCD_PUTC, "nitro " );
}

LCD_Gotoxy (1, 2) ;
printf (LCD_PUTC, "Y=%f", kalAngleY);
LCD_Gotoxy (10, 2) ;
printf (LCD_PUTC, " " );
 if(kalAngleY>200)
{
LCD_Gotoxy (10, 2) ;
printf (LCD_PUTC, "left " );
}
if(kalAngleY<160)
{
LCD_Gotoxy (10, 2) ;
printf (LCD_PUTC, "right " );
}
delay_ms(100);
}
}