Wireless Compass Rev 0.01

CON
  _clkmode = xtal1 + pll16x
  _xinfreq = 5_000_000

  XB_Rx     = 0    ' XBee DOUT
  XB_Tx     = 1    ' XBee DIN
  XB_Baud   = 9600
  CR        = 13   ' Carriage Return value
  datapin   = 4    'SDA
  clockPin  = 5    'SCL

  WRITE_DATA     = $3C 'Used to perform a Write operation
  READ_DATA      = $3D 'Used to perform a Read operation

  CNFG_A         = $00 'Read/Write Register, Sets Data Output Rate. Default = 15Hz & 8 samples per measurement
                       '160Hz can be achieved by monitoring DRDY pin in single measurement mode.
  CNFG_B         = $01 'Read/Write Register, Sets the Device Gain(230-1370 Gauss). Default = 1090 Gauss
  MODE           = $02 'Read/Write Register, Selects the operating mode. Default = Single measurement
                       'Send $3C $02 $00 on power up to change to continuous measurement mode.
  OUTPUT_X_MSB   = $03 'Read Register, Output of X MSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  OUTPUT_X_LSB   = $04 'Read Register, Output of X LSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  OUTPUT_Z_MSB   = $05 'Read Register, Output of Z MSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  OUTPUT_Z_LSB   = $06 'Read Register, Output of Z LSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  OUTPUT_Y_MSB   = $07 'Read Register, Output of Y MSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  OUTPUT_Y_LSB   = $08 'Read Register, Output of Y LSB 8-bit value. (Will read -4096 if math overflow during bias measurement)
  STATUS         = $09 'Read Register, indicates device status.
  ID_A           = $0A 'Read Register, (ASCII value H)
  ID_B           = $0B 'Read Register, (ASCII value 4)
  ID_C           = $0C 'Read Register, (ASCII value 3)

OBJ
  XB    : "FullDuplexSerial"
  math      :   "SL32_INTEngine_2"

VAR
  long CompassStack[1000]
  long GotoAngleStack[100]
  long Angle
  long x
  long y
  long z

  byte NE
  byte SE
  byte SW
  byte NW

Pub  Start | range
XB.start(XB_Rx, XB_Tx, 0, XB_Baud) ' Initialize comms for XBee
cognew(Compass, @CompassStack)

Repeat
    XB.str(string("Heading in Degrees:", 32))
    XB.dec(Angle)
    XB.tx(13)

PUB Compass

  waitcnt(clkfreq/100_000 + cnt)      'Wait while compass has time to startup.

  setcont                             'sets measurements to go continuously

  repeat                              'Repeat indefinitely
     setpointer(OUTPUT_X_MSB)         'Start with Register OUT_X_MSB
     getRaw                           'Gather raw data from compass
     Angle := aziadjust
     waitcnt(clkfreq/80 + cnt)

PUB aziadjust  : value

{{ Converts the Azimuth to Degrees from 0 - 360. }}

NW~
NE~
SE~
SW~

  if  x =< 0
        if azimuth =< 0
           value := AZ_A
           NW := 1
        else
            NW~

        if azimuth > 0
           value := AZ_D
           SW := 1
        else
            SW~

  if x > 0
        if azimuth =< 0
          value := AZ_B
          NE := 1
        else
           NE~


        if azimuth > 0
          value := AZ_C
          SE := 1
        else
            SE~

  value := 1 #> value <# 360

  value := (value + 270) // 360

PUB SetCont

{{ Sets the Compass to Continuous output mode.}}

  start
  send(WRITE_DATA)
  send(MODE)
  send($00)
  stop

PUB SetPointer(Register)

{{ Start pointer at user specified Register. }}

start
send(WRITE_DATA)
send(Register)
stop

PUB GetRaw

{{ Get raw data from continous output.}}

  start
  send(READ_DATA)
  x := ((receive(true) << 8) | receive(true))            'RegisterA and RegisterB
  z := ((receive(true) << 8) | receive(true))
  y := ((receive(true) << 8) | receive(false))
  stop
  ~~x
  ~~z
  ~~y
  x := x
  z := z
  y := y

PRI Azimuth

    'Azimuth = arcTan(y/x)

           result := math.arctan(y,x)

PRI AZ_A        'NE

               result := ||azimuth

PRI AZ_B        'SE

               result := azimuth  + 180

PRI AZ_C        'SW

               result := azimuth  + 180

PRI AZ_D        'NW

               result := -azimuth + 360

PRI send(value) ' I²C Send data - 4 Stack Longs

  value := ((!value) >< 8)

  repeat 8
    dira[dataPin]  := value
    dira[clockPin] := false
    dira[clockPin] := true
    value >>= 1

  dira[dataPin]  := false
  dira[clockPin] := false
  result         := !(ina[dataPin])
  dira[clockPin] := true
  dira[dataPin]  := true

PRI receive(aknowledge) ' I²C receive data - 4 Stack Longs

  dira[dataPin] := false

  repeat 8
    result <<= 1
    dira[clockPin] := false
    result         |= ina[dataPin]
    dira[clockPin] := true

  dira[dataPin]  := aknowledge
  dira[clockPin] := false
  dira[clockPin] := true
  dira[dataPin]  := true

PRI start ' 3 Stack Longs

  outa[dataPin]  := false
  outa[clockPin] := false
  dira[dataPin]  := true
  dira[clockPin] := true

PRI stop ' 3 Stack Longs

  dira[clockPin] := false
  dira[dataPin]  := false

DAT

Azm     byte "Azimuth = ",0
XRaw       byte "X = ",0
YRaw       byte "Y = ",0
ZRaw       byte "Z = ",0