Diff: md25.cpp

Revision:

0:e7f4a9247af2

Child:

1:8046f460a725

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/md25.cpp	Thu May 19 12:42:06 2011 +0000
@@ -0,0 +1,174 @@
+/*
+    SD21 - 21 Channel Servo Driver Module Library
+    Copyright (c) 2011 Jim Herd
+    Based on Arduino code by Richie Reynolds
+
+    The MD25 motor controller uses a +5v I2C interfaced with a protocol
+    similar to 24C04 EEPROM device.  Reading data uses a
+    WRITE/RESTART/READ sequence.
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "MD25.h"
+#include "mbed.h"
+
+/*-----------------------------------------------------------------------------
+ * Constructors
+ */
+MD25::MD25(PinName sda, PinName scl) : _i2c(sda, scl) {
+    MD25_i2cAddress =  MD25_DEFAULT_ADDRESS;
+    current_mode = MODE_0;
+};
+
+MD25::MD25(PinName sda, PinName scl, int i2cAddress) :  _i2c(sda, scl) {
+    MD25_i2cAddress = i2cAddress;
+    current_mode = MODE_0;
+};
+
+/*-----------------------------------------------------------------------------
+ * Public Methods
+ */
+
+int32_t MD25::getEncoder1() {
+    union {
+        char buffer[4];
+        int32_t data;
+    } value;
+
+    readRegisterbyte(MD25_ENCODER1_REG);   // dummy read high byte to get current encoder value
+    value.buffer[0] = readRegisterbyte(MD25_ENCODER1_REG + 3);
+    value.buffer[1] = readRegisterbyte(MD25_ENCODER1_REG + 2);
+    value.buffer[2] = readRegisterbyte(MD25_ENCODER1_REG + 1);
+    value.buffer[3] = readRegisterbyte(MD25_ENCODER1_REG + 0);
+
+    return value.data;
+}
+
+int32_t MD25::getEncoder2() {
+    union {
+        uint8_t  buffer[4];
+        int32_t  data;
+    } value;
+    
+    readRegisterbyte(MD25_ENCODER2_REG);   // dummy read high byte to get current encoder value
+    value.buffer[0] = readRegisterbyte(MD25_ENCODER2_REG + 3);
+    value.buffer[1] = readRegisterbyte(MD25_ENCODER2_REG + 2);
+    value.buffer[2] = readRegisterbyte(MD25_ENCODER2_REG + 1);
+    value.buffer[3] = readRegisterbyte(MD25_ENCODER2_REG + 0);
+
+    return value.data;
+}
+
+uint32_t MD25::getSoftwareVersion() {
+    return readRegisterbyte(MD25_SOFTWAREVER_REG);
+}
+
+float MD25::getBatteryVolts() {
+    return (float)(readRegisterbyte(MD25_VOLTAGE_REG))/10.0;
+}
+
+uint8_t MD25::getAccelerationRate() {
+    return readRegisterbyte(MD25_ACCELRATE_REG);
+}
+
+uint8_t MD25::getMotor1Speed() {
+    return readRegisterbyte(MD25_SPEED1_REG);
+}
+
+uint8_t MD25::getMotor2Speed() {
+    return readRegisterbyte(MD25_SPEED2_REG);
+}
+
+uint8_t MD25::getMotor1Current() {
+    return readRegisterbyte(MD25_CURRENT1_REG);
+}
+
+uint8_t MD25::getMotor2Current() {
+    return readRegisterbyte(MD25_CURRENT2_REG);
+}
+
+uint8_t MD25::getMode() {
+    return readRegisterbyte(MD25_MODE_REG);
+}
+
+void MD25::setSpeedRegisters(uint8_t speed_1, uint8_t speed_2) {
+    writeRegisterbyte(MD25_SPEED1_REG, speed_1);
+    writeRegisterbyte(MD25_SPEED2_REG, speed_2);
+}
+
+void MD25::stopMotor1() {
+    switch (current_mode) {
+        case MODE_0 :
+        case MODE_2 :
+            writeRegisterbyte(MD25_SPEED1_REG, 128);
+            break;
+        case MODE_1 :
+        case MODE_3 :
+            writeRegisterbyte(MD25_SPEED1_REG, 0);
+            break;
+    }
+}
+
+void MD25::stopMotor2() {
+    switch (current_mode) {
+        case MODE_0 :
+        case MODE_2 :
+            writeRegisterbyte(MD25_SPEED2_REG, 128);
+            break;
+        case MODE_1 :
+        case MODE_3 :
+            writeRegisterbyte(MD25_SPEED2_REG, 0);
+            break;
+    }
+}
+
+void MD25::stopMotors() {
+    stopMotor1();
+    stopMotor2();
+}
+
+void MD25::setMode(uint8_t mode) {
+    writeRegisterbyte(MD25_MODE_REG, mode);
+}
+
+void MD25::setAccelerationRate(uint8_t rate) {
+    writeRegisterbyte(MD25_ACCELRATE_REG, rate);
+}
+
+void MD25::setCommand(uint8_t command) {
+    writeRegisterbyte(MD25_CMD_REG, command);
+}
+
+/*
+ * Private Methods
+ */
+
+uint8_t MD25::readRegisterbyte(uint8_t reg) {
+    char  buffer;
+
+    buffer = reg;
+    _i2c.write(MD25_i2cAddress, &buffer, 1, true);       // suppress STOP condition
+    _i2c.read(MD25_i2cAddress, &buffer, 1, false);
+    return buffer;
+}
+
+void MD25::writeRegisterbyte(uint8_t reg, uint8_t value) {
+    char  buffer[2];
+
+    buffer[0] = reg;
+    buffer[1] = value;
+    _i2c.write(MD25_i2cAddress, &buffer[0], 2);
+    return;
+}