// MD22 Motor controller - Javalin Control

/* Notes:-

  See www.robot-electronics.co.uk for full details of the MD22 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 md22 documentation for wiring diagram, check that you have not got a "ground loop".

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

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

  I am using mode 0 on the MD22 to give two motors 0-128-255 representing full forward -> Stop -> full backward.
  You can use any mode with this code - with slight modifications.

  My motors are connected reversed to give both forward, both reverse on a positive/negative voltage.
  i.e.      motor  /    md22
             M1+   =    M1+
             M1-   =    M1-
             M2+   =    M2-
             M2-   =    M2+
*/

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

public class md22example
{
  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);

  // the address of the MD22 on the i2c bus - see the documentation on www.robot-electronics.com
  final static int MD22_ADDRESS = 0xB0;
  final static int MD22_ADDRESS_WRITE = 0xB1;

  // temp variable (the javalin does no garbage collection!!)
  static int Data1 = 0;

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

    // set MD22 mode - see the documentation on www.robot-electronics.com
    SetMD22Mode(0);

    // get software revision
    System.out.print("MD22 software rev is ");
    System.out.println(GetMD22softwareRev());

    // stop - 128 is stop, 0 = full ahead, 255 = full reversed
    commandMotors(128,128);

    // wait
    CPU.delay(20000);

    // forwards half
    commandMotors(64,64);

    // wait
    CPU.delay(20000);

    // stop
    commandMotors(128,128);

    // wait
    CPU.delay(20000);

    // turn
    commandMotors(64,192);

    // wait
    CPU.delay(20000);

    // stop
    commandMotors(128,128);

    // wait
    CPU.delay(20000);

    // backwards half speed
    commandMotors(192,192);

    // wait
    CPU.delay(20000);

    // stop
    commandMotors(128,128);
  }


  public static void SetMD22Mode(int MD22Mode)
  {
    // initialize the MD22 Motor controller Mode
    System.out.print("Send Motor Mode....");
    i2cBus.start();
    i2cBus.write(MD22_ADDRESS);        // send the address
    CPU.delay(1);
    i2cBus.write(0x0);                 // send the register
    CPU.delay(1);
    i2cBus.write(MD22Mode);            // send the value
    i2cBus.stop();
    System.out.println("done");
  }

  public static int GetMD22softwareRev()
  {
     // get the software rev
    //System.out.print("Get SW revision.....");
    i2cBus.start();
    i2cBus.write(MD22_ADDRESS);       // send the address
    CPU.delay(1);
    i2cBus.write(0x7);                // send the register
    CPU.delay(1);
    i2cBus.start();
    i2cBus.write(MD22_ADDRESS_WRITE); // send the address again with write bit
    CPU.delay(1);
    Data1 = i2cBus.read(0);           // read the data - and send an ACK
    i2cBus.stop();
   // System.out.println("....done");
    return(Data1);
  }

  public static void commandMotors(int motorLeft,int motorRight)
  {
    // send to bytes to the MD22 to control left and right motor control
    System.out.print("Send Motor Command.....");
    i2cBus.start();
    i2cBus.write(MD22_ADDRESS);       // send the address
    CPU.delay(1);
    i2cBus.write(0x1);                // send the register - motor one
    CPU.delay(1);
    i2cBus.write(motorLeft);          // send the value - left motor speed
    CPU.delay(1);
    i2cBus.write(motorRight);         // send the value - right motor speed - the MD22 automatically increments the register to 0x2
    i2cBus.stop();
    System.out.println("done");
  }

}