// Parallax HM55B compass example.
// Need to save trig.java and unsignedintmath.java librarys to
// the \lib\stamp\math directory under your "Javalin Stamp IDE" install directory
// librarys from Javalin Code on Yahoo Groups.
// http://forums.parallax.com/forums/default.aspx?f=8&m=56462
//
// Thanks to Peter Verkaik and diafysal both from forums.parallax.com/forums
// for the math parts of this code.
//
// Product documentation is here:  http://www.parallax.com/detail.asp?product_id=29123
//
// Author:  Javalin (aka James) on forums.parallax.com
//
// Need to seperate the DI and DO control lines - else you seem to get all %1111111's back.....
// Pin out is: pin0 = Compass Data In Pin
//             pin1 = Compass Data Enable Pin
//             pin2 = Compass Data Clock Pin
//             pin4 = Compass Data Out Pin

import stamp.math.*;
import stamp.core.CPU;

public class HM55B_compass {

  final static char HOME = 0x01;            // Position cursor upper-left
  final static char CLS = '\u0010';         // Clear Screen

  final static int HM55B_DI = CPU.pin0;     // pin outs. From the compass's point of view
  final static int HM55B_EN = CPU.pin1;
  final static int HM55B_CLK = CPU.pin2;
  final static int HM55B_DO = CPU.pin3;

  static int HM55B_x = 0;                   // variables delared
  static int HM55B_y = 0;
  static int HM55B_Status = 0;
  static int HM55B_Direction = 0;

  final static int HM55B_Ready = 12;        // constants
  final static int HM55B_Measure = 1;       // these are LSB switched for ease later.
  final static int HM55B_Report = 3;        // basically MSB 8 is LSB 1.
  final static int HM55B_Reset = 0;         // think MSB 8 Binary = %1000 and LSB 8 Binary is %0001

  public static void main()
  {
    System.out.println(CLS);
    System.out.print(HOME);
    System.out.println("HM55B Compass test:");

    // reset
    System.out.println("Reset");
    CPU.writePin (HM55B_EN,true);
    CPU.writePin (HM55B_EN,false);
    CPU.shiftOut (HM55B_DI,HM55B_CLK,4,CPU.SHIFT_LSB,HM55B_Reset);       //reset

    // measure
    System.out.println("Measure");
    CPU.writePin (HM55B_EN,true);
    CPU.writePin (HM55B_EN,false);
    CPU.shiftOut (HM55B_DI,HM55B_CLK,4,CPU.SHIFT_LSB,HM55B_Measure);    //measure

    while (HM55B_Status != HM55B_Ready)
    {
      CPU.writePin (HM55B_EN,true);
      CPU.writePin (HM55B_EN,false);

      HM55B_Status = 0;
      CPU.shiftOut (HM55B_DI,HM55B_CLK,4,CPU.SHIFT_LSB, HM55B_Report);  // report
      HM55B_Status = CPU.shiftIn(HM55B_DO,HM55B_CLK,4,CPU.POST_CLOCK_MSB);

      System.out.print("Status==");
      System.out.print(HM55B_Status);
      System.out.print("  ReadyIs==");
      System.out.println(HM55B_Ready);

      CPU.delay(10000);
    }

    // get
    System.out.println("Get Direction: ");
    HM55B_x = CPU.shiftIn(HM55B_DO,HM55B_CLK,11,CPU.POST_CLOCK_MSB); //Get x & y axis values
    HM55B_y = CPU.shiftIn(HM55B_DO,HM55B_CLK,11,CPU.POST_CLOCK_MSB);

    CPU.writePin(HM55B_EN,true);

    if ((HM55B_x & 0x0200) != 0) HM55B_x |= (short)0xF800;          //convert 11bit signed integer
    if ((HM55B_y & 0x0200) != 0) HM55B_y |= (short)0xF800;          //to 16bit signed integer

    HM55B_Direction = Trig.atan2(HM55B_y, HM55B_x) / 10;            // convert from 1200 (120.0) to 120

    HM55B_Direction = abs(HM55B_Direction);

    System.out.print("Compass: X=");
    System.out.print(HM55B_x);
    System.out.print("  Y=");
    System.out.print(HM55B_y);
    System.out.print("  Direction=");
    System.out.println(HM55B_Direction);
  }

  public static int abs(int a) {
    return (a<0)?-a:a;
  }
} 