#include <htc.h>
#include <stdio.h>

#include "rs232.h"
#include "i2c.h"
#include "svastara.h"

#define ADRESA_ACC (0x30)
#define ADRESA_GYRO (0xD2)
#define ADRESA_MAG (0x3C)

#ifndef _XTAL_FREQ
#define _XTAL_FREQ 4000000
#endif 

__CONFIG (WDTE_OFF & MCLRE_OFF);

void main()
{
	char high, low;
	init_PIC();
	start(5);
	rs232_init();

	while(!i2c_init(ADRESA_ACC, 0x20, 0b00100111));
	while(!i2c_init(ADRESA_GYRO, 0x20, 0b00101111));

	while(!i2c_init(ADRESA_MAG, 0x00, 0b00111000));
	while(!i2c_init(ADRESA_MAG, 0x01, 0b00100000));
	while(!i2c_init(ADRESA_MAG, 0x02, 0b00000000));

	
	rs232_send_byte(0x0C);
	rs232_send_text("PROJEKT: 4. zadatak\rPrikupljanje, sklairanje i ispis svih podataka u 3 osi sa IMU-a.\rPress any key to continue...\r");
	
	while(RA3==1);
	__delay_ms(1000);
	rs232_send_byte(0x0C);	
	
	while(1)
	{
/************************************************/
/****Citanje i ispis sve 3 osi akcelerometra*****/
/************************************************/

		low = read_i2c (ADRESA_ACC, 0x28);
		high = read_i2c (ADRESA_ACC, 0x29);
		rs232_send_text("Acc x-os: ");
		acc_skaliranje (high, low);
		rs232_send_text(" g\r");

		low = read_i2c (ADRESA_ACC, 0x2A);
		high = read_i2c (ADRESA_ACC, 0x2B);
		rs232_send_text("Acc y-os: ");
		acc_skaliranje (high, low);
		rs232_send_text(" g\r");

		low = read_i2c (ADRESA_ACC, 0x2C);
		high = read_i2c (ADRESA_ACC, 0x2D);
		rs232_send_text("Acc z-os: ");
		acc_skaliranje (high, low);
		rs232_send_text(" g\r\r");

/************************************************/
/******Citanje i ispis sve 3 osi ziroskopa*******/
/************************************************/

		low = read_i2c (ADRESA_GYRO, 0x28);
		high = read_i2c (ADRESA_GYRO, 0x29);
		rs232_send_text("Gyro x-os: ");
		gyro_skaliranje (high, low);
		rs232_send_text(" rad/s\r");

		low = read_i2c (ADRESA_GYRO, 0x2A);
		high = read_i2c (ADRESA_GYRO, 0x2B);
		rs232_send_text("Gyro y-os: ");
		gyro_skaliranje (high, low);
		rs232_send_text(" rad/s\r");

		low = read_i2c (ADRESA_GYRO, 0x2C);
		high = read_i2c (ADRESA_GYRO, 0x2D);
		rs232_send_text("Gyro z-os: ");
		gyro_skaliranje (high, low);
		rs232_send_text(" rad/s\r\r");

/************************************************/
/*****Citanje i ispis sve 3 osi magnetometra*****/
/************************************************/
		PORTC = 0x01;
		low = read_i2c (ADRESA_MAG, 0x04);
		high = read_i2c (ADRESA_MAG, 0x03);
		rs232_send_text("Mag x-os: ");
		mag_skaliranje (high, low);
		rs232_send_text(" gauss\r");


		low = read_i2c (ADRESA_MAG, 0x08);
		high = read_i2c (ADRESA_MAG, 0x07);
		rs232_send_text("Mag y-os: ");
		mag_skaliranje (high, low);
		rs232_send_text(" gauss\r");


		low = read_i2c (ADRESA_MAG, 0x06);
		high = read_i2c (ADRESA_MAG, 0x05);
		rs232_send_text("Mag z-os: ");
		mag_skaliranje (high, low);
		rs232_send_text(" gauss\r");

		__delay_ms(100);
		rs232_send_byte(0x0C);
	}
}
