Diff: ZumoShield.cpp

Revision:

0:c69b20870374

diff -r 000000000000 -r c69b20870374 ZumoShield.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield.cpp	Mon Oct 12 11:32:33 2015 +0000
@@ -0,0 +1,122 @@
+#include "ZumoShield.h"
+#include "mbed.h"
+
+ZumoShield::ZumoShield(PinName m1_pwm_pin, PinName m1_dir_pin,
+                       PinName m2_pwm_pin, PinName m2_dir_pin) :
+//                     PinName a0_pin, PinName a1_pin, PinName a2_pin, PinName a3_pin, PinName a4_pin, PinName a5_pin) :
+    m1pwm(m1_pwm_pin),
+    m1dir(m1_dir_pin),
+    m2pwm(m2_pwm_pin),
+    m2dir(m2_dir_pin)
+    /*
+    a0sens(a0_pin),
+    a1sens(a1_pin),
+    a2sens(a2_pin),
+    a3sens(a3_pin),
+    a4sens(a4_pin),
+    a5sens(a5_pin)
+    */
+{
+}
+
+void ZumoShield::left_motor(float speed)
+{
+    if (speed >= 0) {
+        m2pwm = speed;
+        m2dir = 0;
+    } else {
+        m2pwm = -speed;
+        m2dir = 1;
+    }
+}
+
+void ZumoShield::right_motor(float speed)
+{
+    if (speed >= 0) {
+        m1pwm = speed;
+        m1dir = 1;
+    } else {
+        m1pwm = -speed;
+        m1dir = 0;
+    }
+}
+
+void ZumoShield::turn_left(float speed)
+{
+    left_motor(0);
+    right_motor(speed);
+}
+
+void ZumoShield::turn_right(float speed)
+{
+    left_motor(speed);
+    right_motor(0);
+}
+
+void ZumoShield::left(float speed)
+{
+    left_motor(-speed);
+    right_motor(speed);
+}
+
+void ZumoShield::right(float speed)
+{
+    left_motor(speed);
+    right_motor(-speed);
+}
+
+void ZumoShield::forward(float speed)
+{
+    if (speed == 0) {
+        left_motor(0);
+        right_motor(0);
+    } else {
+        left_motor(speed);
+        right_motor(speed);
+    }
+}
+
+void ZumoShield::backward(float speed)
+{
+    if (speed == 0) {
+        left_motor(0);
+        right_motor(0);
+    } else {
+        left_motor(-speed);
+        right_motor(-speed);
+    }
+}
+
+void ZumoShield::stop(int motor)
+{
+    if (motor == 1) {
+        stopLeft();
+    } else if (motor == 2) {
+        stopRight();
+    }
+}
+
+void ZumoShield::stopLeft()
+{
+    m2pwm = 0;
+    m2dir = 0;
+}
+
+void ZumoShield::stopRight()
+{
+    m1pwm = 0;
+    m1dir = 0;
+}
+
+void ZumoShield::stopAll()
+{
+    stopLeft();
+    stopRight();
+}
+
+/* TODO
+float ZumoShield::position()
+{
+    return output;
+}
+*/