Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

MotorDriver.cpp@31:d65576185bdf, 2015-04-28 (annotated)

Committer:
pvaibhav
Date:
Tue Apr 28 09:08:03 2015 +0000
Revision:
31:d65576185bdf
Parent:
18:f51b1a94a6e2
Child:
32:d37447aec6b4
- Gyro doesn't disable interrupt anymore (fix for SML2 v0.95); - Accelerometer frequency reduced to 125 Hz

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 12:1632d7391453 6 MotorDriver::MotorDriver(I2C &i2c, const uint8_t address) : I2CPeripheral(i2c, address)
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 }