/****************************************************************
*                    Arduino MD25 example code                  *
*                   MD25 running in serial mode                 *
*                     by James Henderson 2011                   *
*                                                               *
*  This example makes use of the LCDi2c library by Dale Wentz   *
*       http://www.arduino.cc/playground/Code/LCDi2c            *
*****************************************************************/

#include <Wire.h>
#include <LCDi2cR.h>

#define ADDRESS 0x00                    // address of md25
#define WRITESP1 0x31                   // Byte writes speed to motor 1
#define WRITEACCEL 0x33                 // Byte writes a value of acceleration
#define RESETREG 0x35                   // Byte resets encoders
#define SETMODE 0x34                    // Byte sets mode
#define READIVS 0x2C                    // Byte reads motor currents and battery voltage        
#define READENCS 0x25                   // Byte reads both encoders

LCDi2cR lcd = LCDi2cR(4,40,0x63,0);     // Defines lcd screen

long encValue = 0;

void setup(){
  Serial.begin(9600);
  Serial.print(ADDRESS, BYTE);          // Send address
  Serial.print(WRITEACCEL, BYTE);       // Send byte to write acceleration
  Serial.print(10,BYTE);                // Send value for Acceleration
  delayMicroseconds(10);                // Wait for this to be processed
  Serial.print(ADDRESS, BYTE);
  Serial.print(RESETREG, BYTE);         // Reset the encoder registers to 0
  lcd.init();                           // clears screen and sets cursor to home
  Serial.print(ADDRESS, BYTE);
  Serial.print(SETMODE, BYTE);
  Serial.print(2, BYTE);                // Set mode to 2, Both motors controlled by writing to speed 1
}

void loop(){ 
  while(encValue < 3000){               // While encoder 1 value less than 3000 move forward
    Serial.print(ADDRESS,BYTE);
    Serial.print(WRITESP1, BYTE);
    Serial.print(255, BYTE);
    encValue = readEncoder();
    readVolts();
  }
  delay(100);
  while(encValue > 100){                // While encoder 1 value greater than 100 reverse
    Serial.print(ADDRESS,BYTE); 
    Serial.print(WRITESP1, BYTE);
    Serial.print(0, BYTE);
    encValue = readEncoder();
    readVolts();
  }
  delay(100);
}

long readEncoder(){                                              // Function to read and display the value of both encoders, returns value of first encoder
  long result1 = 0; 
  long result2 = 0;
  Serial.print(ADDRESS, BYTE);
  Serial.print(READENCS, BYTE);
  while(Serial.available() < 8){}                                // Wait for 8 bytes, first 4 encoder 1 values second 4 encoder 2 values 
  result1 = Serial.read();                                    // First byte for encoder 1, HH.
  result1 <<= 8;
  result1 += Serial.read();                                    // Second byte for encoder 1, HL
  result1 <<= 8;
  result1 += Serial.read();                                    // Third byte for encoder 1, LH
  result1 <<= 8;
  result1  += Serial.read();                                    // Fourth byte for encoder 1, LL
  result2 = Serial.read();
  result2 <<= 8;
  result2 += Serial.read();
  result2 <<= 8;
  result2 += Serial.read();
  result2 <<= 8;
  result2 += Serial.read();
  lcd.setCursor(0,0);                                             // Sets cursor of lcd screen and then prints encoder values
  lcd.print("encoder1:");
  lcd.print(result1);
  lcd.print(" ");
  lcd.setCursor(1,0);
  lcd.print("encoder2:");
  lcd.print(result2);
  lcd.print(" ");
  return result1;                                   
}
  
void readVolts(){                                                 // Function reads current for both motors and battery voltage
  byte batteryVolts, mot1_current, mot2_current = 0;
  Serial.print(ADDRESS, BYTE);
  Serial.print(READIVS, BYTE);                                    // Send byte to readbattery voltage and motor currents
  while(Serial.available() < 3){}                                 // Wait for the 3 bytes to become available then get them
  batteryVolts = Serial.read();
  mot1_current = Serial.read();
  mot2_current = Serial.read();
  int vRemainder = batteryVolts % 10;                             // in order to get a better battery reading we work out the remainder of batteryVolts and then divede batteryVolts by 10, this will give us voltage to 1 decimal place
  batteryVolts = batteryVolts / 10;                               // 
  lcd.setCursor(2,0);                                             // Set cursor and then display results
  lcd.print("volts:");
  lcd.print(batteryVolts, DEC);
  lcd.print(".");
  lcd.print(vRemainder, DEC);
  lcd.setCursor(3,0);
  lcd.print("cur1:");
  lcd.print(mot1_current, DEC);
  lcd.print("  ");
  lcd.print(" cur2:");
  lcd.print(mot2_current, DEC);
  lcd.print("  ");
}