// CMPS03 Motor controller - Javalin Control

/* Notes:-

  See www.robot-electronics.co.uk for full details of the CMPS03 hardware
  see www.parallax.com for details on the Javalin Stamp and the Board of Education development board (BOE)

  PIN OUTS:
    14 -> I2CBUS - SDA
    15 -> I2CBUS - SCL

  See the CMPS03 documentation for wiring diagram.

  This example programmed by James Burrows, on an Javalin Stamp v1, using the CMPS03 compass (sw rev 9) and the Parallax BOE.

  I have used 4.7K resisters to pull up the SDA and SCL lines to +5V - again see the CMPS03 documenation diagram

  COMPASS Power;             You will need to power the compass from a seperate +5V regulator.  You cannot power the compass from the
                             BS2 or Javalin internal regulators - and guess what - this is also in the CMPS03 documation (BS2 I2C example page!)!!

  COMPASS specific notes;    This is the Davantech device I have had the most trouble with (my fault not theirs!!).  I would suggest
                             checking your connections carefully and you *WILL* need to perform a compass calibration, perhaps several times.  You will
                             need to be very careful performing the calibration.   Finally read the excellent documentation and QA on
                             www.robot-electronics.co.uk site.

  COMPASS Calibration;       Implemented here but not tested.  Should work!  For a revision 9 compass (which I have) you need to implement
                             four calls to writeCalibrationPoint(); and i would suggest a delay of several seconds between calls CPU.delay(36000);
                             should be big enough!
*/

import stamp.core.*;
import stamp.peripheral.io.I2C;

public class cmps03example
{
  final static char CLS = '\u0010';
  final static char HOME = 0x01;

  // phillips i2c bus --> SDA on pin 14; SCL on pin 15
  static I2C i2cBus = new I2C(CPU.pin14, CPU.pin15);

  // constants
  final static int CMPS03_ADDRESS = 0xC0;
  final static int CMPS03_ADDRESS_WRITE = 0xC1;

  // final direction
  static int Direction=0;
  static int HighByte = 0;    // temp
  static int LowByte = 0;

  public static void main()
  {
    System.out.println(CLS);
    System.out.print(HOME);
    System.out.println("CMPS03 Example");

    // get the software revision.  You will need to know this before
    // you can calibrate!! (procedure changes after sw rev 7)
    System.out.print("Compass Software revision is: ");
    System.out.println(getCompassSWRev());

    while (true)
    {
      // read the 0-255 direction byte from the compass
      Direction = getDirectionByte();

      // show it on the message screen
      System.out.print("Direction Byte (0-255): ");
      System.out.println(Direction);

      // read the 0-359 direction byte from the compass
      Direction = getDirectionWord();

      // show it on the message screen
      System.out.print("Direction Word (0-359): ");
      System.out.println(Direction);

      // throw a blank line
      System.out.println("");

      CPU.delay(10000);
    }
  }

  public static int getDirectionWord()
  {
    // CMPS03 on I2c bus address 0XC0 & 0XC1
//    System.out.print("Get some direction....");

    i2cBus.start();
    i2cBus.write(CMPS03_ADDRESS);            // send the compass read address
    CPU.delay(1);
    i2cBus.write(2);                         // register for 0-359 direction
    CPU.delay(1);
    i2cBus.start();
    CPU.delay(1);
    i2cBus.write(CMPS03_ADDRESS_WRITE);      // send the write address for the compass
    CPU.delay(1);
    HighByte = i2cBus.read(i2cBus.ACK);      // read high byte - do not send ACK
    CPU.delay(1);
    LowByte = i2cBus.read(i2cBus.NAK);       // read low byte - send ACK (note this is register 3 as is a sequential read)
    HighByte = HighByte << 8;                // shift the high-byte left 8 bits to make it a "high byte"
    HighByte = HighByte + LowByte;           // and add to Direction integer
    HighByte = HighByte / 10;                // divide result (0-3599) by 10 (gives .1 resolution)
//    System.out.print(HighByte);
//    System.out.print("  ");
//    System.out.print(LowByte);
    i2cBus.stop();                           // stop the bus
//    System.out.println("...done");

    return(HighByte);
  }

  public static int getDirectionByte()
  {
    // CMPS03 on I2c bus address 0XC0 & 0XC1
//    System.out.print("Get some direction....");

    i2cBus.start();
    i2cBus.write(CMPS03_ADDRESS);             // send the compass read address
    CPU.delay(1);
    i2cBus.write(1);                          // register for 0-255 direction
    CPU.delay(1);
    i2cBus.start();
    CPU.delay(1);
    i2cBus.write(CMPS03_ADDRESS_WRITE);       // send the write address for the compass
    CPU.delay(1);
    HighByte = i2cBus.read(i2cBus.NAK);       // read direction byte - with NAK
    CPU.delay(1);
  //  System.out.print(HighByte);
    i2cBus.stop();                            // stop the bus
//    System.out.println("...done");

    return(HighByte);
  }

  public static int getCompassSWRev()
  {
    // CMPS03 on I2c bus address 0XC0 & 0XC1
//    System.out.print("Get software revision....");

    i2cBus.start();
    i2cBus.write(CMPS03_ADDRESS);             // send the compass read address
    CPU.delay(1);
    i2cBus.write(0);                          // register for software revision
    CPU.delay(1);
    i2cBus.start();
    CPU.delay(1);
    i2cBus.write(CMPS03_ADDRESS_WRITE);       // send the write address for the compass
    CPU.delay(1);
    HighByte = i2cBus.read(i2cBus.NAK);       // read software revision - no ACK
    CPU.delay(1);
  //  System.out.print(HighByte);
    i2cBus.stop();                            // stop the bus
//    System.out.println("...done");

    return(HighByte);
  }

  public static void writeCalibrationPoint()
  {
    // CMPS03 on I2c bus address 0XC0 & 0XC1
    i2cBus.start();
    i2cBus.write(CMPS03_ADDRESS);             // send the compass read address
    CPU.delay(1);
    i2cBus.write(15);                         // register for calibration
    CPU.delay(1);
    i2cBus.write(255);                        // 255 (0xff) to mark a point of compass
    CPU.delay(1);
    i2cBus.stop();                            // stop the bus

    System.out.println("Calibration point marked!!");
  }
}