Clear

	Define 		OSC	4	' Set Xtal Frequency
      
	Red_LED		Var	PortB.1	' Assign the green LED to this pin
	Green_LED	Var	PortB.2	' Assign the red LED to this pin
	' IR_Sensor	Var	PortB.4	' Assign the IR Sensor to this pin
 	SCL		VAR   	PortB.0	' I²C Clock
	SDA 		VAR 	PORTB.7	' I²C Data
	Compass 	CON	$C0	' I²C adres van compas
	Reg1 		CON	1	' Adres register 1 : Byte
	Reg2 		CON	2	' Begin adres register 2 : Word
	RegCalibrate 	CON	15	' Calibrate adres (write 255 to caibrate)
	BearingReg 	VAR	Word
	Teller 		VAR	Byte


Pause 500

High Red_LED
Pause 100
Low Red_LED
High Green_LED
Pause 100
Low Green_LED


'PIN mode with 4-bit
'Set LCD Data port
DEFINE LCD_DREG PORTA
'Set starting Data bit (0 or 4) if 4-bit bus
DEFINE LCD_DBIT 0
'Set REGISTER Select port
DEFINE LCD_RSREG PORTB
' Set LCD Register Select bit
DEFINE LCD_RSBIT 5
' Set LCD Enable port
DEFINE LCD_EREG PORTB
' Set LCD Enable bit
DEFINE LCD_EBIT 6
' Set LCD bus size (4 or 8 bits)
DEFINE LCD_BITS 4
' Set number of lines on LCD
DEFINE LCD_LINES 2

@ bcf LCDinitFlag

Symbol Cmd = $FE
Symbol ClearLCD = 1
Symbol Home = 2
Symbol Line2 = $C0
Symbol CursorOff = $0C
Symbol LineOn = $0E
Symbol BlinkOn = $0F
Symbol Left = $10
Symbol Right = $14
Symbol Procent = $25
Symbol ShiftLeft = $08

I2CREAD SDA,SCL,Compass,0,[teller]   ' Reg0 software version CMPS03
LCDOUT Cmd, ClearLCD, "Compass TEST v", dec teller


Loop:

I2CREAD SDA,SCL,Compass,Reg2,[BearingReg] ' 16 bit
'LCDOUT Cmd, Line2, dec BearingReg dig 3, dec BearingReg dig 2, dec BearingReg dig 1, ".", dec BearingReg dig 0
LCDOUT Cmd, Line2, dec3 BearingReg/10, ".", dec1 BearingReg//10, $DF   ' 000.0° - 359.9°


' I2CREAD SDA,SCL,Compass,Reg1,[Teller] ' 8 bit
' LCDOUT Cmd, Line2, dec teller dig 2, dec teller dig 1, dec teller dig 0



Pause 100

Goto Loop

Calibrate:

For Teller = 1 to 4
  LCDOUT Cmd, ClearLCD, "kwadrant", dec Teller
  ' Wacht op gebruiker (IR-remote)
  I2CWRITE SDA,SCL,Compass,RegCalibrate,[255]
Next