md25.cpp

00001 /*
00002     SD21 - 21 Channel Servo Driver Module Library
00003     Copyright (c) 2011 Jim Herd
00004     Based on Arduino code by Richie Reynolds
00005 
00006     The MD25 motor controller uses a +5v I2C interfaced with a protocol
00007     similar to 24C04 EEPROM device.  Reading data uses a
00008     WRITE/RESTART/READ sequence.
00009 
00010     This program is free software: you can redistribute it and/or modify
00011     it under the terms of the GNU General Public License as published by
00012     the Free Software Foundation, either version 3 of the License, or
00013     (at your option) any later version.
00014 
00015     This program is distributed in the hope that it will be useful,
00016     but WITHOUT ANY WARRANTY; without even the implied warranty of
00017     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018     GNU General Public License for more details.
00019 
00020     You should have received a copy of the GNU General Public License
00021     along with this program.  If not, see <http://www.gnu.org/licenses/>.
00022 */
00023 
00024 #include "MD25.h"
00025 #include "mbed.h"
00026 
00027 /*-----------------------------------------------------------------------------
00028  * Constructors
00029  */
00030 MD25::MD25(PinName sda, PinName scl) : _i2c(sda, scl) {
00031     MD25_i2cAddress =  MD25_DEFAULT_ADDRESS;
00032     current_mode = MODE_0;
00033 };
00034 
00035 MD25::MD25(PinName sda, PinName scl, int i2cAddress) :  _i2c(sda, scl) {
00036     MD25_i2cAddress = i2cAddress;
00037     current_mode = MODE_0;
00038 };
00039 
00040 /*-----------------------------------------------------------------------------
00041  * Public Methods
00042  */
00043 
00044 int32_t MD25::getEncoder1(void) {
00045     union {
00046         char buffer[4];
00047         int32_t data;
00048     } value;
00049 
00050     readRegisterbyte(MD25_ENCODER1_REG);   // dummy read high byte to get current encoder value
00051     value.buffer[0] = readRegisterbyte(MD25_ENCODER1_REG + 3);
00052     value.buffer[1] = readRegisterbyte(MD25_ENCODER1_REG + 2);
00053     value.buffer[2] = readRegisterbyte(MD25_ENCODER1_REG + 1);
00054     value.buffer[3] = readRegisterbyte(MD25_ENCODER1_REG + 0);
00055 
00056     return value.data;
00057 }
00058 
00059 int32_t MD25::getEncoder2(void) {
00060     union {
00061         uint8_t  buffer[4];
00062         int32_t  data;
00063     } value;
00064     
00065     readRegisterbyte(MD25_ENCODER2_REG);   // dummy read high byte to get current encoder value
00066     value.buffer[0] = readRegisterbyte(MD25_ENCODER2_REG + 3);
00067     value.buffer[1] = readRegisterbyte(MD25_ENCODER2_REG + 2);
00068     value.buffer[2] = readRegisterbyte(MD25_ENCODER2_REG + 1);
00069     value.buffer[3] = readRegisterbyte(MD25_ENCODER2_REG + 0);
00070 
00071     return value.data;
00072 }
00073 
00074 uint32_t MD25::getSoftwareVersion(void) {
00075     return readRegisterbyte(MD25_SOFTWAREVER_REG);
00076 }
00077 
00078 float MD25::getBatteryVolts(void) {
00079     return (float)(readRegisterbyte(MD25_VOLTAGE_REG))/10.0;
00080 }
00081 
00082 uint8_t MD25::getAccelerationRate() {
00083     return readRegisterbyte(MD25_ACCELRATE_REG);
00084 }
00085 
00086 uint8_t MD25::getMotor1Speed(void) {
00087     return readRegisterbyte(MD25_SPEED1_REG);
00088 }
00089 
00090 uint8_t MD25::getMotor2Speed(void) {
00091     return readRegisterbyte(MD25_SPEED2_REG);
00092 }
00093 
00094 uint8_t MD25::getMotor1Current(void) {
00095     return readRegisterbyte(MD25_CURRENT1_REG);
00096 }
00097 
00098 uint8_t MD25::getMotor2Current(void) {
00099     return readRegisterbyte(MD25_CURRENT2_REG);
00100 }
00101 
00102 uint8_t MD25::getMode(void) {
00103     return readRegisterbyte(MD25_MODE_REG);
00104 }
00105 
00106 void MD25::setSpeedRegisters(uint8_t speed_1, uint8_t speed_2) {
00107     writeRegisterbyte(MD25_SPEED1_REG, speed_1);
00108     writeRegisterbyte(MD25_SPEED2_REG, speed_2);
00109 }
00110 
00111 void MD25::stopMotor1(void) {
00112     switch (current_mode) {
00113         case MODE_0 :
00114         case MODE_2 :
00115             writeRegisterbyte(MD25_SPEED1_REG, 128);
00116             break;
00117         case MODE_1 :
00118         case MODE_3 :
00119             writeRegisterbyte(MD25_SPEED1_REG, 0);
00120             break;
00121     }
00122 }
00123 
00124 void MD25::stopMotor2(void) {
00125     switch (current_mode) {
00126         case MODE_0 :
00127         case MODE_2 :
00128             writeRegisterbyte(MD25_SPEED2_REG, 128);
00129             break;
00130         case MODE_1 :
00131         case MODE_3 :
00132             writeRegisterbyte(MD25_SPEED2_REG, 0);
00133             break;
00134     }
00135 }
00136 
00137 void MD25::stopMotors(void) {
00138     stopMotor1();
00139     stopMotor2();
00140 }
00141 
00142 void MD25::setMode(uint8_t mode) {
00143     writeRegisterbyte(MD25_MODE_REG, mode);
00144     current_mode = mode;
00145 }
00146 
00147 void MD25::setAccelerationRate(uint8_t rate) {
00148     writeRegisterbyte(MD25_ACCELRATE_REG, rate);
00149 }
00150 
00151 void MD25::setCommand(uint8_t command) {
00152     writeRegisterbyte(MD25_CMD_REG, command);
00153 }
00154 
00155 /*
00156  * Private Methods
00157  */
00158 
00159 uint8_t MD25::readRegisterbyte(uint8_t reg) {
00160     char  buffer;
00161 
00162     buffer = reg;
00163     _i2c.write(MD25_i2cAddress, &buffer, 1, true);       // suppress STOP condition
00164     _i2c.read(MD25_i2cAddress, &buffer, 1, false);
00165     return buffer;
00166 }
00167 
00168 void MD25::writeRegisterbyte(uint8_t reg, uint8_t value) {
00169     char  buffer[2];
00170 
00171     buffer[0] = reg;
00172     buffer[1] = value;
00173     _i2c.write(MD25_i2cAddress, &buffer[0], 2);
00174     return;
00175 }