Diff: LOCOMOTION.cpp

Revision:

23:455f7da3dd7a

Parent:

19:2dd81b864e14

Child:

24:fb1f083ebd62

--- a/LOCOMOTION.cpp	Sun Apr 03 06:15:52 2016 +0000
+++ b/LOCOMOTION.cpp	Sun Apr 03 07:08:18 2016 +0000
@@ -1,22 +1,35 @@
 #include "LOCOMOTION.h"
 
-LOCOMOTION::LOCOMOTION (PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2):
-    _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2)
+LOCOMOTION::LOCOMOTION (PinName en, PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2):
+    _en(en), _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2)
+{
+    s=0;
+    stopMotors();
+    _m1dir=0;
+    _m2dir=0;
+    _en=1;
+}
+
+inline void LOCOMOTION::stopMotors(void)
 {
     _m1f=0;
     _m1b=0;
     _m2f=0;
     _m2b=0;
-    _m1dir=0;
-    _m2dir=0;
 }
 
-void LOCOMOTION::stopMotors(void)
+
+inline int LOCOMOTION::wrap(int num)
 {
-    _m1f=0;
-    _m1b=0;
-    _m2f=0;
-    _m2b=0;
+    return num%360;
+}
+
+void LOCOMOTION::eStop(void)
+{
+    //Stop Motors
+    stopMotors();
+    //Disable Motor Drivers
+    _en=0;
 }
 
 bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
@@ -127,10 +140,4 @@
             break;
     }
     return false;
-}
-
-inline int LOCOMOTION::wrap(int num)
-{
-    return num%360;
-}
-
+}
\ No newline at end of file