// SD21 Motor controller - Javalin Control

/* Notes:-

  See www.robot-electronics.co.uk for full details of the SD21 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 SD21 documentation for wiring diagram.

  This example programmed by James Burrows, on an Javalin Stamp v1, using the SD21 servo controller and
  the Parallax BOE.

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

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

public class sd21example
{
  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 SD21_ADDRESS = 0xC2;
  final static int SD21_ADDRESS_WRITE = 0xC3;

  // temp variables
  static int DataHigh;
  static int DataLow;

  // temp variable
  static int Data1 = 0;

  // store the voltage
  static int Voltage=0;

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

    System.out.print("Sofware rev is:");
    System.out.println(GetSD21softwareRev());

    while (true)
    {
      // get the voltage
      GetBatteryVoltageSD22();

      // center servo 1, use max speed (0), and position 1500 (middle)
      // send servo number as 1-21 inclusive.
      commandServo(1,0,1500);

      // wait
      CPU.delay(20000);

      // move servo all one way
      commandServo(1,0,1000);

      // wait
      CPU.delay(20000);

      // move servo all the other way.
      commandServo(1,0,2000);

      // wait
      CPU.delay(20000);

      // center it again
      commandServo(1,0,1500);
    }
  }

  public static void commandServo(int Servo,int Speed, int Position)
  {
      // Command the SD21 servo controller
      System.out.print("Send Servo Command....");
      i2cBus.start();
      i2cBus.write(SD21_ADDRESS);         // send the address
      CPU.delay(1);
      i2cBus.write((Servo-1) * 3);        // work out servo register for the servo number supplied (1-21)
      CPU.delay(1);
      i2cBus.write(Speed);                // write the speed register.  the SD21 increments the register during sequencial writes
      CPU.delay(1);                       // so Servo1 speed is written to register 0x1

      // high byte
      DataHigh = Position >> 8;           // get the high byte part of Java's 16bit integer - to leave the high byte

      // low byte
      DataLow = Position & 255;           // mask off the high byte part of Java's 16bit integer - to leave the low byte (8bits)

  /*    System.out.print("Position is ");   // un-comment this to debug!
      System.out.print((int)Position);
      System.out.print("  High Byte = ");
      System.out.print((int)Data1);
      System.out.print("  Low Byte = ");
      System.out.print((int)Data2); */

      i2cBus.write(DataLow);              // write the low byte (for servo=1 this equals register 0x2)
      CPU.delay(1);
      i2cBus.write(DataHigh);             // write the low byte (for servo=1 this equals register 0x3)
      i2cBus.stop();
      System.out.println("done");
  }

  public static void GetBatteryVoltageSD22()
  {
     // get the software rev
    System.out.print("Get Voltage.....");
    i2cBus.start();
    i2cBus.write(SD21_ADDRESS);           // send the address
    CPU.delay(1);
    i2cBus.write(65);                     // write the register (65) which contains the voltage
    CPU.delay(1);
    i2cBus.start();
    i2cBus.write(SD21_ADDRESS_WRITE);     // send the address + 1 (write byte)
    CPU.delay(1);
    Voltage = i2cBus.read(1);
    System.out.print(Voltage / 25);       // divide the 39 milivolts into volts
    System.out.print(".");
    System.out.print(Voltage % 39);       // show the remainder of the division of the volts (this is the .1 part)
    i2cBus.stop();
    System.out.println("....done");
  }

  public static int GetSD21softwareRev()
  {
     // get the software rev
    //System.out.print("Get SW Revision.....");
    i2cBus.start();
    i2cBus.write(SD21_ADDRESS);           // send the address
    CPU.delay(1);
    i2cBus.write(64);                     // write the register (64) which contains the sw revision
    CPU.delay(1);
    i2cBus.start();
    i2cBus.write(SD21_ADDRESS_WRITE);     // send the address + 1 (write byte)
    CPU.delay(1);
    Data1 = i2cBus.read(0);               // read with ack
    i2cBus.stop();
   // System.out.println("....done");
    return(Data1);
  }
}