Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

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?

UserRevisionLine numberNew 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 }