Revision 0:a06e2ad45792, committed 2012-02-20

Comitter:

UBAL

Date:

Mon Feb 20 10:37:14 2012 +0000

Commit message:

Working library for the MD25 motor controller.
Currently only I2C is supported.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MD25.cpp	Mon Feb 20 10:37:14 2012 +0000
@@ -0,0 +1,566 @@
+/********************************************************************
+ * @filename    MD25.cpp
+ * @author      Thomas B. Joergensen (thomas.bal.jorgensen@gmail.com)
+ * @date        06 FEB 2012
+ * @version     1.0
+ * @target      mbed (NXP LPC1768 - ARM Cortex M3 - 32-bit)
+ *
+ * @desciption  A library for interacting with the MD25 DC motor
+ *              controller. Find more at:
+ *              http://www.robot-electronics.co.uk/htm/md25tech.htm
+ *              http://www.robot-electronics.co.uk/htm/md25i2c.htm
+ *******************************************************************/
+
+/* Includes */
+#include "MD25.h"
+
+/*** CONSTRUCTOR AND DESTRUCTOR ***/
+
+MD25::MD25(I2C *i2c_interface, char i2c_address) {
+    this->i2c_interface = i2c_interface;
+    this->i2c_address = i2c_address;
+    
+    /* Default values */
+    mode = 0;
+}
+
+MD25::~MD25(void) {};
+
+/*** GENERIC METHODS ***/
+
+/**
+ * Write a number of bytes.
+ *
+ * @param reg_addr      Address of register to access.
+ * @param data          Pointer to where data is stored.
+ * @param bytes         Number of bytes to write.
+ */
+int MD25::write(char reg_addr, char *data, int bytes) {
+    /* Variables */
+    char data_out[bytes + 1];
+    
+    /* Add register address to char array */
+    data_out[0] = reg_addr;
+    
+    /* Populate data array */
+    for (int i = 1; i < bytes + 1; i++) {
+        data_out[i] = data[i - 1];
+    }
+    
+    /* Write address and data */
+    if (i2c_interface->write(i2c_address + I2C_WRITE_BIT, &data_out[0], bytes + 1, true)) {
+        return -1;
+    }
+
+    /* Return success */
+    return 0;
+}
+
+/**
+ * Read a number of bytes.
+ *
+ * @param reg_addr      Address of register to access.
+ * @param data          Pointer to where data it to be stored.
+ * @param bytes         Number of bytes to read.
+ */
+int MD25::read(char reg_addr, char *data, int bytes) {
+    /* Setup register to read */
+    if (!i2c_interface->write(i2c_address + I2C_WRITE_BIT, &reg_addr, 1, true)) {
+        /* Read register */
+        if (i2c_interface->read(i2c_address + I2C_READ_BIT, data, bytes, false)) {
+            return -1;
+        }
+    } else {
+        return -1;
+    }
+
+    /* Return success */
+    return 0;
+}
+
+/**
+ * Discover I2C devices.
+ *
+ * @return  char containing indicator of i2c device presence in form of bits
+ *          i.e. if LSB of returned char is 1 then a device is present at I2C_START_ADDR.
+ *          If bit 2 of char is 1 then a device is present at I2C_START_ADDR + 2.
+ */
+char MD25::doDiscover() {
+    /* Initialize device char */
+    char device = 0;
+    
+    /* Check for devices */
+    for (int i = 0; i < 8; i++) {
+        if (!i2c_interface->write(I2C_START_ADDR + (2 * i), NULL, 0)) { // 0 returned is ok
+            device |= (1<<i);
+        }
+    }
+        
+    /* Return device char */
+    return device;
+}
+
+/*** CONTROL METHODS ***/
+
+/**
+ * Set mode of operation.
+ *
+ * @param mode      Set mode of operation (0 - 3) (default: 0).
+ */
+int MD25::mode_set(int mode) {
+    /* Check input is valid */
+    if (mode < 0 || mode > 3) return -1;
+    
+    /* Variables */
+    char data = (char)mode;
+    
+    /* Set mode */
+    if (write(REG_MODE, &data, 1) == -1) return -1;
+    
+    /* Check set */
+    data = mode_get();    
+    if (data != mode) return -1;
+    
+    /* Set mode variable */
+    this->mode = mode;
+    
+    /* Return success */
+    return 0;
+}
+
+/**
+ * Get mode of operation.
+ */
+int MD25::mode_get() {
+    /* Variables */
+    char data;
+    
+    /* Get mode */
+    if (read(REG_MODE, &data, 1) == -1) return -1;
+    
+    /* Return mode */
+    return data;
+}
+
+/**
+ * Reset the encoder registers.
+ */
+int MD25::encoder_reset() {
+    /* Variables */
+    char data = CMD_ENCODER_RESET;
+
+    /* Reset encoders */
+    if (write(REG_COMMAND, &data, 1) == -1) return -1;
+    
+    /* Return success */
+    return 0;
+}
+
+/**
+ * Enable/disable automatic speed regulation (default: enabled).
+ *
+ * @param enabled   enable = 1 | disable = 0.
+ */
+int MD25::auto_speed_set(bool enabled) {
+    /* Variables */
+    char data;
+    
+    /* Set data */
+    if (enabled) {
+        data = CMD_AUTO_SPEED_ENABLE;
+    } else {
+        data = CMD_AUTO_SPEED_DISABLE;
+    }
+
+    /* Reset encoders */
+    if (write(REG_COMMAND, &data, 1) == -1) return -1;
+
+    /* Return success */
+    return 0;
+}
+
+/**
+ * Enable/disable 2 sec timeout of motors when no I2C comms (default: enabled).
+ *
+ * @param enabled   enable = 1 | disable = 0.
+ */
+int MD25::timeout_set(bool enabled) {
+    /* Variables */
+    char data;
+    
+    /* Set data */
+    if (enabled) {
+        data = CMD_TIMEOUT_ENABLE;
+    } else {
+        data = CMD_TIMEOUT_DISABLE;
+    }
+
+    /* Reset encoders */
+    if (write(REG_COMMAND, &data, 1) == -1) return -1;
+
+    /* Return success */
+    return 0;
+}
+
+/**
+ * Set a new I2C device address (Default: 0xB0).
+ *
+ * @param address   Address of device (B0 -> BE; inc: 2).
+ */
+int MD25::i2c_addr_set(char address) {
+    /* Check input is valid */
+    if (address < I2C_START_ADDR || address > I2C_START_ADDR + 0x0F) return -1;
+    if (address % 2 == 1) return -1;
+    
+    /* Set data */
+    char data[4];
+    data[0] = CMD_CHANGE_I2C_ADDR_1;
+    data[1] = CMD_CHANGE_I2C_ADDR_2;
+    data[2] = CMD_CHANGE_I2C_ADDR_3;
+    data[3] = address;
+    
+    /* Set new address */
+    if (write(REG_COMMAND, &data[0], 4) == -1) return -1;
+    
+    /* Set address variable */
+    i2c_address = address;
+    
+    /* Return success */
+    return 0;
+}
+
+/*** DATA METHODS ***/
+
+/**
+ * Set the speed of motor 1. (Only mode 0 or 1).
+ *
+ * @param speed     Faster the higher a number (mode 0: 0 -> 255 | mode 1: -128 to 127).
+ */
+int MD25::speed1_set(int speed) {
+    /* Check input is valid */
+    if (mode == 0) {
+        if (speed < 0 || speed >255) return -1;
+    } else if (mode == 1) {
+        if (speed < -128 || speed > 127) return -1;
+    } else {
+        return 0;
+    }
+    
+    /* Variable */
+    char data = (char)speed;
+    
+    /* Set data */
+    if (mode == 1) data = (signed char)data;
+    
+    /* Set speed */
+    if (write(REG_SPEED1, &data, 1) == -1) return -1;
+    
+    /* Return success */
+    return 0;
+}
+
+/**
+ * Get the set speed of motor 1. (Only mode 0 or 1).
+ *
+ * @return  Faster the higher a number (mode 0: 0 -> 255 | mode 1: -128 to 127).
+ */
+int MD25::speed1_get() {
+    /* Check valid mode */
+    if (mode < 0 || mode > 1) return -1;
+    
+    /* Variables */
+    char data;
+    
+    /* Get speed */
+    if (read(REG_SPEED1, &data, 1) == -1) return -1;
+    
+    /* Return speed */
+    if (mode == 1) return (signed char)data;
+    return data;
+}
+
+/**
+ * Set the speed of motor 2. (Only mode 0 or 1).
+ *
+ * @param speed     Faster the higher a number (mode 0: 0 -> 255 | mode 1: -128 to 127).
+ */
+int MD25::speed2_set(int speed) {
+    /* Check input is valid */
+    if (mode == 0) {
+        if (speed < 0 || speed >255) return -1;
+    } else if (mode == 1) {
+        if (speed < -128 || speed > 127) return -1;
+    } else {
+        return 0;
+    }
+    
+    /* Variable */
+    char data = (char)speed;
+    
+    /* Set data */
+    if (mode == 1) data = (signed char)data;
+    
+    /* Set speed */
+    if (write(REG_SPEED2, &data, 1) == -1) return -1;
+    
+    /* Return success */
+    return 0;
+}
+
+/**
+ * Get the set speed of motor 2. (Only mode 0 or 1).
+ *
+ * @return  Faster the higher a number (mode 0: 0 -> 255 | mode 1: -128 to 127).
+ */
+int MD25::speed2_get() {
+    /* Check valid mode */
+    if (mode < 0 || mode > 1) return -1;
+    
+    /* Variables */
+    char data;
+    
+    /* Get speed */
+    if (read(REG_SPEED2, &data, 1) == -1) return -1;
+    
+    /* Return speed */
+    if (mode == 1) return (signed char)data;
+    return data;
+}
+
+/**
+ * Set the speed. (Only mode 2 or 3).
+ *
+ * @param speed     Faster the higher a number (mode 2: 0 -> 255 | mode 3: -128 to 127).
+ */
+int MD25::speed_set(int speed) {
+    /* Check input is valid */
+    if (mode == 2) {
+        if (speed < 0 || speed > 255) return -1;
+    } else if (mode == 3) {
+        if (speed < -128 || speed > 127) return -1;
+    } else {
+        return 0;
+    }
+    
+    /* Set data */
+    char data = (char)speed;
+    
+    /* Set speed */
+    if (write(REG_SPEED1, &data, 1) == -1) return -1;
+    
+    /* Return success */
+    return 0;
+}
+
+/**
+ * Get the set speed. (Only mode 2 or 3).
+ *
+ * @return  Faster the higher a number (mode 2: 0 -> 255 | mode 3: -128 to 127).
+ */
+int MD25::speed_get() {
+    /* Check valid mode */
+    if (mode < 2 || mode > 3) return -1;
+    
+    /* Variables */
+    char data;
+    
+    /* Get speed */
+    if (read(REG_SPEED1, &data, 1) == -1) return -1;
+    
+    /* Return speed */
+    return data;
+}
+
+/**
+ * Set the turn. (Only mode 2 or 3).
+ *
+ * @param turn      Faster the higher a number (mode 2: 0 -> 255 | mode 3: -128 to 127).
+ */
+int MD25::turn_set(int turn) {
+    /* Check input is valid */
+    if (mode == 2) {
+        if (turn < 0 || turn > 255) return -1;
+    } else if (mode == 3) {
+        if (turn < -128 || turn > 127) return -1;
+    } else {
+        return 0;
+    }
+    
+    /* Set data */
+    char data = (char)turn;
+    
+    /* Set turn */
+    if (write(REG_SPEED2, &data, 1) == -1) return -1;
+    
+    /* Return success */
+    return 0;
+}
+
+/**
+ * Get the set turn. (Only mode 2 or 3).
+ *
+ * @return  Faster the higher a number (mode 2: 0 -> 255 | mode 3: -128 to 127).
+ */
+int MD25::turn_get() {
+    /* Check valid mode */
+    if (mode < 2 || mode > 3) return -1;
+    
+    /* Variables */
+    char data;
+    
+    /* Get turn */
+    if (read(REG_SPEED2, &data, 1) == -1) return -1;
+    
+    /* Return turn */
+    return data;
+}
+
+/**
+ * Set the desired acceleration rate.
+ *
+ * if new speed > current speed:
+ *      steps = (new speed - current speed) / acceleration register 
+ * if new speed < current speed:
+ *      steps = (current speed - new speed) / acceleration register
+ * time = steps * 25ms 
+ * Example: 
+ *      Time/step: 25ms | Current speed: 0 | New speed: 255 | Steps: 255 | Acceleration time: 6.375s.
+ * @param acceleration  Faster the higher a number (default: 5).
+ */
+int MD25::acceleration_set(int acceleration) {
+    /* Check input is valid */    
+    if (acceleration < 0 || acceleration > 255) return -1;
+    
+    /* Set data */
+    char data = (char)acceleration;
+    
+    /* Set acceleration */
+    if (write(REG_ACCELERATION_RATE, &data, 1) == -1) return -1;
+    
+    /* Return success */
+    return 0;
+}
+
+/**
+ * Get the set desired acceleration rate.
+ *
+ * if new speed > current speed:
+ *      steps = (new speed - current speed) / acceleration register 
+ * if new speed < current speed:
+ *      steps = (current speed - new speed) / acceleration register
+ * time = steps * 25ms 
+ * Example: 
+ *      Time/step: 25ms | Current speed: 0 | New speed: 255 | Steps: 255 | Acceleration time: 6.375s.
+ * @return  Faster the higher a number (default: 5).
+ */
+int MD25::acceleration_get() {
+    /* Variables */
+    char data;
+    
+    /* Get acceleration */
+    if (read(REG_ACCELERATION_RATE, &data, 1) == -1) return -1;
+    
+    /* Return acceleration */
+    return data;
+}
+
+/**
+ * Get encoder 1 position.
+ */
+int MD25::encoder1_get() {
+    /* Variables */
+    char data[4];
+    int position;
+    
+    /* Get encoder bytes */
+    if (read(REG_ENC1A, &data[0], 4) == -1) return -1;
+    
+    /* Combine the position bytes */
+    position = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
+    
+    /* Return position */
+    return position;
+}
+
+/**
+ * Get encoder 2 position.
+ */
+int MD25::encoder2_get() {
+    /* Variables */
+    char data[4];
+    int position;
+    
+    /* Get encoder bytes */
+    if (read(REG_ENC2A, &data[0], 4) == -1) return -1;
+    
+    /* Combine the position bytes */
+    position = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
+    
+    /* Return position */
+    return position;
+}
+
+/**
+ * Get battery voltage.
+ * It reads 10 times the voltage i.e. 121 for 12.1V.
+ *
+ * @return  Normalized float.
+ */
+float MD25::bat_voltage_get() {
+    /* Variables */
+    char data;
+    
+    /* Get battery voltages */
+    if (read(REG_BATTERY_VOLTS, &data, 1) == -1) return -1;
+    
+    /* Return battery voltage */
+    return (float)(data / 10.0);
+}
+
+/**
+ * Get current of motor 1.
+ * It reads 10 times the current i.e. 25 for 2.5A.
+ *
+ * @return  Normalized float.
+ */
+float MD25::motor1_current_get() {
+    /* Variables */
+    char data;
+    
+    /* Get motor current */
+    if (read(REG_MOTOR1_CURRENT, &data, 1) == -1) return -1;
+    
+    /* Return motor current */
+    return (float)(data / 10.0);
+}
+
+/**
+ * Get current of motor 2.
+ * It reads 10 times the current i.e. 25 for 2.5A.
+ *
+ * @return  Normalized float.
+ */
+float MD25::motor2_current_get() {
+    /* Variables */
+    char data;
+    
+    /* Get motor current */
+    if (read(REG_MOTOR2_CURRENT, &data, 1) == -1) return -1;
+    
+    /* Return motor current */
+    return (float)(data / 10.0);
+}
+
+/**
+ * Get the software revision number in the PIC16F873 controller.
+ */
+int MD25::software_rev_num_get() {
+    /* Variables */
+    char data;
+    
+    /* Get software revision number */
+    if (read(REG_SOFTWARE_REVISION, &data, 1) == -1) return -1;
+    
+    /* Return software revision number */
+    return data;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MD25.h	Mon Feb 20 10:37:14 2012 +0000
@@ -0,0 +1,105 @@
+/********************************************************************
+ * @filename    MD25.h
+ * @author      Thomas B. Joergensen (thomas.bal.jorgensen@gmail.com)
+ * @date        06 FEB 2012
+ * @version     1.0
+ * @target      mbed (NXP LPC1768 - ARM Cortex M3 - 32-bit)
+ *
+ * @desciption  A library for interacting with the MD25 DC motor
+ *              controller. Find more at:
+ *              http://www.robot-electronics.co.uk/htm/md25tech.htm
+ *              http://www.robot-electronics.co.uk/htm/md25i2c.htm
+ *******************************************************************/
+
+/* Includes */
+#include "mbed.h"
+
+/* REGISTERS */
+#define REG_SPEED1                  0x00    // R/W  | Motor1 speed (mode 0,1) or speed (mode 2,3)
+#define REG_SPEED2                  0x01    // R/W  | Motor2 speed (mode 0,1) or turn (mode 2,3) 
+#define REG_ENC1A                   0x02    // R    | Encoder 1 position, 1st byte (highest), capture count when read
+#define REG_ENC1B                   0x03    // R    | Encoder 1 position, 2nd byte
+#define REG_ENC1C                   0x04    // R    | Encoder 1 position, 3rd byte
+#define REG_ENC1D                   0x05    // R    | Encoder 1 position, 4th (lowest byte)
+#define REG_ENC2A                   0x06    // R    | Encoder 2 position, 1st  byte (highest), capture count when read
+#define REG_ENC2B                   0x07    // R    | Encoder 2 position, 2nd byte
+#define REG_ENC2C                   0x08    // R    | Encoder 2 position, 3rd byte
+#define REG_ENC2D                   0x09    // R    | Encoder 2 position, 4th byte (lowest byte) 
+#define REG_BATTERY_VOLTS           0x0A    // R    | The supply battery voltage
+#define REG_MOTOR1_CURRENT          0x0B    // R    | The current through motor 1
+#define REG_MOTOR2_CURRENT          0x0C    // R    | The current through motor 2
+#define REG_SOFTWARE_REVISION       0x0D    // R    | Software Revision Number
+#define REG_ACCELERATION_RATE       0x0E    // R/W  | Optional Acceleration register 
+#define REG_MODE                    0x0F    // R/W  | Mode of operation (see below)
+#define REG_COMMAND                 0x10    // R/W  | Used for reset of encoder counts and module address changes
+
+/* MODES */
+#define MODE_0                      0x00    // The meaning of the speed registers is literal speeds in the range of 0 (Full Reverse), 128 (Stop), 255 (Full Forward) (Default Setting).
+#define MODE_1                      0x01    // The meaning of the speed registers is literal speeds in the range of -128 (Full Reverse), 0 (Stop), 127 (Full Forward).
+#define MODE_2                      0x02    // Speed1 control both motors speed, and speed2 becomes the turn value. Data is in the range of 0 (Full Reverse), 128 (Stop), 255 (Full  Forward).
+#define MODE_3                      0x03    // Speed1 control both motors speed, and speed2 becomes the turn value. Data is in the range of -128 (Full Reverse), 0 (Stop), 127 (Full Forward).
+
+/* COMMANDS */
+#define CMD_ENCODER_RESET           0x20    // Resets the encoder registers to zero
+#define CMD_AUTO_SPEED_DISABLE      0x30    // Disables automatic speed regulation
+#define CMD_AUTO_SPEED_ENABLE       0x31    // Enables automatic speed regulation (default)
+#define CMD_TIMEOUT_DISABLE         0x32    // Disables 2 second timeout of motors (Version 2 onwards only)
+#define CMD_TIMEOUT_ENABLE          0x33    // Enables 2 second timeout of motors when no I2C comms (default) (Version 2 onwards only)
+#define CMD_CHANGE_I2C_ADDR_1       0xA0    // 1st in sequence to change I2C address
+#define CMD_CHANGE_I2C_ADDR_2       0xAA    // 2nd in sequence to change I2C address
+#define CMD_CHANGE_I2C_ADDR_3       0xA5    // 3rd in sequence to change I2C address
+
+/* I2C ADDRESS */
+#define I2C_START_ADDR              0xB0    // The start address of valid I2C addresses. LSB indicates R/W, so add 2 to get next valid. Last valid is 0xBE.
+#define I2C_WRITE_BIT               0x00    // Add this to I2C address to perform a write.
+#define I2C_READ_BIT                0x01    // Add this to I2C address to perform a read.
+
+class MD25 {
+private:
+    /* Communication */
+    I2C *i2c_interface;
+    
+    /* Variables */
+    int mode;
+    char i2c_address;
+    
+    /* Generic methods */
+    int write(char reg_addr, char *data, int bytes);
+    int read(char reg_addr, char *data, int bytes);
+    char doDiscover();
+public:
+    /* Constructor and Destructor */
+    MD25(I2C *i2c_interface, char i2c_address);
+    ~MD25(void);
+    
+    /* Control methods */
+    int mode_set(int mode);                 // Set the mode of operation (0 - 3) (default: 0).
+    int mode_get();                         // Get the current mode of operation.
+    int encoder_reset();                    // Resets the encoder registers.
+    int auto_speed_set(bool enabled);       // Enable/disable automatic speed regulation (default: enabled).
+    int timeout_set(bool enabled);          // Enable/disable 2 sec timeout of motors when no I2C comms (default: enabled).
+    int i2c_addr_set(char address);         // Set a new I2C device address (Default: 0xB0).
+    
+    /* Data methods */
+    int speed1_set(int speed);              // Set the speed of motor 1. (Only mode 0 or 1).
+    int speed1_get();                       // Get the set speed of motor 1. (Only mode 0 or 1).
+    int speed2_set(int speed);              // Set the speed of motor 2. (Only mode 0 or 1).
+    int speed2_get();                       // Get the set speed of motor 2. (Only mode 0 or 1).
+    
+    int speed_set(int speed);               // Set the speed. (Only mode 2 or 3).
+    int speed_get();                        // Get the set speed. (Only mode 2 or 3).
+    int turn_set(int turn);                 // Set the turn speed. (Only mode 2 or 3).
+    int turn_get();                         // Get the set turn speed. (Only mode 2 or 3).
+    
+    int acceleration_set(int acceleration); // Set a desired acceleration rate.
+    int acceleration_get();                 // Get the set desired acceleration rate.
+    
+    int encoder1_get();                     // Encoder 1 position.
+    int encoder2_get();                     // Encoder 2 position.
+    
+    float bat_voltage_get();                // Get battery voltage.
+    float motor1_current_get();             // Get current for motor 1.
+    float motor2_current_get();             // Get current for motor 2.
+    
+    int software_rev_num_get();             // Returns the software revision in the PIC16F873 controller.
+};
\ No newline at end of file