Library of routines to drive a MD25 motor control board
Dependents: Nucleo_motors HTU21D_HELLOWORLD Major_dHome pixyMajordhome ... more
MD25.h
- Committer:
- jimherd
- Date:
- 2011-05-20
- Revision:
- 2:e575d390c730
- Parent:
- 1:8046f460a725
File content as of revision 2:e575d390c730:
// *********************************************************************** // MBED MD25 H-bridge Motor Controller. // // Based on Arduino code by Richie Reynolds // *********************************************************************** #ifndef MBED_MD25_h #define MBED_MD25_h #include "mbed.h" #define MD25_DEFAULT_ADDRESS 0xB0 // // modes // // Mode Description // 0 Skid-steer with unsigned speed values (STOP = 128) // 1 Skid-steer with signed speed values (STOP = 0) // 2 Forward/turn steer with unsigned values (speed1 to control both motors, speed2 for turn) // 3 Forward/turn steer with signed values (speed1 to control both motors, speed2 for turn) // #define MODE_0 0 #define MODE_1 1 #define MODE_2 2 #define MODE_3 3 // // register definitions // #define MD25_SPEED1_REG 0 #define MD25_SPEED2_REG 1 #define MD25_ENCODER1_REG 2 #define MD25_ENCODER2_REG 6 #define MD25_VOLTAGE_REG 10 #define MD25_CURRENT1_REG 11 #define MD25_CURRENT2_REG 12 #define MD25_SOFTWAREVER_REG 13 #define MD25_ACCELRATE_REG 14 #define MD25_MODE_REG 15 #define MD25_CMD_REG 16 // command register // // Command register command set // #define MD25_RESET_ENCODERS 0x20 #define MD25_DIABLE SPEED_REGULATION 0x30 #define MD25_ENABLE_SPEED_REGULATION 0x31 #define MD25_DISABLE_TIMEOUT 0x32 #define MD25_ENABLE_TIMEOUT 0x33 /** MD25 class * * Allow access to an MD25 Dual 12V 2.8A H-Bridge DC Motor Driver * * @code * MD25 motor_control(p9,p10); // assumes default address of 0xB0 * or * MD25 motor_control(p9, p10, 0xB0); * @endcode */ class MD25 { public: /** Constructor for the MD25 connected to specified I2C pins at a specified address * * @param sda I2C data pin * @param scl I2C clock pin * @param i2cAddress I2C address */ MD25(PinName sda, PinName scl, int MD25_i2cAddress); /** Constructor for the MD25 connected to specified I2C pins at default address * * @param sda I2C data pin * @param scl I2C clock pin */ MD25(PinName sda, PinName scl); /** Read encoder for channel 1 * * @return 32-bit signed integer value of current encoder value for channel 1 */ int32_t getEncoder1(void); /** Read encoder for channel 2 * * @return 32-bit signed integer value of current encoder value for channel 2 */ int32_t getEncoder2(void); /** set speed registers for both channels * * Effect of value is dependent on system mode * * @param speed_1 speed register for channel 1 (0->255) * @param speed_2 speed register for channel 2 (0->255) */ void setSpeedRegisters(uint8_t speed_1, uint8_t speed_2); /** set speed register for channel 1 * * Effect of value is dependent on system mode * * @param speed_1 speed register for channel 1 (0->255) */ void setSpeed1Reg(uint8_t speed); /** set speed register for channel 2 * * Effect of value is dependent on system mode * * @param speed_2 speed register for channel 2 (0->255) */ void setSpeed2Reg(uint8_t speed); /** switch motor 1 off */ void stopMotor1(void); /** switch motor 2 off */ void stopMotor2(void); /** switch both motors off */ void stopMotors(void); /** read current software version * * @return version number */ uint32_t getSoftwareVersion(void); /** read battery voltage * * @return voltage value in float format */ float getBatteryVolts(void); /** read acceleration rate * * @return acceleration rate */ uint8_t getAccelerationRate(void); /** read current from motor channel 1 * * @return current value */ uint8_t getMotor1Current(void); /** read current from motor channel 2 * * @return current value in apprx. units of 0.1amp */ uint8_t getMotor2Current(void); /** read current speed register for motor channel 1 * * @return speed value (0->255); meaning dependent on mode */ uint8_t getMotor1Speed(void); /** read current speed register for motor channel 2 * * @return speed value (0->255); meaning dependent on mode */ uint8_t getMotor2Speed(void); /** read current mode * * @return mode value (0, 1, 2, or 3); default is mode 0 */ uint8_t getMode(void); /** set system mode * * @param mode value (0, 1, 2, or 3) */ void setMode(uint8_t mode); /** set acceleration rate * * @param rate acceleration rate */ void setAccelerationRate(uint8_t rate); /** send command to command register * * @param command command code * * MD25_RESET_ENCODERS 0x20 * MD25_DIABLE SPEED_REGULATION 0x30 * MD25_ENABLE_SPEED_REGULATION 0x31 * MD25_DISABLE_TIMEOUT 0x32 * MD25_ENABLE_TIMEOUT 0x33 */ void setCommand(uint8_t command); private: I2C _i2c; uint8_t current_mode; uint8_t MD25_i2cAddress; uint8_t readRegisterbyte(uint8_t reg); void writeRegisterbyte(uint8_t reg, uint8_t value); }; #endif // MBED_md25_h