Diff: MotorDriver.cpp

Revision:

3:ee90a9ada112

Parent:

0:943820483318

Child:

4:e759b8c756da

--- a/MotorDriver.cpp	Wed Jan 14 17:57:11 2015 +0000
+++ b/MotorDriver.cpp	Wed Jan 21 10:34:45 2015 +0000
@@ -5,13 +5,18 @@
 MotorDriver::MotorDriver(I2C &i2c, const uint8_t address) : I2CPeripheral(i2c, address) {}
 
 void MotorDriver::setVoltage(const float voltage) {
-    assert(abs(voltage) >= 0.48);
     assert(abs(voltage) <= 5.06);
     
-    const uint8_t DAC_val = floor(abs(voltage) / 0.0804) + 1; // 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
+    const bool stop = fabs(voltage) < 0.48;
+    const float Vr = 1.285; // internal reference voltage
     
-    write_reg(0x00, (DAC_val << 2) | direction);
+    if (stop) {
+        write_reg(0x00, 0x03); // last 2 bits = 11 = "brake"
+    } 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) {