/**************************************
*    Arduino example code for MD49    *
*                                     *
*    By James Henderson, 2010.        *
**************************************/

#define CMD        0x00
#define GET_VER    0x29
#define GET_ENC1   0x23
#define GET_ENC2   0x24
#define GET_VI     0x2c
#define GET_ERROR  0x2d
#define SET_ACCEL  0x33
#define SET_SPEED1 0x31
#define SET_SPEED2 0x32

byte enc1a, enc1b, enc1c, enc1d = 0;
byte enc2a, enc2b, enc2c, enc2d = 0;
byte bat_volt, mot1_cur, mot2_cur = 0;
byte ver = 0;
byte error = 0;

void setup()
{
   Serial.begin(38400);
}

void loop()
{
  Serial.write(CMD);                        // command byte
  Serial.write(SET_ACCEL);                  
  Serial.write(5);                          // Set accelleration to 5
  Serial.write(CMD);
  Serial.write(SET_SPEED1);
  Serial.write(140);                        // Set motor 1 speed
  Serial.write(CMD);
  Serial.write(SET_SPEED2);
  Serial.write(140);                        // Set motor 2 speed

  Serial.write(CMD);
  Serial.write(GET_VER);                    // Recieve version back
  delay(50);
  if(Serial.available() > 0)
  {
    ver = Serial.read();
  }
  
  Serial.write(CMD);
  Serial.write(GET_ERROR);                  // Recieve error byte back
  delay(50);
  if(Serial.available() > 0)
  {
    error = Serial.read();
  }

  Serial.write(CMD);
  Serial.write(GET_VI);                     // Recieve battery volts and both motor currents back
  delay(50);
  if(Serial.available() > 2)
  {
    bat_volt = Serial.read();
    mot1_cur = Serial.read();
    mot2_cur = Serial.read();
  } 

  Serial.write(CMD);
  Serial.write(GET_ENC1);                   // Recieve encoder 1 value
  delay(50);
  if(Serial.available() > 3)
  {
    enc1a = Serial.read();
    enc1b = Serial.read();
    enc1c = Serial.read();
    enc1d = Serial.read();
  } 
  
  Serial.write(CMD);
  Serial.write(GET_ENC2);                   // Recieve encoder 2 value
  delay(50);
  if(Serial.available() > 3)
  {
    enc2a = Serial.read();
    enc2b = Serial.read();
    enc2c = Serial.read();
    enc2d = Serial.read();
  }
  
  Serial.println("");                       // Display all data to the arduino serial monitor
  Serial.println("");
  
  Serial.print("Version = ");
  Serial.println(ver,DEC);
  Serial.print("Battery = ");
  Serial.println(bat_volt,DEC);
  Serial.print("Motor 1 current = ");
  Serial.println(mot1_cur,DEC);
  Serial.print("Motor 2 current = ");
  Serial.println(mot2_cur,DEC);
  Serial.print("Error byte = ");
  Serial.println(error,HEX);

  Serial.print("Encoder 1 = ");
  Serial.print(enc1a,HEX);
  Serial.print(enc1b,HEX);
  Serial.print(enc1c,HEX);
  Serial.println(enc1d,HEX);
  
  Serial.print("Encoder 2 = ");
  Serial.print(enc2a,HEX);
  Serial.print(enc2b,HEX);
  Serial.print(enc2c,HEX);
  Serial.println(enc2d,HEX);


  Serial.println("");
  Serial.println("");
}