Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
MotorDriver.cpp@32:d37447aec6b4, 2015-05-04 (annotated)
- Committer:
- pvaibhav
- Date:
- Mon May 04 15:16:57 2015 +0000
- Revision:
- 32:d37447aec6b4
- Parent:
- 31:d65576185bdf
- Child:
- 34:01dec68de3ed
Interrupt lines added to all sensors and motor driver, power aware I2C subclass added (currently doesn't do anything)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pvaibhav | 0:943820483318 | 1 | #include "MotorDriver.h" |
pvaibhav | 0:943820483318 | 2 | #include <cmath> |
pvaibhav | 4:e759b8c756da | 3 | #define DEBUG "Motor" |
pvaibhav | 4:e759b8c756da | 4 | #include "Logger.h" |
pvaibhav | 0:943820483318 | 5 | |
pvaibhav | 32:d37447aec6b4 | 6 | MotorDriver::MotorDriver(I2C &i2c, const uint8_t address, PinName interruptPin) : I2CPeripheral(i2c, address), faultLine(interruptPin) |
pvaibhav | 12:1632d7391453 | 7 | { |
pvaibhav | 4:e759b8c756da | 8 | INFO("Motor driver at addr=%#x ready", address); |
pvaibhav | 4:e759b8c756da | 9 | } |
pvaibhav | 0:943820483318 | 10 | |
pvaibhav | 18:f51b1a94a6e2 | 11 | void MotorDriver::setVoltage(float voltage) |
pvaibhav | 12:1632d7391453 | 12 | { |
pvaibhav | 18:f51b1a94a6e2 | 13 | if (voltage < -5.06) |
pvaibhav | 18:f51b1a94a6e2 | 14 | voltage = -5.06; |
pvaibhav | 18:f51b1a94a6e2 | 15 | if (voltage > 5.06) |
pvaibhav | 18:f51b1a94a6e2 | 16 | voltage = 5.06; |
pvaibhav | 12:1632d7391453 | 17 | |
pvaibhav | 3:ee90a9ada112 | 18 | const bool stop = fabs(voltage) < 0.48; |
pvaibhav | 3:ee90a9ada112 | 19 | const float Vr = 1.285; // internal reference voltage |
pvaibhav | 12:1632d7391453 | 20 | |
pvaibhav | 3:ee90a9ada112 | 21 | if (stop) { |
pvaibhav | 31:d65576185bdf | 22 | write_reg(0x00, 0x00); // last 2 bits = 11 = "brake", last 2 bits = 00 = "coast" |
pvaibhav | 3:ee90a9ada112 | 23 | } else { |
pvaibhav | 3:ee90a9ada112 | 24 | const uint8_t DAC_val = ceil( (16.0 * fabs(voltage) / Vr) - 1.0 ); // derived from table on DRV8830 datasheet page 10 |
pvaibhav | 3:ee90a9ada112 | 25 | const uint8_t direction = (voltage > 0.0) ? 0x01 : 0x02; // forward or reverse is set through last 2 bits |
pvaibhav | 3:ee90a9ada112 | 26 | write_reg(0x00, (DAC_val << 2) | direction); |
pvaibhav | 3:ee90a9ada112 | 27 | } |
pvaibhav | 0:943820483318 | 28 | } |
pvaibhav | 0:943820483318 | 29 | |
pvaibhav | 12:1632d7391453 | 30 | MotorDriver& MotorDriver::operator=(const float voltage) |
pvaibhav | 12:1632d7391453 | 31 | { |
pvaibhav | 0:943820483318 | 32 | setVoltage(voltage); |
pvaibhav | 0:943820483318 | 33 | return *this; |
pvaibhav | 0:943820483318 | 34 | } |