Thomas Jørgensen / MD25

A library for the MD25 motor controller from www.robot-electronics.co.uk. The MD25 supports both I2C and UART, but this library is designed for use with I2C only.

MD25.cpp@0:a06e2ad45792, 2012-02-20 (annotated)

Committer:
UBAL
Date:
Mon Feb 20 10:37:14 2012 +0000
Revision:
0:a06e2ad45792
Working library for the MD25 motor controller.

Currently only I2C is supported.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
UBAL 0:a06e2ad45792 1 /********************************************************************
UBAL 0:a06e2ad45792 2 * @filename MD25.cpp
UBAL 0:a06e2ad45792 3 * @author Thomas B. Joergensen (thomas.bal.jorgensen@gmail.com)
UBAL 0:a06e2ad45792 4 * @date 06 FEB 2012
UBAL 0:a06e2ad45792 5 * @version 1.0
UBAL 0:a06e2ad45792 6 * @target mbed (NXP LPC1768 - ARM Cortex M3 - 32-bit)
UBAL 0:a06e2ad45792 7 *
UBAL 0:a06e2ad45792 8 * @desciption A library for interacting with the MD25 DC motor
UBAL 0:a06e2ad45792 9 * controller. Find more at:
UBAL 0:a06e2ad45792 10 * http://www.robot-electronics.co.uk/htm/md25tech.htm
UBAL 0:a06e2ad45792 11 * http://www.robot-electronics.co.uk/htm/md25i2c.htm
UBAL 0:a06e2ad45792 12 *******************************************************************/
UBAL 0:a06e2ad45792 13
UBAL 0:a06e2ad45792 14 /* Includes */
UBAL 0:a06e2ad45792 15 #include "MD25.h"
UBAL 0:a06e2ad45792 16
UBAL 0:a06e2ad45792 17 /*** CONSTRUCTOR AND DESTRUCTOR ***/
UBAL 0:a06e2ad45792 18
UBAL 0:a06e2ad45792 19 MD25::MD25(I2C *i2c_interface, char i2c_address) {
UBAL 0:a06e2ad45792 20 this->i2c_interface = i2c_interface;
UBAL 0:a06e2ad45792 21 this->i2c_address = i2c_address;
UBAL 0:a06e2ad45792 22
UBAL 0:a06e2ad45792 23 /* Default values */
UBAL 0:a06e2ad45792 24 mode = 0;
UBAL 0:a06e2ad45792 25 }
UBAL 0:a06e2ad45792 26
UBAL 0:a06e2ad45792 27 MD25::~MD25(void) {};
UBAL 0:a06e2ad45792 28
UBAL 0:a06e2ad45792 29 /*** GENERIC METHODS ***/
UBAL 0:a06e2ad45792 30
UBAL 0:a06e2ad45792 31 /**
UBAL 0:a06e2ad45792 32 * Write a number of bytes.
UBAL 0:a06e2ad45792 33 *
UBAL 0:a06e2ad45792 34 * @param reg_addr Address of register to access.
UBAL 0:a06e2ad45792 35 * @param data Pointer to where data is stored.
UBAL 0:a06e2ad45792 36 * @param bytes Number of bytes to write.
UBAL 0:a06e2ad45792 37 */
UBAL 0:a06e2ad45792 38 int MD25::write(char reg_addr, char *data, int bytes) {
UBAL 0:a06e2ad45792 39 /* Variables */
UBAL 0:a06e2ad45792 40 char data_out[bytes + 1];
UBAL 0:a06e2ad45792 41
UBAL 0:a06e2ad45792 42 /* Add register address to char array */
UBAL 0:a06e2ad45792 43 data_out[0] = reg_addr;
UBAL 0:a06e2ad45792 44
UBAL 0:a06e2ad45792 45 /* Populate data array */
UBAL 0:a06e2ad45792 46 for (int i = 1; i < bytes + 1; i++) {
UBAL 0:a06e2ad45792 47 data_out[i] = data[i - 1];
UBAL 0:a06e2ad45792 48 }
UBAL 0:a06e2ad45792 49
UBAL 0:a06e2ad45792 50 /* Write address and data */
UBAL 0:a06e2ad45792 51 if (i2c_interface->write(i2c_address + I2C_WRITE_BIT, &data_out[0], bytes + 1, true)) {
UBAL 0:a06e2ad45792 52 return -1;
UBAL 0:a06e2ad45792 53 }
UBAL 0:a06e2ad45792 54
UBAL 0:a06e2ad45792 55 /* Return success */
UBAL 0:a06e2ad45792 56 return 0;
UBAL 0:a06e2ad45792 57 }
UBAL 0:a06e2ad45792 58
UBAL 0:a06e2ad45792 59 /**
UBAL 0:a06e2ad45792 60 * Read a number of bytes.
UBAL 0:a06e2ad45792 61 *
UBAL 0:a06e2ad45792 62 * @param reg_addr Address of register to access.
UBAL 0:a06e2ad45792 63 * @param data Pointer to where data it to be stored.
UBAL 0:a06e2ad45792 64 * @param bytes Number of bytes to read.
UBAL 0:a06e2ad45792 65 */
UBAL 0:a06e2ad45792 66 int MD25::read(char reg_addr, char *data, int bytes) {
UBAL 0:a06e2ad45792 67 /* Setup register to read */
UBAL 0:a06e2ad45792 68 if (!i2c_interface->write(i2c_address + I2C_WRITE_BIT, &reg_addr, 1, true)) {
UBAL 0:a06e2ad45792 69 /* Read register */
UBAL 0:a06e2ad45792 70 if (i2c_interface->read(i2c_address + I2C_READ_BIT, data, bytes, false)) {
UBAL 0:a06e2ad45792 71 return -1;
UBAL 0:a06e2ad45792 72 }
UBAL 0:a06e2ad45792 73 } else {
UBAL 0:a06e2ad45792 74 return -1;
UBAL 0:a06e2ad45792 75 }
UBAL 0:a06e2ad45792 76
UBAL 0:a06e2ad45792 77 /* Return success */
UBAL 0:a06e2ad45792 78 return 0;
UBAL 0:a06e2ad45792 79 }
UBAL 0:a06e2ad45792 80
UBAL 0:a06e2ad45792 81 /**
UBAL 0:a06e2ad45792 82 * Discover I2C devices.
UBAL 0:a06e2ad45792 83 *
UBAL 0:a06e2ad45792 84 * @return char containing indicator of i2c device presence in form of bits
UBAL 0:a06e2ad45792 85 * i.e. if LSB of returned char is 1 then a device is present at I2C_START_ADDR.
UBAL 0:a06e2ad45792 86 * If bit 2 of char is 1 then a device is present at I2C_START_ADDR + 2.
UBAL 0:a06e2ad45792 87 */
UBAL 0:a06e2ad45792 88 char MD25::doDiscover() {
UBAL 0:a06e2ad45792 89 /* Initialize device char */
UBAL 0:a06e2ad45792 90 char device = 0;
UBAL 0:a06e2ad45792 91
UBAL 0:a06e2ad45792 92 /* Check for devices */
UBAL 0:a06e2ad45792 93 for (int i = 0; i < 8; i++) {
UBAL 0:a06e2ad45792 94 if (!i2c_interface->write(I2C_START_ADDR + (2 * i), NULL, 0)) { // 0 returned is ok
UBAL 0:a06e2ad45792 95 device |= (1<<i);
UBAL 0:a06e2ad45792 96 }
UBAL 0:a06e2ad45792 97 }
UBAL 0:a06e2ad45792 98
UBAL 0:a06e2ad45792 99 /* Return device char */
UBAL 0:a06e2ad45792 100 return device;
UBAL 0:a06e2ad45792 101 }
UBAL 0:a06e2ad45792 102
UBAL 0:a06e2ad45792 103 /*** CONTROL METHODS ***/
UBAL 0:a06e2ad45792 104
UBAL 0:a06e2ad45792 105 /**
UBAL 0:a06e2ad45792 106 * Set mode of operation.
UBAL 0:a06e2ad45792 107 *
UBAL 0:a06e2ad45792 108 * @param mode Set mode of operation (0 - 3) (default: 0).
UBAL 0:a06e2ad45792 109 */
UBAL 0:a06e2ad45792 110 int MD25::mode_set(int mode) {
UBAL 0:a06e2ad45792 111 /* Check input is valid */
UBAL 0:a06e2ad45792 112 if (mode < 0 || mode > 3) return -1;
UBAL 0:a06e2ad45792 113
UBAL 0:a06e2ad45792 114 /* Variables */
UBAL 0:a06e2ad45792 115 char data = (char)mode;
UBAL 0:a06e2ad45792 116
UBAL 0:a06e2ad45792 117 /* Set mode */
UBAL 0:a06e2ad45792 118 if (write(REG_MODE, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 119
UBAL 0:a06e2ad45792 120 /* Check set */
UBAL 0:a06e2ad45792 121 data = mode_get();
UBAL 0:a06e2ad45792 122 if (data != mode) return -1;
UBAL 0:a06e2ad45792 123
UBAL 0:a06e2ad45792 124 /* Set mode variable */
UBAL 0:a06e2ad45792 125 this->mode = mode;
UBAL 0:a06e2ad45792 126
UBAL 0:a06e2ad45792 127 /* Return success */
UBAL 0:a06e2ad45792 128 return 0;
UBAL 0:a06e2ad45792 129 }
UBAL 0:a06e2ad45792 130
UBAL 0:a06e2ad45792 131 /**
UBAL 0:a06e2ad45792 132 * Get mode of operation.
UBAL 0:a06e2ad45792 133 */
UBAL 0:a06e2ad45792 134 int MD25::mode_get() {
UBAL 0:a06e2ad45792 135 /* Variables */
UBAL 0:a06e2ad45792 136 char data;
UBAL 0:a06e2ad45792 137
UBAL 0:a06e2ad45792 138 /* Get mode */
UBAL 0:a06e2ad45792 139 if (read(REG_MODE, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 140
UBAL 0:a06e2ad45792 141 /* Return mode */
UBAL 0:a06e2ad45792 142 return data;
UBAL 0:a06e2ad45792 143 }
UBAL 0:a06e2ad45792 144
UBAL 0:a06e2ad45792 145 /**
UBAL 0:a06e2ad45792 146 * Reset the encoder registers.
UBAL 0:a06e2ad45792 147 */
UBAL 0:a06e2ad45792 148 int MD25::encoder_reset() {
UBAL 0:a06e2ad45792 149 /* Variables */
UBAL 0:a06e2ad45792 150 char data = CMD_ENCODER_RESET;
UBAL 0:a06e2ad45792 151
UBAL 0:a06e2ad45792 152 /* Reset encoders */
UBAL 0:a06e2ad45792 153 if (write(REG_COMMAND, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 154
UBAL 0:a06e2ad45792 155 /* Return success */
UBAL 0:a06e2ad45792 156 return 0;
UBAL 0:a06e2ad45792 157 }
UBAL 0:a06e2ad45792 158
UBAL 0:a06e2ad45792 159 /**
UBAL 0:a06e2ad45792 160 * Enable/disable automatic speed regulation (default: enabled).
UBAL 0:a06e2ad45792 161 *
UBAL 0:a06e2ad45792 162 * @param enabled enable = 1 | disable = 0.
UBAL 0:a06e2ad45792 163 */
UBAL 0:a06e2ad45792 164 int MD25::auto_speed_set(bool enabled) {
UBAL 0:a06e2ad45792 165 /* Variables */
UBAL 0:a06e2ad45792 166 char data;
UBAL 0:a06e2ad45792 167
UBAL 0:a06e2ad45792 168 /* Set data */
UBAL 0:a06e2ad45792 169 if (enabled) {
UBAL 0:a06e2ad45792 170 data = CMD_AUTO_SPEED_ENABLE;
UBAL 0:a06e2ad45792 171 } else {
UBAL 0:a06e2ad45792 172 data = CMD_AUTO_SPEED_DISABLE;
UBAL 0:a06e2ad45792 173 }
UBAL 0:a06e2ad45792 174
UBAL 0:a06e2ad45792 175 /* Reset encoders */
UBAL 0:a06e2ad45792 176 if (write(REG_COMMAND, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 177
UBAL 0:a06e2ad45792 178 /* Return success */
UBAL 0:a06e2ad45792 179 return 0;
UBAL 0:a06e2ad45792 180 }
UBAL 0:a06e2ad45792 181
UBAL 0:a06e2ad45792 182 /**
UBAL 0:a06e2ad45792 183 * Enable/disable 2 sec timeout of motors when no I2C comms (default: enabled).
UBAL 0:a06e2ad45792 184 *
UBAL 0:a06e2ad45792 185 * @param enabled enable = 1 | disable = 0.
UBAL 0:a06e2ad45792 186 */
UBAL 0:a06e2ad45792 187 int MD25::timeout_set(bool enabled) {
UBAL 0:a06e2ad45792 188 /* Variables */
UBAL 0:a06e2ad45792 189 char data;
UBAL 0:a06e2ad45792 190
UBAL 0:a06e2ad45792 191 /* Set data */
UBAL 0:a06e2ad45792 192 if (enabled) {
UBAL 0:a06e2ad45792 193 data = CMD_TIMEOUT_ENABLE;
UBAL 0:a06e2ad45792 194 } else {
UBAL 0:a06e2ad45792 195 data = CMD_TIMEOUT_DISABLE;
UBAL 0:a06e2ad45792 196 }
UBAL 0:a06e2ad45792 197
UBAL 0:a06e2ad45792 198 /* Reset encoders */
UBAL 0:a06e2ad45792 199 if (write(REG_COMMAND, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 200
UBAL 0:a06e2ad45792 201 /* Return success */
UBAL 0:a06e2ad45792 202 return 0;
UBAL 0:a06e2ad45792 203 }
UBAL 0:a06e2ad45792 204
UBAL 0:a06e2ad45792 205 /**
UBAL 0:a06e2ad45792 206 * Set a new I2C device address (Default: 0xB0).
UBAL 0:a06e2ad45792 207 *
UBAL 0:a06e2ad45792 208 * @param address Address of device (B0 -> BE; inc: 2).
UBAL 0:a06e2ad45792 209 */
UBAL 0:a06e2ad45792 210 int MD25::i2c_addr_set(char address) {
UBAL 0:a06e2ad45792 211 /* Check input is valid */
UBAL 0:a06e2ad45792 212 if (address < I2C_START_ADDR || address > I2C_START_ADDR + 0x0F) return -1;
UBAL 0:a06e2ad45792 213 if (address % 2 == 1) return -1;
UBAL 0:a06e2ad45792 214
UBAL 0:a06e2ad45792 215 /* Set data */
UBAL 0:a06e2ad45792 216 char data[4];
UBAL 0:a06e2ad45792 217 data[0] = CMD_CHANGE_I2C_ADDR_1;
UBAL 0:a06e2ad45792 218 data[1] = CMD_CHANGE_I2C_ADDR_2;
UBAL 0:a06e2ad45792 219 data[2] = CMD_CHANGE_I2C_ADDR_3;
UBAL 0:a06e2ad45792 220 data[3] = address;
UBAL 0:a06e2ad45792 221
UBAL 0:a06e2ad45792 222 /* Set new address */
UBAL 0:a06e2ad45792 223 if (write(REG_COMMAND, &data[0], 4) == -1) return -1;
UBAL 0:a06e2ad45792 224
UBAL 0:a06e2ad45792 225 /* Set address variable */
UBAL 0:a06e2ad45792 226 i2c_address = address;
UBAL 0:a06e2ad45792 227
UBAL 0:a06e2ad45792 228 /* Return success */
UBAL 0:a06e2ad45792 229 return 0;
UBAL 0:a06e2ad45792 230 }
UBAL 0:a06e2ad45792 231
UBAL 0:a06e2ad45792 232 /*** DATA METHODS ***/
UBAL 0:a06e2ad45792 233
UBAL 0:a06e2ad45792 234 /**
UBAL 0:a06e2ad45792 235 * Set the speed of motor 1. (Only mode 0 or 1).
UBAL 0:a06e2ad45792 236 *
UBAL 0:a06e2ad45792 237 * @param speed Faster the higher a number (mode 0: 0 -> 255 | mode 1: -128 to 127).
UBAL 0:a06e2ad45792 238 */
UBAL 0:a06e2ad45792 239 int MD25::speed1_set(int speed) {
UBAL 0:a06e2ad45792 240 /* Check input is valid */
UBAL 0:a06e2ad45792 241 if (mode == 0) {
UBAL 0:a06e2ad45792 242 if (speed < 0 || speed >255) return -1;
UBAL 0:a06e2ad45792 243 } else if (mode == 1) {
UBAL 0:a06e2ad45792 244 if (speed < -128 || speed > 127) return -1;
UBAL 0:a06e2ad45792 245 } else {
UBAL 0:a06e2ad45792 246 return 0;
UBAL 0:a06e2ad45792 247 }
UBAL 0:a06e2ad45792 248
UBAL 0:a06e2ad45792 249 /* Variable */
UBAL 0:a06e2ad45792 250 char data = (char)speed;
UBAL 0:a06e2ad45792 251
UBAL 0:a06e2ad45792 252 /* Set data */
UBAL 0:a06e2ad45792 253 if (mode == 1) data = (signed char)data;
UBAL 0:a06e2ad45792 254
UBAL 0:a06e2ad45792 255 /* Set speed */
UBAL 0:a06e2ad45792 256 if (write(REG_SPEED1, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 257
UBAL 0:a06e2ad45792 258 /* Return success */
UBAL 0:a06e2ad45792 259 return 0;
UBAL 0:a06e2ad45792 260 }
UBAL 0:a06e2ad45792 261
UBAL 0:a06e2ad45792 262 /**
UBAL 0:a06e2ad45792 263 * Get the set speed of motor 1. (Only mode 0 or 1).
UBAL 0:a06e2ad45792 264 *
UBAL 0:a06e2ad45792 265 * @return Faster the higher a number (mode 0: 0 -> 255 | mode 1: -128 to 127).
UBAL 0:a06e2ad45792 266 */
UBAL 0:a06e2ad45792 267 int MD25::speed1_get() {
UBAL 0:a06e2ad45792 268 /* Check valid mode */
UBAL 0:a06e2ad45792 269 if (mode < 0 || mode > 1) return -1;
UBAL 0:a06e2ad45792 270
UBAL 0:a06e2ad45792 271 /* Variables */
UBAL 0:a06e2ad45792 272 char data;
UBAL 0:a06e2ad45792 273
UBAL 0:a06e2ad45792 274 /* Get speed */
UBAL 0:a06e2ad45792 275 if (read(REG_SPEED1, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 276
UBAL 0:a06e2ad45792 277 /* Return speed */
UBAL 0:a06e2ad45792 278 if (mode == 1) return (signed char)data;
UBAL 0:a06e2ad45792 279 return data;
UBAL 0:a06e2ad45792 280 }
UBAL 0:a06e2ad45792 281
UBAL 0:a06e2ad45792 282 /**
UBAL 0:a06e2ad45792 283 * Set the speed of motor 2. (Only mode 0 or 1).
UBAL 0:a06e2ad45792 284 *
UBAL 0:a06e2ad45792 285 * @param speed Faster the higher a number (mode 0: 0 -> 255 | mode 1: -128 to 127).
UBAL 0:a06e2ad45792 286 */
UBAL 0:a06e2ad45792 287 int MD25::speed2_set(int speed) {
UBAL 0:a06e2ad45792 288 /* Check input is valid */
UBAL 0:a06e2ad45792 289 if (mode == 0) {
UBAL 0:a06e2ad45792 290 if (speed < 0 || speed >255) return -1;
UBAL 0:a06e2ad45792 291 } else if (mode == 1) {
UBAL 0:a06e2ad45792 292 if (speed < -128 || speed > 127) return -1;
UBAL 0:a06e2ad45792 293 } else {
UBAL 0:a06e2ad45792 294 return 0;
UBAL 0:a06e2ad45792 295 }
UBAL 0:a06e2ad45792 296
UBAL 0:a06e2ad45792 297 /* Variable */
UBAL 0:a06e2ad45792 298 char data = (char)speed;
UBAL 0:a06e2ad45792 299
UBAL 0:a06e2ad45792 300 /* Set data */
UBAL 0:a06e2ad45792 301 if (mode == 1) data = (signed char)data;
UBAL 0:a06e2ad45792 302
UBAL 0:a06e2ad45792 303 /* Set speed */
UBAL 0:a06e2ad45792 304 if (write(REG_SPEED2, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 305
UBAL 0:a06e2ad45792 306 /* Return success */
UBAL 0:a06e2ad45792 307 return 0;
UBAL 0:a06e2ad45792 308 }
UBAL 0:a06e2ad45792 309
UBAL 0:a06e2ad45792 310 /**
UBAL 0:a06e2ad45792 311 * Get the set speed of motor 2. (Only mode 0 or 1).
UBAL 0:a06e2ad45792 312 *
UBAL 0:a06e2ad45792 313 * @return Faster the higher a number (mode 0: 0 -> 255 | mode 1: -128 to 127).
UBAL 0:a06e2ad45792 314 */
UBAL 0:a06e2ad45792 315 int MD25::speed2_get() {
UBAL 0:a06e2ad45792 316 /* Check valid mode */
UBAL 0:a06e2ad45792 317 if (mode < 0 || mode > 1) return -1;
UBAL 0:a06e2ad45792 318
UBAL 0:a06e2ad45792 319 /* Variables */
UBAL 0:a06e2ad45792 320 char data;
UBAL 0:a06e2ad45792 321
UBAL 0:a06e2ad45792 322 /* Get speed */
UBAL 0:a06e2ad45792 323 if (read(REG_SPEED2, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 324
UBAL 0:a06e2ad45792 325 /* Return speed */
UBAL 0:a06e2ad45792 326 if (mode == 1) return (signed char)data;
UBAL 0:a06e2ad45792 327 return data;
UBAL 0:a06e2ad45792 328 }
UBAL 0:a06e2ad45792 329
UBAL 0:a06e2ad45792 330 /**
UBAL 0:a06e2ad45792 331 * Set the speed. (Only mode 2 or 3).
UBAL 0:a06e2ad45792 332 *
UBAL 0:a06e2ad45792 333 * @param speed Faster the higher a number (mode 2: 0 -> 255 | mode 3: -128 to 127).
UBAL 0:a06e2ad45792 334 */
UBAL 0:a06e2ad45792 335 int MD25::speed_set(int speed) {
UBAL 0:a06e2ad45792 336 /* Check input is valid */
UBAL 0:a06e2ad45792 337 if (mode == 2) {
UBAL 0:a06e2ad45792 338 if (speed < 0 || speed > 255) return -1;
UBAL 0:a06e2ad45792 339 } else if (mode == 3) {
UBAL 0:a06e2ad45792 340 if (speed < -128 || speed > 127) return -1;
UBAL 0:a06e2ad45792 341 } else {
UBAL 0:a06e2ad45792 342 return 0;
UBAL 0:a06e2ad45792 343 }
UBAL 0:a06e2ad45792 344
UBAL 0:a06e2ad45792 345 /* Set data */
UBAL 0:a06e2ad45792 346 char data = (char)speed;
UBAL 0:a06e2ad45792 347
UBAL 0:a06e2ad45792 348 /* Set speed */
UBAL 0:a06e2ad45792 349 if (write(REG_SPEED1, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 350
UBAL 0:a06e2ad45792 351 /* Return success */
UBAL 0:a06e2ad45792 352 return 0;
UBAL 0:a06e2ad45792 353 }
UBAL 0:a06e2ad45792 354
UBAL 0:a06e2ad45792 355 /**
UBAL 0:a06e2ad45792 356 * Get the set speed. (Only mode 2 or 3).
UBAL 0:a06e2ad45792 357 *
UBAL 0:a06e2ad45792 358 * @return Faster the higher a number (mode 2: 0 -> 255 | mode 3: -128 to 127).
UBAL 0:a06e2ad45792 359 */
UBAL 0:a06e2ad45792 360 int MD25::speed_get() {
UBAL 0:a06e2ad45792 361 /* Check valid mode */
UBAL 0:a06e2ad45792 362 if (mode < 2 || mode > 3) return -1;
UBAL 0:a06e2ad45792 363
UBAL 0:a06e2ad45792 364 /* Variables */
UBAL 0:a06e2ad45792 365 char data;
UBAL 0:a06e2ad45792 366
UBAL 0:a06e2ad45792 367 /* Get speed */
UBAL 0:a06e2ad45792 368 if (read(REG_SPEED1, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 369
UBAL 0:a06e2ad45792 370 /* Return speed */
UBAL 0:a06e2ad45792 371 return data;
UBAL 0:a06e2ad45792 372 }
UBAL 0:a06e2ad45792 373
UBAL 0:a06e2ad45792 374 /**
UBAL 0:a06e2ad45792 375 * Set the turn. (Only mode 2 or 3).
UBAL 0:a06e2ad45792 376 *
UBAL 0:a06e2ad45792 377 * @param turn Faster the higher a number (mode 2: 0 -> 255 | mode 3: -128 to 127).
UBAL 0:a06e2ad45792 378 */
UBAL 0:a06e2ad45792 379 int MD25::turn_set(int turn) {
UBAL 0:a06e2ad45792 380 /* Check input is valid */
UBAL 0:a06e2ad45792 381 if (mode == 2) {
UBAL 0:a06e2ad45792 382 if (turn < 0 || turn > 255) return -1;
UBAL 0:a06e2ad45792 383 } else if (mode == 3) {
UBAL 0:a06e2ad45792 384 if (turn < -128 || turn > 127) return -1;
UBAL 0:a06e2ad45792 385 } else {
UBAL 0:a06e2ad45792 386 return 0;
UBAL 0:a06e2ad45792 387 }
UBAL 0:a06e2ad45792 388
UBAL 0:a06e2ad45792 389 /* Set data */
UBAL 0:a06e2ad45792 390 char data = (char)turn;
UBAL 0:a06e2ad45792 391
UBAL 0:a06e2ad45792 392 /* Set turn */
UBAL 0:a06e2ad45792 393 if (write(REG_SPEED2, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 394
UBAL 0:a06e2ad45792 395 /* Return success */
UBAL 0:a06e2ad45792 396 return 0;
UBAL 0:a06e2ad45792 397 }
UBAL 0:a06e2ad45792 398
UBAL 0:a06e2ad45792 399 /**
UBAL 0:a06e2ad45792 400 * Get the set turn. (Only mode 2 or 3).
UBAL 0:a06e2ad45792 401 *
UBAL 0:a06e2ad45792 402 * @return Faster the higher a number (mode 2: 0 -> 255 | mode 3: -128 to 127).
UBAL 0:a06e2ad45792 403 */
UBAL 0:a06e2ad45792 404 int MD25::turn_get() {
UBAL 0:a06e2ad45792 405 /* Check valid mode */
UBAL 0:a06e2ad45792 406 if (mode < 2 || mode > 3) return -1;
UBAL 0:a06e2ad45792 407
UBAL 0:a06e2ad45792 408 /* Variables */
UBAL 0:a06e2ad45792 409 char data;
UBAL 0:a06e2ad45792 410
UBAL 0:a06e2ad45792 411 /* Get turn */
UBAL 0:a06e2ad45792 412 if (read(REG_SPEED2, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 413
UBAL 0:a06e2ad45792 414 /* Return turn */
UBAL 0:a06e2ad45792 415 return data;
UBAL 0:a06e2ad45792 416 }
UBAL 0:a06e2ad45792 417
UBAL 0:a06e2ad45792 418 /**
UBAL 0:a06e2ad45792 419 * Set the desired acceleration rate.
UBAL 0:a06e2ad45792 420 *
UBAL 0:a06e2ad45792 421 * if new speed > current speed:
UBAL 0:a06e2ad45792 422 * steps = (new speed - current speed) / acceleration register
UBAL 0:a06e2ad45792 423 * if new speed < current speed:
UBAL 0:a06e2ad45792 424 * steps = (current speed - new speed) / acceleration register
UBAL 0:a06e2ad45792 425 * time = steps * 25ms
UBAL 0:a06e2ad45792 426 * Example:
UBAL 0:a06e2ad45792 427 * Time/step: 25ms | Current speed: 0 | New speed: 255 | Steps: 255 | Acceleration time: 6.375s.
UBAL 0:a06e2ad45792 428 * @param acceleration Faster the higher a number (default: 5).
UBAL 0:a06e2ad45792 429 */
UBAL 0:a06e2ad45792 430 int MD25::acceleration_set(int acceleration) {
UBAL 0:a06e2ad45792 431 /* Check input is valid */
UBAL 0:a06e2ad45792 432 if (acceleration < 0 || acceleration > 255) return -1;
UBAL 0:a06e2ad45792 433
UBAL 0:a06e2ad45792 434 /* Set data */
UBAL 0:a06e2ad45792 435 char data = (char)acceleration;
UBAL 0:a06e2ad45792 436
UBAL 0:a06e2ad45792 437 /* Set acceleration */
UBAL 0:a06e2ad45792 438 if (write(REG_ACCELERATION_RATE, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 439
UBAL 0:a06e2ad45792 440 /* Return success */
UBAL 0:a06e2ad45792 441 return 0;
UBAL 0:a06e2ad45792 442 }
UBAL 0:a06e2ad45792 443
UBAL 0:a06e2ad45792 444 /**
UBAL 0:a06e2ad45792 445 * Get the set desired acceleration rate.
UBAL 0:a06e2ad45792 446 *
UBAL 0:a06e2ad45792 447 * if new speed > current speed:
UBAL 0:a06e2ad45792 448 * steps = (new speed - current speed) / acceleration register
UBAL 0:a06e2ad45792 449 * if new speed < current speed:
UBAL 0:a06e2ad45792 450 * steps = (current speed - new speed) / acceleration register
UBAL 0:a06e2ad45792 451 * time = steps * 25ms
UBAL 0:a06e2ad45792 452 * Example:
UBAL 0:a06e2ad45792 453 * Time/step: 25ms | Current speed: 0 | New speed: 255 | Steps: 255 | Acceleration time: 6.375s.
UBAL 0:a06e2ad45792 454 * @return Faster the higher a number (default: 5).
UBAL 0:a06e2ad45792 455 */
UBAL 0:a06e2ad45792 456 int MD25::acceleration_get() {
UBAL 0:a06e2ad45792 457 /* Variables */
UBAL 0:a06e2ad45792 458 char data;
UBAL 0:a06e2ad45792 459
UBAL 0:a06e2ad45792 460 /* Get acceleration */
UBAL 0:a06e2ad45792 461 if (read(REG_ACCELERATION_RATE, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 462
UBAL 0:a06e2ad45792 463 /* Return acceleration */
UBAL 0:a06e2ad45792 464 return data;
UBAL 0:a06e2ad45792 465 }
UBAL 0:a06e2ad45792 466
UBAL 0:a06e2ad45792 467 /**
UBAL 0:a06e2ad45792 468 * Get encoder 1 position.
UBAL 0:a06e2ad45792 469 */
UBAL 0:a06e2ad45792 470 int MD25::encoder1_get() {
UBAL 0:a06e2ad45792 471 /* Variables */
UBAL 0:a06e2ad45792 472 char data[4];
UBAL 0:a06e2ad45792 473 int position;
UBAL 0:a06e2ad45792 474
UBAL 0:a06e2ad45792 475 /* Get encoder bytes */
UBAL 0:a06e2ad45792 476 if (read(REG_ENC1A, &data[0], 4) == -1) return -1;
UBAL 0:a06e2ad45792 477
UBAL 0:a06e2ad45792 478 /* Combine the position bytes */
UBAL 0:a06e2ad45792 479 position = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
UBAL 0:a06e2ad45792 480
UBAL 0:a06e2ad45792 481 /* Return position */
UBAL 0:a06e2ad45792 482 return position;
UBAL 0:a06e2ad45792 483 }
UBAL 0:a06e2ad45792 484
UBAL 0:a06e2ad45792 485 /**
UBAL 0:a06e2ad45792 486 * Get encoder 2 position.
UBAL 0:a06e2ad45792 487 */
UBAL 0:a06e2ad45792 488 int MD25::encoder2_get() {
UBAL 0:a06e2ad45792 489 /* Variables */
UBAL 0:a06e2ad45792 490 char data[4];
UBAL 0:a06e2ad45792 491 int position;
UBAL 0:a06e2ad45792 492
UBAL 0:a06e2ad45792 493 /* Get encoder bytes */
UBAL 0:a06e2ad45792 494 if (read(REG_ENC2A, &data[0], 4) == -1) return -1;
UBAL 0:a06e2ad45792 495
UBAL 0:a06e2ad45792 496 /* Combine the position bytes */
UBAL 0:a06e2ad45792 497 position = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
UBAL 0:a06e2ad45792 498
UBAL 0:a06e2ad45792 499 /* Return position */
UBAL 0:a06e2ad45792 500 return position;
UBAL 0:a06e2ad45792 501 }
UBAL 0:a06e2ad45792 502
UBAL 0:a06e2ad45792 503 /**
UBAL 0:a06e2ad45792 504 * Get battery voltage.
UBAL 0:a06e2ad45792 505 * It reads 10 times the voltage i.e. 121 for 12.1V.
UBAL 0:a06e2ad45792 506 *
UBAL 0:a06e2ad45792 507 * @return Normalized float.
UBAL 0:a06e2ad45792 508 */
UBAL 0:a06e2ad45792 509 float MD25::bat_voltage_get() {
UBAL 0:a06e2ad45792 510 /* Variables */
UBAL 0:a06e2ad45792 511 char data;
UBAL 0:a06e2ad45792 512
UBAL 0:a06e2ad45792 513 /* Get battery voltages */
UBAL 0:a06e2ad45792 514 if (read(REG_BATTERY_VOLTS, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 515
UBAL 0:a06e2ad45792 516 /* Return battery voltage */
UBAL 0:a06e2ad45792 517 return (float)(data / 10.0);
UBAL 0:a06e2ad45792 518 }
UBAL 0:a06e2ad45792 519
UBAL 0:a06e2ad45792 520 /**
UBAL 0:a06e2ad45792 521 * Get current of motor 1.
UBAL 0:a06e2ad45792 522 * It reads 10 times the current i.e. 25 for 2.5A.
UBAL 0:a06e2ad45792 523 *
UBAL 0:a06e2ad45792 524 * @return Normalized float.
UBAL 0:a06e2ad45792 525 */
UBAL 0:a06e2ad45792 526 float MD25::motor1_current_get() {
UBAL 0:a06e2ad45792 527 /* Variables */
UBAL 0:a06e2ad45792 528 char data;
UBAL 0:a06e2ad45792 529
UBAL 0:a06e2ad45792 530 /* Get motor current */
UBAL 0:a06e2ad45792 531 if (read(REG_MOTOR1_CURRENT, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 532
UBAL 0:a06e2ad45792 533 /* Return motor current */
UBAL 0:a06e2ad45792 534 return (float)(data / 10.0);
UBAL 0:a06e2ad45792 535 }
UBAL 0:a06e2ad45792 536
UBAL 0:a06e2ad45792 537 /**
UBAL 0:a06e2ad45792 538 * Get current of motor 2.
UBAL 0:a06e2ad45792 539 * It reads 10 times the current i.e. 25 for 2.5A.
UBAL 0:a06e2ad45792 540 *
UBAL 0:a06e2ad45792 541 * @return Normalized float.
UBAL 0:a06e2ad45792 542 */
UBAL 0:a06e2ad45792 543 float MD25::motor2_current_get() {
UBAL 0:a06e2ad45792 544 /* Variables */
UBAL 0:a06e2ad45792 545 char data;
UBAL 0:a06e2ad45792 546
UBAL 0:a06e2ad45792 547 /* Get motor current */
UBAL 0:a06e2ad45792 548 if (read(REG_MOTOR2_CURRENT, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 549
UBAL 0:a06e2ad45792 550 /* Return motor current */
UBAL 0:a06e2ad45792 551 return (float)(data / 10.0);
UBAL 0:a06e2ad45792 552 }
UBAL 0:a06e2ad45792 553
UBAL 0:a06e2ad45792 554 /**
UBAL 0:a06e2ad45792 555 * Get the software revision number in the PIC16F873 controller.
UBAL 0:a06e2ad45792 556 */
UBAL 0:a06e2ad45792 557 int MD25::software_rev_num_get() {
UBAL 0:a06e2ad45792 558 /* Variables */
UBAL 0:a06e2ad45792 559 char data;
UBAL 0:a06e2ad45792 560
UBAL 0:a06e2ad45792 561 /* Get software revision number */
UBAL 0:a06e2ad45792 562 if (read(REG_SOFTWARE_REVISION, &data, 1) == -1) return -1;
UBAL 0:a06e2ad45792 563
UBAL 0:a06e2ad45792 564 /* Return software revision number */
UBAL 0:a06e2ad45792 565 return data;
UBAL 0:a06e2ad45792 566 }