Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
MotorDriver.cpp
- Committer:
- pvaibhav
- Date:
- 2015-05-04
- Revision:
- 32:d37447aec6b4
- Parent:
- 31:d65576185bdf
- Child:
- 34:01dec68de3ed
File content as of revision 32:d37447aec6b4:
#include "MotorDriver.h" #include <cmath> #define DEBUG "Motor" #include "Logger.h" MotorDriver::MotorDriver(I2C &i2c, const uint8_t address, PinName interruptPin) : I2CPeripheral(i2c, address), faultLine(interruptPin) { INFO("Motor driver at addr=%#x ready", address); } void MotorDriver::setVoltage(float voltage) { if (voltage < -5.06) voltage = -5.06; if (voltage > 5.06) voltage = 5.06; const bool stop = fabs(voltage) < 0.48; const float Vr = 1.285; // internal reference voltage if (stop) { write_reg(0x00, 0x00); // last 2 bits = 11 = "brake", last 2 bits = 00 = "coast" } else { const uint8_t DAC_val = ceil( (16.0 * fabs(voltage) / Vr) - 1.0 ); // derived from table on DRV8830 datasheet page 10 const uint8_t direction = (voltage > 0.0) ? 0x01 : 0x02; // forward or reverse is set through last 2 bits write_reg(0x00, (DAC_val << 2) | direction); } } MotorDriver& MotorDriver::operator=(const float voltage) { setVoltage(voltage); return *this; }