Diff: DualMotorController.cpp

Revision:

1:b85eb4e982a9

Parent:

0:9ac0748f78d4

Child:

2:c0e11aaa32e7

--- a/DualMotorController.cpp	Sun Oct 22 08:08:35 2017 +0000
+++ b/DualMotorController.cpp	Sun Oct 22 10:38:11 2017 +0200
@@ -0,0 +1,41 @@
+#include "DualMotorController.hpp"
+
+DualMotorController::DualMotorController(PinName pwmL, PinName dirL0, PinName dirL1, PinName pwmR, PinName dirR0, PinName dirR1, float frequency) :
+    _pwmL(pwmA),
+    _dirL0(dirA0,0),
+    _dirL1(dirA1,0),
+    _pwmR(pwmR),
+    _dirR0(dirR0,0),
+    _dirR1(dirR1,0)
+{
+    _pwmL.period(1.0f/frequency);
+    _pwmR.period(1.0f/frequency);
+
+    _speedL = 0.0f;
+    _speedR = 0.0f;
+    _inverted = false;
+}
+
+void DualMotorController::SetLeft(float speed, boolean forward) {
+    if( forward ) {
+        _dirL0.write(1);
+        _dirL1.write(0);
+    } else {
+        _dirL0.write(0);
+        _dirL1.write(1);
+    }
+
+    _pwmL.write(speed);
+}
+
+void DualMotorController::SetRight(float speed, boolean forward) {
+    if( forward ) {
+        _dirR0.write(1);
+        _dirR1.write(0);
+    } else {
+        _dirR0.write(0);
+        _dirR1.write(1);
+    }
+
+    _pwmR.write(speed);
+}
\ No newline at end of file