Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
MotorDriver.cpp@34:01dec68de3ed, 2015-05-06 (annotated)
- Committer:
- pvaibhav
- Date:
- Wed May 06 07:50:02 2015 +0000
- Revision:
- 34:01dec68de3ed
- Parent:
- 32:d37447aec6b4
motor voltage under 0.5V supported, magneto calibration initial values changed for smartplane2, complementary filter added to sensor fusion (default off), dt now sent with fusion parameters.
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 | 34:01dec68de3ed | 18 | const bool stop = fabs(voltage) < 0.01; |
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 | } |