jim herd / MD25

Library of routines to drive a MD25 motor control board

Dependents:   Nucleo_motors HTU21D_HELLOWORLD Major_dHome pixyMajordhome ... more

md25.cpp@1:8046f460a725, 2011-05-20 (annotated)

Committer:
jimherd
Date:
Fri May 20 21:18:59 2011 +0000
Revision:
1:8046f460a725
Parent:
0:e7f4a9247af2 
Mode to store set mode.

Who changed what in which revision?

UserRevisionLine numberNew 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 }