Library of routines to drive a MD25 motor control board
Dependents: Nucleo_motors HTU21D_HELLOWORLD Major_dHome pixyMajordhome ... more
Diff: md25.cpp
- Revision:
- 0:e7f4a9247af2
- Child:
- 1:8046f460a725
diff -r 000000000000 -r e7f4a9247af2 md25.cpp --- /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; +}