problem with HMC6352 compass

[Macgman2000]

Hello,

I tried to port this code from a BS2, to get it to work on a 16F877A. I am experiencing erratic
behavior. The display flickers terribly and the values are meaingless. What am I missing?

           INCLUDE "Modedefs.Bas"
@ Device pic16F877A, HS_OSC, LVP_OFF, WDT_OFF,protect_off
         DEFINE OSC 20
   

; Set port direction and turn off unused peripherals

        CMCON  = 7                      ' shut offcomparators
        TRISA = %00000000                ' Set PORTA to input "1" or output "0"
        TRISB = %00000000                ' set portB to output "0" or input "1"
        TRISC = %00000000
        TRISD = %00000000
        TRISE = %00000000

; initialize all ports to low

        portA = %00000000
        portB = %00000000
        portC = %00000000
        portD = %00000000
        portE = %00000000

; define LCD 4 line x 20, 4 bit interface 
        
        Define  LCD_DREG  PORTD  
        Define  LCD_DBIT  0  
        Define  LCD_RSREG PORTD  
        Define  LCD_RSBIT 4  
        Define  LCD_EREG  PORTD 
        Define  LCD_EBIT  5  
        Define  LCD_BITS 4
        Define  LCD_LINES  4
        define  LCD_COMMANDUS   1500
        DEFINE  LCD_DATAUS   44


'main2:

'        Lcdout $fe, 1, $fe, $80   ' Clear LCD screen  
'        Lcdout $FE, $80, "testing"     
'        Pause 2000       ' Wait .5 second
'        goto main2


' -----[ Pins/Constants/Variables ]-------------------------------------------
SDA            VAR    portB.0                ' PB0 transceives to/from SDA
SCL            VAR    portB.1                ' PB1 sends clock pulses

HMC6352_READ   CON      $43                  ' HMC6352 read address
HMC6352_WRITE  CON      $42                  ' HMC6352 write address
GET_HEADING    CON      $41                  ' Read in the calculated heading
START_CALIB    CON      $43                  ' Start the calibration routine
END_CALIB      CON      $45                  ' Finish the calibration routine

Heading        VAR      Word                 ' Heading read from compass
I2C_DATA       VAR      Byte                 ' Temporary I2C variable
I2C_LSB        VAR      Bit                  ' Temporary I2C variable




' -----[ Main Routine ]-------------------------------------------------------


DO
  pause 500
  GOSUB Compass_Get_Heading                  ' Get angle

  ;DEBUG HOME, "Heading = ", DEC Heading / 10, " degrees" ' Print aingle
Lcdout $fe, 1, $fe, $80          ' Clear LCD screen  
Lcdout $FE, $80, "Heading = ", DEC Heading / 10, " deg"     

LOOP


' -----[ Subroutines ]--------------------------------------------------------

Compass_Get_Heading:                         ' Compass module subroutine

  GOSUB I2C_Start                            ' Start communications on the I2C buss
  I2C_DATA = HMC6352_WRITE
  GOSUB I2C_Write                            ' Address the HMC6352 in write mode
  I2C_DATA = GET_HEADING
  GOSUB I2C_Write                            ' Send the get heading command
  GOSUB I2C_Stop                             ' End communications on the I2C buss

  GOSUB I2C_Start                            ' Start communications on the I2C buss
  I2C_DATA = HMC6352_READ
  GOSUB I2C_Write                            ' Address the HMC6352 in read mode
  PAUSE 6                                    ' Give the HMC6352 time to calculate the heading
  GOSUB I2C_Read
  Heading.HIGHBYTE = I2C_DATA                ' Read in the high byte
  GOSUB I2C_ACK
  GOSUB I2C_Read
  Heading.LOWBYTE = I2C_DATA                 ' Read in the low byte
  GOSUB I2C_NACK
  GOSUB I2C_Stop                             ' End communications on the I2C buss

RETURN

I2C_Start:
  LOW SDA                                    ' Pull SDA low
  LOW SCL                                    ' Pull SCL low
RETURN

I2C_Stop:
  LOW   SDA                                  ' Pull SDA low
  INPUT SCL                                  ' Let SCL float high
  INPUT SDA                                  ' Let SDA float high
RETURN

I2C_ACK:
  LOW   SDA                                  ' Pull SDA low
  INPUT SCL                                  ' Let SCL float high
  LOW   SCL                                  ' Pull SCL low
  INPUT SDA                                  ' Let SDA float high
RETURN

I2C_NACK:
  INPUT SDA                                  ' Let SDA float high
  INPUT SCL                                  ' Let SCL float high
  LOW   SCL                                  ' Pull SCL low
RETURN

I2C_Read:
  SHIFTIN SDA, SCL, MSBPRE, [I2C_DATA]       ' Read in 8 bits, MSB first
RETURN

I2C_Write:
  I2C_LSB = I2C_DATA.BIT0                    ' Store the status of the LSB
  I2C_DATA = I2C_DATA / 2
  SHIFTOUT SDA, SCL, MSBFIRST, [I2C_DATA\7]  ' Write out the first 7 bits, MSB first
  IF I2C_LSB THEN 
    INPUT SDA 
  ELSE 
    LOW SDA     ' Write the 8th bit
  endif
  INPUT SCL                                  ' Using an open collector output
  LOW SCL
  INPUT SDA                                  ' Leave SDA as an input
  INPUT SCL                                  ' Ignore the ACK bit
  LOW SCL
RETURN