Library of routines to drive a MD25 motor control board
Dependents: Nucleo_motors HTU21D_HELLOWORLD Major_dHome pixyMajordhome ... more
md25.cpp@2:e575d390c730, 2011-05-20 (annotated)
- Committer:
- jimherd
- Date:
- Fri May 20 22:28:50 2011 +0000
- Revision:
- 2:e575d390c730
- Parent:
- 1:8046f460a725
Updated documentation
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jimherd | 0:e7f4a9247af2 | 1 | /* |
jimherd | 0:e7f4a9247af2 | 2 | SD21 - 21 Channel Servo Driver Module Library |
jimherd | 0:e7f4a9247af2 | 3 | Copyright (c) 2011 Jim Herd |
jimherd | 0:e7f4a9247af2 | 4 | Based on Arduino code by Richie Reynolds |
jimherd | 0:e7f4a9247af2 | 5 | |
jimherd | 0:e7f4a9247af2 | 6 | The MD25 motor controller uses a +5v I2C interfaced with a protocol |
jimherd | 0:e7f4a9247af2 | 7 | similar to 24C04 EEPROM device. Reading data uses a |
jimherd | 0:e7f4a9247af2 | 8 | WRITE/RESTART/READ sequence. |
jimherd | 0:e7f4a9247af2 | 9 | |
jimherd | 0:e7f4a9247af2 | 10 | This program is free software: you can redistribute it and/or modify |
jimherd | 0:e7f4a9247af2 | 11 | it under the terms of the GNU General Public License as published by |
jimherd | 0:e7f4a9247af2 | 12 | the Free Software Foundation, either version 3 of the License, or |
jimherd | 0:e7f4a9247af2 | 13 | (at your option) any later version. |
jimherd | 0:e7f4a9247af2 | 14 | |
jimherd | 0:e7f4a9247af2 | 15 | This program is distributed in the hope that it will be useful, |
jimherd | 0:e7f4a9247af2 | 16 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
jimherd | 0:e7f4a9247af2 | 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
jimherd | 0:e7f4a9247af2 | 18 | GNU General Public License for more details. |
jimherd | 0:e7f4a9247af2 | 19 | |
jimherd | 0:e7f4a9247af2 | 20 | You should have received a copy of the GNU General Public License |
jimherd | 0:e7f4a9247af2 | 21 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
jimherd | 0:e7f4a9247af2 | 22 | */ |
jimherd | 0:e7f4a9247af2 | 23 | |
jimherd | 0:e7f4a9247af2 | 24 | #include "MD25.h" |
jimherd | 0:e7f4a9247af2 | 25 | #include "mbed.h" |
jimherd | 0:e7f4a9247af2 | 26 | |
jimherd | 0:e7f4a9247af2 | 27 | /*----------------------------------------------------------------------------- |
jimherd | 0:e7f4a9247af2 | 28 | * Constructors |
jimherd | 0:e7f4a9247af2 | 29 | */ |
jimherd | 0:e7f4a9247af2 | 30 | MD25::MD25(PinName sda, PinName scl) : _i2c(sda, scl) { |
jimherd | 0:e7f4a9247af2 | 31 | MD25_i2cAddress = MD25_DEFAULT_ADDRESS; |
jimherd | 0:e7f4a9247af2 | 32 | current_mode = MODE_0; |
jimherd | 0:e7f4a9247af2 | 33 | }; |
jimherd | 0:e7f4a9247af2 | 34 | |
jimherd | 0:e7f4a9247af2 | 35 | MD25::MD25(PinName sda, PinName scl, int i2cAddress) : _i2c(sda, scl) { |
jimherd | 0:e7f4a9247af2 | 36 | MD25_i2cAddress = i2cAddress; |
jimherd | 0:e7f4a9247af2 | 37 | current_mode = MODE_0; |
jimherd | 0:e7f4a9247af2 | 38 | }; |
jimherd | 0:e7f4a9247af2 | 39 | |
jimherd | 0:e7f4a9247af2 | 40 | /*----------------------------------------------------------------------------- |
jimherd | 0:e7f4a9247af2 | 41 | * Public Methods |
jimherd | 0:e7f4a9247af2 | 42 | */ |
jimherd | 0:e7f4a9247af2 | 43 | |
jimherd | 1:8046f460a725 | 44 | int32_t MD25::getEncoder1(void) { |
jimherd | 0:e7f4a9247af2 | 45 | union { |
jimherd | 0:e7f4a9247af2 | 46 | char buffer[4]; |
jimherd | 0:e7f4a9247af2 | 47 | int32_t data; |
jimherd | 0:e7f4a9247af2 | 48 | } value; |
jimherd | 0:e7f4a9247af2 | 49 | |
jimherd | 0:e7f4a9247af2 | 50 | readRegisterbyte(MD25_ENCODER1_REG); // dummy read high byte to get current encoder value |
jimherd | 0:e7f4a9247af2 | 51 | value.buffer[0] = readRegisterbyte(MD25_ENCODER1_REG + 3); |
jimherd | 0:e7f4a9247af2 | 52 | value.buffer[1] = readRegisterbyte(MD25_ENCODER1_REG + 2); |
jimherd | 0:e7f4a9247af2 | 53 | value.buffer[2] = readRegisterbyte(MD25_ENCODER1_REG + 1); |
jimherd | 0:e7f4a9247af2 | 54 | value.buffer[3] = readRegisterbyte(MD25_ENCODER1_REG + 0); |
jimherd | 0:e7f4a9247af2 | 55 | |
jimherd | 0:e7f4a9247af2 | 56 | return value.data; |
jimherd | 0:e7f4a9247af2 | 57 | } |
jimherd | 0:e7f4a9247af2 | 58 | |
jimherd | 1:8046f460a725 | 59 | int32_t MD25::getEncoder2(void) { |
jimherd | 0:e7f4a9247af2 | 60 | union { |
jimherd | 0:e7f4a9247af2 | 61 | uint8_t buffer[4]; |
jimherd | 0:e7f4a9247af2 | 62 | int32_t data; |
jimherd | 0:e7f4a9247af2 | 63 | } value; |
jimherd | 0:e7f4a9247af2 | 64 | |
jimherd | 0:e7f4a9247af2 | 65 | readRegisterbyte(MD25_ENCODER2_REG); // dummy read high byte to get current encoder value |
jimherd | 0:e7f4a9247af2 | 66 | value.buffer[0] = readRegisterbyte(MD25_ENCODER2_REG + 3); |
jimherd | 0:e7f4a9247af2 | 67 | value.buffer[1] = readRegisterbyte(MD25_ENCODER2_REG + 2); |
jimherd | 0:e7f4a9247af2 | 68 | value.buffer[2] = readRegisterbyte(MD25_ENCODER2_REG + 1); |
jimherd | 0:e7f4a9247af2 | 69 | value.buffer[3] = readRegisterbyte(MD25_ENCODER2_REG + 0); |
jimherd | 0:e7f4a9247af2 | 70 | |
jimherd | 0:e7f4a9247af2 | 71 | return value.data; |
jimherd | 0:e7f4a9247af2 | 72 | } |
jimherd | 0:e7f4a9247af2 | 73 | |
jimherd | 1:8046f460a725 | 74 | uint32_t MD25::getSoftwareVersion(void) { |
jimherd | 0:e7f4a9247af2 | 75 | return readRegisterbyte(MD25_SOFTWAREVER_REG); |
jimherd | 0:e7f4a9247af2 | 76 | } |
jimherd | 0:e7f4a9247af2 | 77 | |
jimherd | 1:8046f460a725 | 78 | float MD25::getBatteryVolts(void) { |
jimherd | 0:e7f4a9247af2 | 79 | return (float)(readRegisterbyte(MD25_VOLTAGE_REG))/10.0; |
jimherd | 0:e7f4a9247af2 | 80 | } |
jimherd | 0:e7f4a9247af2 | 81 | |
jimherd | 0:e7f4a9247af2 | 82 | uint8_t MD25::getAccelerationRate() { |
jimherd | 0:e7f4a9247af2 | 83 | return readRegisterbyte(MD25_ACCELRATE_REG); |
jimherd | 0:e7f4a9247af2 | 84 | } |
jimherd | 0:e7f4a9247af2 | 85 | |
jimherd | 1:8046f460a725 | 86 | uint8_t MD25::getMotor1Speed(void) { |
jimherd | 0:e7f4a9247af2 | 87 | return readRegisterbyte(MD25_SPEED1_REG); |
jimherd | 0:e7f4a9247af2 | 88 | } |
jimherd | 0:e7f4a9247af2 | 89 | |
jimherd | 1:8046f460a725 | 90 | uint8_t MD25::getMotor2Speed(void) { |
jimherd | 0:e7f4a9247af2 | 91 | return readRegisterbyte(MD25_SPEED2_REG); |
jimherd | 0:e7f4a9247af2 | 92 | } |
jimherd | 0:e7f4a9247af2 | 93 | |
jimherd | 1:8046f460a725 | 94 | uint8_t MD25::getMotor1Current(void) { |
jimherd | 0:e7f4a9247af2 | 95 | return readRegisterbyte(MD25_CURRENT1_REG); |
jimherd | 0:e7f4a9247af2 | 96 | } |
jimherd | 0:e7f4a9247af2 | 97 | |
jimherd | 1:8046f460a725 | 98 | uint8_t MD25::getMotor2Current(void) { |
jimherd | 0:e7f4a9247af2 | 99 | return readRegisterbyte(MD25_CURRENT2_REG); |
jimherd | 0:e7f4a9247af2 | 100 | } |
jimherd | 0:e7f4a9247af2 | 101 | |
jimherd | 1:8046f460a725 | 102 | uint8_t MD25::getMode(void) { |
jimherd | 0:e7f4a9247af2 | 103 | return readRegisterbyte(MD25_MODE_REG); |
jimherd | 0:e7f4a9247af2 | 104 | } |
jimherd | 0:e7f4a9247af2 | 105 | |
jimherd | 0:e7f4a9247af2 | 106 | void MD25::setSpeedRegisters(uint8_t speed_1, uint8_t speed_2) { |
jimherd | 0:e7f4a9247af2 | 107 | writeRegisterbyte(MD25_SPEED1_REG, speed_1); |
jimherd | 0:e7f4a9247af2 | 108 | writeRegisterbyte(MD25_SPEED2_REG, speed_2); |
jimherd | 0:e7f4a9247af2 | 109 | } |
jimherd | 0:e7f4a9247af2 | 110 | |
jimherd | 1:8046f460a725 | 111 | void MD25::stopMotor1(void) { |
jimherd | 0:e7f4a9247af2 | 112 | switch (current_mode) { |
jimherd | 0:e7f4a9247af2 | 113 | case MODE_0 : |
jimherd | 0:e7f4a9247af2 | 114 | case MODE_2 : |
jimherd | 0:e7f4a9247af2 | 115 | writeRegisterbyte(MD25_SPEED1_REG, 128); |
jimherd | 0:e7f4a9247af2 | 116 | break; |
jimherd | 0:e7f4a9247af2 | 117 | case MODE_1 : |
jimherd | 0:e7f4a9247af2 | 118 | case MODE_3 : |
jimherd | 0:e7f4a9247af2 | 119 | writeRegisterbyte(MD25_SPEED1_REG, 0); |
jimherd | 0:e7f4a9247af2 | 120 | break; |
jimherd | 0:e7f4a9247af2 | 121 | } |
jimherd | 0:e7f4a9247af2 | 122 | } |
jimherd | 0:e7f4a9247af2 | 123 | |
jimherd | 1:8046f460a725 | 124 | void MD25::stopMotor2(void) { |
jimherd | 0:e7f4a9247af2 | 125 | switch (current_mode) { |
jimherd | 0:e7f4a9247af2 | 126 | case MODE_0 : |
jimherd | 0:e7f4a9247af2 | 127 | case MODE_2 : |
jimherd | 0:e7f4a9247af2 | 128 | writeRegisterbyte(MD25_SPEED2_REG, 128); |
jimherd | 0:e7f4a9247af2 | 129 | break; |
jimherd | 0:e7f4a9247af2 | 130 | case MODE_1 : |
jimherd | 0:e7f4a9247af2 | 131 | case MODE_3 : |
jimherd | 0:e7f4a9247af2 | 132 | writeRegisterbyte(MD25_SPEED2_REG, 0); |
jimherd | 0:e7f4a9247af2 | 133 | break; |
jimherd | 0:e7f4a9247af2 | 134 | } |
jimherd | 0:e7f4a9247af2 | 135 | } |
jimherd | 0:e7f4a9247af2 | 136 | |
jimherd | 1:8046f460a725 | 137 | void MD25::stopMotors(void) { |
jimherd | 0:e7f4a9247af2 | 138 | stopMotor1(); |
jimherd | 0:e7f4a9247af2 | 139 | stopMotor2(); |
jimherd | 0:e7f4a9247af2 | 140 | } |
jimherd | 0:e7f4a9247af2 | 141 | |
jimherd | 0:e7f4a9247af2 | 142 | void MD25::setMode(uint8_t mode) { |
jimherd | 0:e7f4a9247af2 | 143 | writeRegisterbyte(MD25_MODE_REG, mode); |
jimherd | 1:8046f460a725 | 144 | current_mode = mode; |
jimherd | 0:e7f4a9247af2 | 145 | } |
jimherd | 0:e7f4a9247af2 | 146 | |
jimherd | 0:e7f4a9247af2 | 147 | void MD25::setAccelerationRate(uint8_t rate) { |
jimherd | 0:e7f4a9247af2 | 148 | writeRegisterbyte(MD25_ACCELRATE_REG, rate); |
jimherd | 0:e7f4a9247af2 | 149 | } |
jimherd | 0:e7f4a9247af2 | 150 | |
jimherd | 0:e7f4a9247af2 | 151 | void MD25::setCommand(uint8_t command) { |
jimherd | 0:e7f4a9247af2 | 152 | writeRegisterbyte(MD25_CMD_REG, command); |
jimherd | 0:e7f4a9247af2 | 153 | } |
jimherd | 0:e7f4a9247af2 | 154 | |
jimherd | 0:e7f4a9247af2 | 155 | /* |
jimherd | 0:e7f4a9247af2 | 156 | * Private Methods |
jimherd | 0:e7f4a9247af2 | 157 | */ |
jimherd | 0:e7f4a9247af2 | 158 | |
jimherd | 0:e7f4a9247af2 | 159 | uint8_t MD25::readRegisterbyte(uint8_t reg) { |
jimherd | 0:e7f4a9247af2 | 160 | char buffer; |
jimherd | 0:e7f4a9247af2 | 161 | |
jimherd | 0:e7f4a9247af2 | 162 | buffer = reg; |
jimherd | 0:e7f4a9247af2 | 163 | _i2c.write(MD25_i2cAddress, &buffer, 1, true); // suppress STOP condition |
jimherd | 0:e7f4a9247af2 | 164 | _i2c.read(MD25_i2cAddress, &buffer, 1, false); |
jimherd | 0:e7f4a9247af2 | 165 | return buffer; |
jimherd | 0:e7f4a9247af2 | 166 | } |
jimherd | 0:e7f4a9247af2 | 167 | |
jimherd | 0:e7f4a9247af2 | 168 | void MD25::writeRegisterbyte(uint8_t reg, uint8_t value) { |
jimherd | 0:e7f4a9247af2 | 169 | char buffer[2]; |
jimherd | 0:e7f4a9247af2 | 170 | |
jimherd | 0:e7f4a9247af2 | 171 | buffer[0] = reg; |
jimherd | 0:e7f4a9247af2 | 172 | buffer[1] = value; |
jimherd | 0:e7f4a9247af2 | 173 | _i2c.write(MD25_i2cAddress, &buffer[0], 2); |
jimherd | 0:e7f4a9247af2 | 174 | return; |
jimherd | 0:e7f4a9247af2 | 175 | } |