Antoine Laurens / Mbed 2 deprecated STRAIGHT_DRIVE_NO_SEP

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BMP280 BNO055_fusion PowerControl mbed

Fork of TEAM_G_FLOW_RIDA by Edwin Cho

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