Diff: LOCOMOTION.cpp

Revision:

27:fb1298d35bd1

Parent:

26:0ea6a550a99d

Child:

28:65daccc10f31

--- a/LOCOMOTION.cpp	Wed Apr 06 04:04:00 2016 +0000
+++ b/LOCOMOTION.cpp	Wed Apr 13 03:14:43 2016 +0000
@@ -20,7 +20,14 @@
     _m2f=x;
     _m2b=x;
 }
- 
+
+inline void LOCOMOTION::setMotors12(float x,float y)
+{
+    _m1f=x;
+    _m1b=x;
+    _m2f=y;
+    _m2b=y;
+}
  
 inline int LOCOMOTION::wrap(int num)
 {
@@ -114,11 +121,11 @@
             if(wrap(current+diff)>180+error) {
                 _m1dir=1;
                 _m2dir=0;
-                setMotors(s);
+                setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
             } else if(wrap(current+diff)<180-error) {
                 _m1dir=0;
                 _m2dir=1;
-                setMotors(s);
+                setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
             } else {
                 setMotors(0);
                 return true;
@@ -223,4 +230,25 @@
 void LOCOMOTION::mStop(void)
 {
     setMotors(0);
+}
+void LOCOMOTION::moveF(int angle)
+{
+    _m1dir=1;
+    _m2dir=1;
+    setMotors12(compG(0.5,_m1dir,angle),compG(0.5,_m2dir,angle)); 
+}
+
+void LOCOMOTION::moveB(void)
+{
+    _m1dir=1;
+    _m2dir=1;
+    setMotors(0.15) ;
+}
+
+inline float LOCOMOTION::compG(float speed, bool dir, int angle)
+{
+    if(dir)
+        return (cos((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed;
+    else
+        return -1*(cos((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed;
 }
\ No newline at end of file