Diff: LOCOMOTION.cpp

Revision:

19:2dd81b864e14

Parent:

18:f9012e93edb8

Child:

23:455f7da3dd7a

--- a/LOCOMOTION.cpp	Tue Mar 29 02:12:08 2016 +0000
+++ b/LOCOMOTION.cpp	Sun Apr 03 05:48:37 2016 +0000
@@ -11,6 +11,14 @@
     _m2dir=0;
 }
 
+void LOCOMOTION::stopMotors(void)
+{
+    _m1f=0;
+    _m1b=0;
+    _m2f=0;
+    _m2b=0;
+}
+
 bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
 {
     //s = 0;
@@ -43,11 +51,7 @@
         _m2f=s;
         _m2b=s;
     } else {
-        s=0;
-        _m1f=s;
-        _m1b=s;
-        _m2f=s;
-        _m2b=s;
+        stopMotors();
         return true;
     }
     return false;
@@ -114,11 +118,7 @@
                 _m2f=s;
                 _m2b=s;
             } else {
-                s=0;
-                _m1f=s;
-                _m1b=s;
-                _m2f=s;
-                _m2b=s;
+                stopMotors();
                 return true;
             }
             break;
@@ -129,7 +129,8 @@
     return false;
 }
 
-int LOCOMOTION::wrap(int num)
+inline int LOCOMOTION::wrap(int num)
 {
     return num%360;
-}
\ No newline at end of file
+}
+