i2c C18 hulp gevraagd

/*********************************************************************
 * set timer0 interrupt 5Hz
 * read 1 adc portA0
 * read i2c gyroscope
 ********************************************************************/
#include <p18cxxx.h>
#include <delays.h>
#include <timers.h>
#include <stdio.h>
#include <i2c.h>

#define LED_1	LATCbits.LATC0
#define LED_2	LATCbits.LATC1
#define LED_3	LATBbits.LATB5

// Global variables
unsigned int  rx_array[20];		// data array

// Function Prototypes
void InterruptHandlerHigh(void);  //needed for 'goto'
void motion_read(unsigned int* array);

// Start of program code
#pragma code HIGH_INTERRUPT_VECTOR = 0x808
void high_ISR (void)
{
    _asm
	goto InterruptHandlerHigh //jump to interrupt routine
	_endasm
}
#pragma code //return to the default code section

#pragma interrupt InterruptHandlerHigh
void InterruptHandlerHigh (void)
{	 
unsigned char adh,adl;		//Local variables
unsigned int result;

if (INTCONbits.TMR0IF) 		//check for TMR0 overflow
	{ 
	INTCONbits.TMR0IF = 0; 	//clear interrupt flag
	WriteTimer0(60859);		//Every 0.2 secs, 5Hz
	
// Read ADC
	ADCON0bits.CHS0=0;		//Read port A0
	ADCON0bits.CHS1=0;
	ADCON0bits.CHS2=0;
	ADCON0bits.CHS3=0;		//CHANNEL 0 set, note, to read Port A1, ADCON0bits.CHS0=1, page 259 of the PIC18f4550 datasheet
	ADCON0bits.GO=1;
while (ADCON0bits.GO); 		//wait for finish

//Convert high and low bytes to a 16 bit value
	adl= ADRESL;
	adh= ADRESH;
	result=adh*256;
	result=result+adl;

//set state of output
	if (result>=20000){	
	LED_1=1;	//21627 = 1.65v
	}
	else{
	LED_1=0;
	}

	LED_3=!LED_3; //to check if it is still all running

    motion_read(rx_array);	// Read data

//set state of output
	if ((((rx_array[5] & 0xFC) << 6) + rx_array[2])>=9000){	//8192 is middle of 14 bit data
	LED_2=1;
	}
	else{
	LED_2=0;
	}
	}
}

void init (void)
{
    ADCON1 |= 0x0F;  // Default all pins to digital
	INTCON2=INTCON2&0B0111111;  //Set pullups on port B

// Start out all LEDs off
	LED_1 = 0;
	LED_2 = 0;
	LED_3 = 0;

// Make LED I/O pins be outputs
	TRISCbits.TRISC0 = 0;	//leds = output
	TRISCbits.TRISC1 = 0;
	TRISBbits.TRISB5 = 0;
    TRISBbits.TRISB0 = 1;	//i2c = input
    TRISBbits.TRISB1 = 1;

//init timer
	OSCCONbits.IRCF2=1;		//Ensure the oscillator is running at a specific speed
	OSCCONbits.IRCF1=1;
	OSCCONbits.IRCF0=1;
	OpenTimer0(TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_256);
	INTCONbits.GIEH = 1; 	// enable interrupts
	INTCONbits.GIE = 1; 	// enable interrupts
	INTCONbits.TMR0IF = 0;  // clear timer 0 interrupt flag
	INTCONbits.TMR0IE = 1;  // enable the timer 0 interrupt

//init ADC
	TRISA=0b11;	//Set port A bits to input
	ADCON2bits.ADCS0=0;//Set FOSC/32
	ADCON2bits.ADCS1=1;
	ADCON2bits.ADCS2=0;
	ADCON1=0b00001101; //Set voltage reference and port A0 and A1 as analogue only, this is detailed in section 21.0, page 260 of the PIC18f4550 datasheet
	ADCON0bits.ADON=1; //Turn on ADC

//init I2C Initialize the module by sending I2C commands to make the gyromodule 'active'
  	SSPCON1 = 0b00101000;
  	SSPCON2 = 0x00;
  	SSPSTAT = 0b10000000;	//Disable SMBus and Slew rate control		
	OpenI2C(MASTER, SLEW_OFF);	
	I2C_SDA = 1;
  	I2C_SCL = 1;
	SSPADD = 0x3B; 	// Set I2C frequency
  	PIR1bits.SSPIF = 0;
  	PIR2bits.BCLIF = 0; 

// init gyro device
    IdleI2C();
    StartI2C();
    IdleI2C();
	WriteI2C(0xA6);
    IdleI2C();
    WriteI2C(0xFE);
    IdleI2C();
    WriteI2C(0x04);
    IdleI2C();   
    StopI2C();
}

// Motion Read: Read the six bytes of data from the gyro. The read must be preceded
// by sending a zero.
void motion_read(unsigned int *array) 
{
	IdleI2C();		
    StartI2C();
	IdleI2C();
    WriteI2C(0xA4);
    IdleI2C();
    WriteI2C(0x00);
    IdleI2C();   
    StopI2C();

	IdleI2C();
    StartI2C();
    IdleI2C();
    WriteI2C(0xA5);
    IdleI2C();
	array[0] = ReadI2C();
	AckI2C();
	array[1] = ReadI2C();
	AckI2C();
	array[2] = ReadI2C();
	AckI2C();
	array[3] = ReadI2C();
	AckI2C();
	array[4] = ReadI2C();
	AckI2C();
	array[5] = ReadI2C();
    NotAckI2C();
    IdleI2C();   
    StopI2C(); 
}

void main(void)
{
init();
	while(1);
}