Diff: wheelchair.cpp

Revision:

8:381a4ec3fef8

Parent:

7:5e38d43fbce3

Child:

10:e5463c11e0a0

--- a/wheelchair.cpp	Sun Jul 22 06:15:03 2018 +0000
+++ b/wheelchair.cpp	Sun Jul 22 18:46:24 2018 +0000
@@ -1,6 +1,6 @@
 #include "wheelchair.h"
 
-Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time)
+Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time )
 {
     x = new PwmOut(xPin);
     y = new PwmOut(yPin);
@@ -53,51 +53,74 @@
     x->write(def);
     y->write(def);
 }
+// counter clockwise is +
+// clockwise is -
 
 void Wheelchair::turn_right()
 {
+    bool overturn = false;
     out->printf("turning right\n");
 
     double start = imu->yaw();
-    double final = start + 90;
+    double final = start - 90;
+    
+    if(final < 0) {
+        final += 360;
+        overturn = true;
+    }
+
+    out->printf("start %f, final %f\n", start, final);
     
-    out->printf("final %f \n", final);
-    out->printf("start %f \n", start);
-    if(final > 360) {
-        final -= 360;
-        out->printf("final %f \n", final);
-        while(imu->yaw() - 360 <= final) {
-            Wheelchair::right();
-            if( out->readable()) {
-                out->printf("stopped\n");
-                Wheelchair::stop();
-                return;
-            }
+    double curr = 361;
+    while(curr >= final) {
+        Wheelchair::right();
+        if( out->readable()) {
+            out->printf("stopped\n");
+            Wheelchair::stop();
+            return;
         }
-    } 
-    
-    else {
-       out->printf("final %f \n", final);
-        while(imu->yaw() <= final) {
-            Wheelchair::right();
-            if( out->readable()) {
-                out->printf("stopped\n");
-                Wheelchair::stop();
-                return;
-            }
+        curr = imu->yaw();
+        //out->printf("curr %f \n", curr);
+        
+        if(overturn && curr >= 0 && curr <= start ) {
+            curr = 360;
         }
     }
+
     out->printf("done turning\n");
+
 }
 
 void Wheelchair::turn_left()
 {
+    bool overturn = false;
+    out->printf("turning left\n");
+
     double start = imu->yaw();
-    double final = start - 90;
-    if(final <0)
-        final += 360;
+    double final = start + 90;
+
+    if(final > 360) {
+        final -= 360;
+        overturn = true;
+    }
+
+out->printf("start %f, final %f\n", start, final);
 
-    while(imu->yaw() >= final) {
+    double curr = -1;
+    while(curr <= final) {
         Wheelchair::left();
+        if( out->readable()) {
+            out->printf("stopped\n");
+            Wheelchair::stop();
+            return;
+        }
+        curr = imu->yaw();
+        if(overturn && curr > (360 - 90) ) {
+            curr = 0;
+        }
+        out->printf("curr %f \n", curr);
     }
+
+    out->printf("done turning\n");
 }
+