/******************************************************
*            MD22 example code for arduino            *
*     MD22 is in I2C mode LCD03 is in serial mode     *
*                                                     *
*              By James Henderson 2012                *
******************************************************/

#include <SoftwareSerial.h>
#include <Wire.h>


#define MD22ADDRESS         0x58                              // address of md 22 (all mode switches on)
#define SOFTREG             0x07                              // Byte for reading software register
#define MOTOR1              0x01                              // Byte for first motor
#define MOTOR2              0x02                              // Byte for second motor
#define ACCELLREG           0x03                              // Byte to set acelleration

#define LCD_RX              0x02                              // RX and TX pins used for LCD0303 serial port
#define LCD_TX              0x03
#define LCD03_HIDE_CUR      0x04
#define LCD03_CLEAR         0x0C

SoftwareSerial lcd_03 =  SoftwareSerial(LCD_RX, LCD_TX);      // Defines software serial port for LCD03

void setup(){
  lcd_03.begin(9600);                                         // Begin serial with LCD03
  Wire.begin();
  delay(100);                                                 // Wait for everything to be powered up
  
  lcd_03.write(LCD03_CLEAR);                                  // Clear the LCD03 screen
  lcd_03.write(LCD03_HIDE_CUR);
  getSoftware();                                              // Function that gets and prints software revision to screen
  setMode();                                                  // Function that sets mode to 2 and sets acceleration
}

void loop(){
  delay(1390);
  
  Wire.beginTransmission(MD22ADDRESS);                // Set first motor to a speed of 255
  Wire.write(MOTOR1);
  Wire.write(255);
  Wire.endTransmission();
  
  Wire.beginTransmission(MD22ADDRESS);                // Set second motor to stop
  Wire.write(MOTOR2);
  Wire.write(128);
  Wire.endTransmission();
  
  delay(1390);
  
  Wire.beginTransmission(MD22ADDRESS);                // Set first motor to speed 0
  Wire.write(MOTOR1);
  Wire.write((byte)0);                                // Values of 0 being sent using write have to be cast as a byteto stop them being misinterperted as NULL this is bug with arduino 1
  Wire.endTransmission();
  
  Wire.beginTransmission(MD22ADDRESS);                // Set second motor to stop
  Wire.write(MOTOR2);
  Wire.write(128);
  Wire.endTransmission();
  
  delay(1390);
  
  Wire.beginTransmission(MD22ADDRESS);                // Set first motor to stop
  Wire.write(MOTOR1);
  Wire.write(128);
  Wire.endTransmission();
  
  Wire.beginTransmission(MD22ADDRESS);                // Set second motor to speed 255
  Wire.write(MOTOR2);  
  Wire.write(255);
  Wire.endTransmission();
  
  delay(1390);
  
  Wire.beginTransmission(MD22ADDRESS);                // set first motor to stop
  Wire.write(MOTOR1);
  Wire.write(128);
  Wire.endTransmission();
  
  Wire.beginTransmission(MD22ADDRESS);                // Set second motor to speed 0
  Wire.write(MOTOR2);
  Wire.write((byte)0);
  Wire.endTransmission();
}

void getSoftware(){                                   // Reads and displays the software version of MD22
  Wire.beginTransmission(MD22ADDRESS);                // Calles software register
  Wire.write(SOFTREG);
  Wire.endTransmission();
  
  Wire.requestFrom(MD22ADDRESS, 1);                   // Requests one byte
  while(Wire.available() < 1);                        // Wait for it to arrive
  int software = Wire.read();                         // Get byte
  lcd_03.print("MD22 Example   V:");
  lcd_03.print(software, DEC);                        // Print byte to LCD03
}

void setMode(){
  Wire.beginTransmission(MD22ADDRESS);                // Set a value of 255 to the acceleration register
  Wire.write(ACCELLREG);
  Wire.write(0xFF);
  Wire.endTransmission();
}