Library of routines to drive a MD25 motor control board
Dependents: Nucleo_motors HTU21D_HELLOWORLD Major_dHome pixyMajordhome ... more
md25.cpp
- Committer:
- jimherd
- Date:
- 2011-05-19
- Revision:
- 0:e7f4a9247af2
- Child:
- 1:8046f460a725
File content as of revision 0:e7f4a9247af2:
/* 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; }