MD25.cpp

00001 /********************************************************************
00002  * @filename    MD25.cpp
00003  * @author      Thomas B. Joergensen (thomas.bal.jorgensen@gmail.com)
00004  * @date        06 FEB 2012
00005  * @version     1.0
00006  * @target      mbed (NXP LPC1768 - ARM Cortex M3 - 32-bit)
00007  *
00008  * @desciption  A library for interacting with the MD25 DC motor
00009  *              controller. Find more at:
00010  *              http://www.robot-electronics.co.uk/htm/md25tech.htm
00011  *              http://www.robot-electronics.co.uk/htm/md25i2c.htm
00012  *******************************************************************/
00013 
00014 /* Includes */
00015 #include "MD25.h"
00016 
00017 /*** CONSTRUCTOR AND DESTRUCTOR ***/
00018 
00019 MD25::MD25(I2C *i2c_interface, char i2c_address) {
00020     this->i2c_interface = i2c_interface;
00021     this->i2c_address = i2c_address;
00022     
00023     /* Default values */
00024     mode = 0;
00025 }
00026 
00027 MD25::~MD25(void) {};
00028 
00029 /*** GENERIC METHODS ***/
00030 
00031 /**
00032  * Write a number of bytes.
00033  *
00034  * @param reg_addr      Address of register to access.
00035  * @param data          Pointer to where data is stored.
00036  * @param bytes         Number of bytes to write.
00037  */
00038 int MD25::write(char reg_addr, char *data, int bytes) {
00039     /* Variables */
00040     char data_out[bytes + 1];
00041     
00042     /* Add register address to char array */
00043     data_out[0] = reg_addr;
00044     
00045     /* Populate data array */
00046     for (int i = 1; i < bytes + 1; i++) {
00047         data_out[i] = data[i - 1];
00048     }
00049     
00050     /* Write address and data */
00051     if (i2c_interface->write(i2c_address + I2C_WRITE_BIT, &data_out[0], bytes + 1, true)) {
00052         return -1;
00053     }
00054 
00055     /* Return success */
00056     return 0;
00057 }
00058 
00059 /**
00060  * Read a number of bytes.
00061  *
00062  * @param reg_addr      Address of register to access.
00063  * @param data          Pointer to where data it to be stored.
00064  * @param bytes         Number of bytes to read.
00065  */
00066 int MD25::read(char reg_addr, char *data, int bytes) {
00067     /* Setup register to read */
00068     if (!i2c_interface->write(i2c_address + I2C_WRITE_BIT, &reg_addr, 1, true)) {
00069         /* Read register */
00070         if (i2c_interface->read(i2c_address + I2C_READ_BIT, data, bytes, false)) {
00071             return -1;
00072         }
00073     } else {
00074         return -1;
00075     }
00076 
00077     /* Return success */
00078     return 0;
00079 }
00080 
00081 /**
00082  * Discover I2C devices.
00083  *
00084  * @return  char containing indicator of i2c device presence in form of bits
00085  *          i.e. if LSB of returned char is 1 then a device is present at I2C_START_ADDR.
00086  *          If bit 2 of char is 1 then a device is present at I2C_START_ADDR + 2.
00087  */
00088 char MD25::doDiscover() {
00089     /* Initialize device char */
00090     char device = 0;
00091     
00092     /* Check for devices */
00093     for (int i = 0; i < 8; i++) {
00094         if (!i2c_interface->write(I2C_START_ADDR + (2 * i), NULL, 0)) { // 0 returned is ok
00095             device |= (1<<i);
00096         }
00097     }
00098         
00099     /* Return device char */
00100     return device;
00101 }
00102 
00103 /*** CONTROL METHODS ***/
00104 
00105 /**
00106  * Set mode of operation.
00107  *
00108  * @param mode      Set mode of operation (0 - 3) (default: 0).
00109  */
00110 int MD25::mode_set(int mode) {
00111     /* Check input is valid */
00112     if (mode < 0 || mode > 3) return -1;
00113     
00114     /* Variables */
00115     char data = (char)mode;
00116     
00117     /* Set mode */
00118     if (write(REG_MODE, &data, 1) == -1) return -1;
00119     
00120     /* Check set */
00121     data = mode_get();    
00122     if (data != mode) return -1;
00123     
00124     /* Set mode variable */
00125     this->mode = mode;
00126     
00127     /* Return success */
00128     return 0;
00129 }
00130 
00131 /**
00132  * Get mode of operation.
00133  */
00134 int MD25::mode_get() {
00135     /* Variables */
00136     char data;
00137     
00138     /* Get mode */
00139     if (read(REG_MODE, &data, 1) == -1) return -1;
00140     
00141     /* Return mode */
00142     return data;
00143 }
00144 
00145 /**
00146  * Reset the encoder registers.
00147  */
00148 int MD25::encoder_reset() {
00149     /* Variables */
00150     char data = CMD_ENCODER_RESET;
00151 
00152     /* Reset encoders */
00153     if (write(REG_COMMAND, &data, 1) == -1) return -1;
00154     
00155     /* Return success */
00156     return 0;
00157 }
00158 
00159 /**
00160  * Enable/disable automatic speed regulation (default: enabled).
00161  *
00162  * @param enabled   enable = 1 | disable = 0.
00163  */
00164 int MD25::auto_speed_set(bool enabled) {
00165     /* Variables */
00166     char data;
00167     
00168     /* Set data */
00169     if (enabled) {
00170         data = CMD_AUTO_SPEED_ENABLE;
00171     } else {
00172         data = CMD_AUTO_SPEED_DISABLE;
00173     }
00174 
00175     /* Reset encoders */
00176     if (write(REG_COMMAND, &data, 1) == -1) return -1;
00177 
00178     /* Return success */
00179     return 0;
00180 }
00181 
00182 /**
00183  * Enable/disable 2 sec timeout of motors when no I2C comms (default: enabled).
00184  *
00185  * @param enabled   enable = 1 | disable = 0.
00186  */
00187 int MD25::timeout_set(bool enabled) {
00188     /* Variables */
00189     char data;
00190     
00191     /* Set data */
00192     if (enabled) {
00193         data = CMD_TIMEOUT_ENABLE;
00194     } else {
00195         data = CMD_TIMEOUT_DISABLE;
00196     }
00197 
00198     /* Reset encoders */
00199     if (write(REG_COMMAND, &data, 1) == -1) return -1;
00200 
00201     /* Return success */
00202     return 0;
00203 }
00204 
00205 /**
00206  * Set a new I2C device address (Default: 0xB0).
00207  *
00208  * @param address   Address of device (B0 -> BE; inc: 2).
00209  */
00210 int MD25::i2c_addr_set(char address) {
00211     /* Check input is valid */
00212     if (address < I2C_START_ADDR || address > I2C_START_ADDR + 0x0F) return -1;
00213     if (address % 2 == 1) return -1;
00214     
00215     /* Set data */
00216     char data[4];
00217     data[0] = CMD_CHANGE_I2C_ADDR_1;
00218     data[1] = CMD_CHANGE_I2C_ADDR_2;
00219     data[2] = CMD_CHANGE_I2C_ADDR_3;
00220     data[3] = address;
00221     
00222     /* Set new address */
00223     if (write(REG_COMMAND, &data[0], 4) == -1) return -1;
00224     
00225     /* Set address variable */
00226     i2c_address = address;
00227     
00228     /* Return success */
00229     return 0;
00230 }
00231 
00232 /*** DATA METHODS ***/
00233 
00234 /**
00235  * Set the speed of motor 1. (Only mode 0 or 1).
00236  *
00237  * @param speed     Faster the higher a number (mode 0: 0 -> 255 | mode 1: -128 to 127).
00238  */
00239 int MD25::speed1_set(int speed) {
00240     /* Check input is valid */
00241     if (mode == 0) {
00242         if (speed < 0 || speed >255) return -1;
00243     } else if (mode == 1) {
00244         if (speed < -128 || speed > 127) return -1;
00245     } else {
00246         return 0;
00247     }
00248     
00249     /* Variable */
00250     char data = (char)speed;
00251     
00252     /* Set data */
00253     if (mode == 1) data = (signed char)data;
00254     
00255     /* Set speed */
00256     if (write(REG_SPEED1, &data, 1) == -1) return -1;
00257     
00258     /* Return success */
00259     return 0;
00260 }
00261 
00262 /**
00263  * Get the set speed of motor 1. (Only mode 0 or 1).
00264  *
00265  * @return  Faster the higher a number (mode 0: 0 -> 255 | mode 1: -128 to 127).
00266  */
00267 int MD25::speed1_get() {
00268     /* Check valid mode */
00269     if (mode < 0 || mode > 1) return -1;
00270     
00271     /* Variables */
00272     char data;
00273     
00274     /* Get speed */
00275     if (read(REG_SPEED1, &data, 1) == -1) return -1;
00276     
00277     /* Return speed */
00278     if (mode == 1) return (signed char)data;
00279     return data;
00280 }
00281 
00282 /**
00283  * Set the speed of motor 2. (Only mode 0 or 1).
00284  *
00285  * @param speed     Faster the higher a number (mode 0: 0 -> 255 | mode 1: -128 to 127).
00286  */
00287 int MD25::speed2_set(int speed) {
00288     /* Check input is valid */
00289     if (mode == 0) {
00290         if (speed < 0 || speed >255) return -1;
00291     } else if (mode == 1) {
00292         if (speed < -128 || speed > 127) return -1;
00293     } else {
00294         return 0;
00295     }
00296     
00297     /* Variable */
00298     char data = (char)speed;
00299     
00300     /* Set data */
00301     if (mode == 1) data = (signed char)data;
00302     
00303     /* Set speed */
00304     if (write(REG_SPEED2, &data, 1) == -1) return -1;
00305     
00306     /* Return success */
00307     return 0;
00308 }
00309 
00310 /**
00311  * Get the set speed of motor 2. (Only mode 0 or 1).
00312  *
00313  * @return  Faster the higher a number (mode 0: 0 -> 255 | mode 1: -128 to 127).
00314  */
00315 int MD25::speed2_get() {
00316     /* Check valid mode */
00317     if (mode < 0 || mode > 1) return -1;
00318     
00319     /* Variables */
00320     char data;
00321     
00322     /* Get speed */
00323     if (read(REG_SPEED2, &data, 1) == -1) return -1;
00324     
00325     /* Return speed */
00326     if (mode == 1) return (signed char)data;
00327     return data;
00328 }
00329 
00330 /**
00331  * Set the speed. (Only mode 2 or 3).
00332  *
00333  * @param speed     Faster the higher a number (mode 2: 0 -> 255 | mode 3: -128 to 127).
00334  */
00335 int MD25::speed_set(int speed) {
00336     /* Check input is valid */
00337     if (mode == 2) {
00338         if (speed < 0 || speed > 255) return -1;
00339     } else if (mode == 3) {
00340         if (speed < -128 || speed > 127) return -1;
00341     } else {
00342         return 0;
00343     }
00344     
00345     /* Set data */
00346     char data = (char)speed;
00347     
00348     /* Set speed */
00349     if (write(REG_SPEED1, &data, 1) == -1) return -1;
00350     
00351     /* Return success */
00352     return 0;
00353 }
00354 
00355 /**
00356  * Get the set speed. (Only mode 2 or 3).
00357  *
00358  * @return  Faster the higher a number (mode 2: 0 -> 255 | mode 3: -128 to 127).
00359  */
00360 int MD25::speed_get() {
00361     /* Check valid mode */
00362     if (mode < 2 || mode > 3) return -1;
00363     
00364     /* Variables */
00365     char data;
00366     
00367     /* Get speed */
00368     if (read(REG_SPEED1, &data, 1) == -1) return -1;
00369     
00370     /* Return speed */
00371     return data;
00372 }
00373 
00374 /**
00375  * Set the turn. (Only mode 2 or 3).
00376  *
00377  * @param turn      Faster the higher a number (mode 2: 0 -> 255 | mode 3: -128 to 127).
00378  */
00379 int MD25::turn_set(int turn) {
00380     /* Check input is valid */
00381     if (mode == 2) {
00382         if (turn < 0 || turn > 255) return -1;
00383     } else if (mode == 3) {
00384         if (turn < -128 || turn > 127) return -1;
00385     } else {
00386         return 0;
00387     }
00388     
00389     /* Set data */
00390     char data = (char)turn;
00391     
00392     /* Set turn */
00393     if (write(REG_SPEED2, &data, 1) == -1) return -1;
00394     
00395     /* Return success */
00396     return 0;
00397 }
00398 
00399 /**
00400  * Get the set turn. (Only mode 2 or 3).
00401  *
00402  * @return  Faster the higher a number (mode 2: 0 -> 255 | mode 3: -128 to 127).
00403  */
00404 int MD25::turn_get() {
00405     /* Check valid mode */
00406     if (mode < 2 || mode > 3) return -1;
00407     
00408     /* Variables */
00409     char data;
00410     
00411     /* Get turn */
00412     if (read(REG_SPEED2, &data, 1) == -1) return -1;
00413     
00414     /* Return turn */
00415     return data;
00416 }
00417 
00418 /**
00419  * Set the desired acceleration rate.
00420  *
00421  * if new speed > current speed:
00422  *      steps = (new speed - current speed) / acceleration register 
00423  * if new speed < current speed:
00424  *      steps = (current speed - new speed) / acceleration register
00425  * time = steps * 25ms 
00426  * Example: 
00427  *      Time/step: 25ms | Current speed: 0 | New speed: 255 | Steps: 255 | Acceleration time: 6.375s.
00428  * @param acceleration  Faster the higher a number (default: 5).
00429  */
00430 int MD25::acceleration_set(int acceleration) {
00431     /* Check input is valid */    
00432     if (acceleration < 0 || acceleration > 255) return -1;
00433     
00434     /* Set data */
00435     char data = (char)acceleration;
00436     
00437     /* Set acceleration */
00438     if (write(REG_ACCELERATION_RATE, &data, 1) == -1) return -1;
00439     
00440     /* Return success */
00441     return 0;
00442 }
00443 
00444 /**
00445  * Get the set desired acceleration rate.
00446  *
00447  * if new speed > current speed:
00448  *      steps = (new speed - current speed) / acceleration register 
00449  * if new speed < current speed:
00450  *      steps = (current speed - new speed) / acceleration register
00451  * time = steps * 25ms 
00452  * Example: 
00453  *      Time/step: 25ms | Current speed: 0 | New speed: 255 | Steps: 255 | Acceleration time: 6.375s.
00454  * @return  Faster the higher a number (default: 5).
00455  */
00456 int MD25::acceleration_get() {
00457     /* Variables */
00458     char data;
00459     
00460     /* Get acceleration */
00461     if (read(REG_ACCELERATION_RATE, &data, 1) == -1) return -1;
00462     
00463     /* Return acceleration */
00464     return data;
00465 }
00466 
00467 /**
00468  * Get encoder 1 position.
00469  */
00470 int MD25::encoder1_get() {
00471     /* Variables */
00472     char data[4];
00473     int position;
00474     
00475     /* Get encoder bytes */
00476     if (read(REG_ENC1A, &data[0], 4) == -1) return -1;
00477     
00478     /* Combine the position bytes */
00479     position = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
00480     
00481     /* Return position */
00482     return position;
00483 }
00484 
00485 /**
00486  * Get encoder 2 position.
00487  */
00488 int MD25::encoder2_get() {
00489     /* Variables */
00490     char data[4];
00491     int position;
00492     
00493     /* Get encoder bytes */
00494     if (read(REG_ENC2A, &data[0], 4) == -1) return -1;
00495     
00496     /* Combine the position bytes */
00497     position = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
00498     
00499     /* Return position */
00500     return position;
00501 }
00502 
00503 /**
00504  * Get battery voltage.
00505  * It reads 10 times the voltage i.e. 121 for 12.1V.
00506  *
00507  * @return  Normalized float.
00508  */
00509 float MD25::bat_voltage_get() {
00510     /* Variables */
00511     char data;
00512     
00513     /* Get battery voltages */
00514     if (read(REG_BATTERY_VOLTS, &data, 1) == -1) return -1;
00515     
00516     /* Return battery voltage */
00517     return (float)(data / 10.0);
00518 }
00519 
00520 /**
00521  * Get current of motor 1.
00522  * It reads 10 times the current i.e. 25 for 2.5A.
00523  *
00524  * @return  Normalized float.
00525  */
00526 float MD25::motor1_current_get() {
00527     /* Variables */
00528     char data;
00529     
00530     /* Get motor current */
00531     if (read(REG_MOTOR1_CURRENT, &data, 1) == -1) return -1;
00532     
00533     /* Return motor current */
00534     return (float)(data / 10.0);
00535 }
00536 
00537 /**
00538  * Get current of motor 2.
00539  * It reads 10 times the current i.e. 25 for 2.5A.
00540  *
00541  * @return  Normalized float.
00542  */
00543 float MD25::motor2_current_get() {
00544     /* Variables */
00545     char data;
00546     
00547     /* Get motor current */
00548     if (read(REG_MOTOR2_CURRENT, &data, 1) == -1) return -1;
00549     
00550     /* Return motor current */
00551     return (float)(data / 10.0);
00552 }
00553 
00554 /**
00555  * Get the software revision number in the PIC16F873 controller.
00556  */
00557 int MD25::software_rev_num_get() {
00558     /* Variables */
00559     char data;
00560     
00561     /* Get software revision number */
00562     if (read(REG_SOFTWARE_REVISION, &data, 1) == -1) return -1;
00563     
00564     /* Return software revision number */
00565     return data;
00566 }