#include <Wire.h>

 

MD23 md(0x58)

void setup()

{

   md.refresh();

   md.setMode(2);

}

 

void loop()

{

  md.move(140,128);

  int bat = md.batteryVolts;

}

/*

  MD23.h - Library for Devantech MD23.

  Created by Erik Oppedijk, November 1, 2009.

  Released into the public domain.

*/

#ifndef MD23_h

#define MD23_h

#include "WProgram.h"

//#include <Wire.h>

//#define md23Address 0x58                                    // Address of the MD23

#define softwareReg 0x0D                                    // Byte to read the software version

#define speed1 0x00                                         // Byte to send speed to first motor

#define speed2 0x01                                         // Byte to send speed to second motor

#define cmdByte 0x10                                        // Command byte

#define encoderOne 0x02                                     // Byte to read motor encoder 1

#define encoderTwo 0x06                                     // Byte to read motor encoder 2

#define voltRead 0x0A                                       // Byte to read battery volts

#define modeByte 0x0F

class MD23

{

  public:

    MD23(int deviceid);

    void refresh();

    void move(int spd, int turn);

    void setMode(int mode);

    void encodeReset(); 

    void stopMotor();

    void rotateDegrees(int deg);

    long e1;

    long e2;

    int batteryVolts;

    int m1;

    int m2;

    int version;

  private:

    int md23Address;

    int _mode;

};

#endif

 

/*

  Morse.cpp - Library for Devantech MD23.

  Created by Erik Oppedijk, November 1, 2009.

  Released into the public domain.

*/

#include "WProgram.h"

#include "MD23.h"

#include <Wire.h>

MD23::MD23(int deviceid)

{

   md23Address = deviceid;

   Wire.begin();

   //refresh();

   

}

void MD23::refresh()

{

 Wire.beginTransmission(md23Address);                      // Send byte to get a reading from encoder 1

  Wire.send(encoderOne);

  Wire.endTransmission();

   

  Wire.requestFrom(md23Address, 12);                         // Request 4 bytes from MD23

  while(Wire.available() < 12);                              // Wait for 4 bytes to arrive

  long firstByte = Wire.receive();                          // Recieve 4 bytes highest byte first

  long secondByte = Wire.receive();

  long thirdByte = Wire.receive();

  long fourthByte = Wire.receive();

   

  e1 = (firstByte << 24) + (secondByte << 16) + (thirdByte << 8) + fourthByte;  // Put them together

   

  firstByte = Wire.receive();                          // Get four bytes highest byte first

  secondByte = Wire.receive();

  thirdByte = Wire.receive();

  fourthByte = Wire.receive();

   

  e2 = (firstByte << 24) + (secondByte << 16) + (thirdByte << 8) + fourthByte;  // Put them together

  batteryVolts = Wire.receive();

  m1 = Wire.receive();

  m2 = Wire.receive();

  version = Wire.receive();

   

}

void MD23::setMode(int mode)

{

  Wire.beginTransmission(md23Address);

  Wire.send(modeByte);

  Wire.send(mode);                                          // Putting the value 0xF to set mode

  Wire.endTransmission();

  _mode = mode;

}

void MD23::encodeReset(){                                         // This function resets the encoder values to 0

  Wire.beginTransmission(md23Address);

  Wire.send(cmdByte);

  Wire.send(0x20);                                          // Putting the value 0x20 to reset encoders

  Wire.endTransmission();

}

void MD23::stopMotor(){                                           // Function to stop motors

  Wire.beginTransmission(md23Address);

  Wire.send(speed1);

  Wire.send(128);                                           // Sends a value of 128 to motor 2 this value stops the motor

  Wire.endTransmission();

  

} 

void MD23::move(int spd, int turn)

{

    Wire.beginTransmission(md23Address);                    // Drive motor 2 at speed value stored in x

    Wire.send(speed1);

    Wire.send(spd);

    Wire.send(turn);                                          

    Wire.endTransmission();

}

void MD23::rotateDegrees(int deg)

{

}

 

MD23 KEYWORD1

move KEYWORD2

stopMotor KEYWORD2

encodeReset KEYWORD2

setMode  KEYWORD2

refresh  KEYWORD2