Diff: wheelchair.cpp

Revision:

7:5e38d43fbce3

Parent:

6:0cd57bdd8fbc

Child:

8:381a4ec3fef8

--- a/wheelchair.cpp	Fri Jul 20 17:54:43 2018 +0000
+++ b/wheelchair.cpp	Sun Jul 22 06:15:03 2018 +0000
@@ -1,12 +1,15 @@
 #include "wheelchair.h"
 
-Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc)
+Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time)
 {
     x = new PwmOut(xPin);
     y = new PwmOut(yPin);
     //imu = new chair_BNO055();
-    imu = new chair_MPU9250(pc);
-    
+    imu = new chair_MPU9250(pc, time);
+    imu->setup();
+    out = pc;
+    out->printf("wheelchair setup done \n");
+
 }
 
 /*
@@ -51,23 +54,40 @@
     y->write(def);
 }
 
-void Wheelchair::turn_right(Serial out)
+void Wheelchair::turn_right()
 {
-    bool stop = false;
+    out->printf("turning right\n");
+
     double start = imu->yaw();
     double final = start + 90;
-    if(final > 360)
+    
+    out->printf("final %f \n", final);
+    out->printf("start %f \n", start);
+    if(final > 360) {
         final -= 360;
-
-    while((imu->yaw() <= final)&& (stop == false)) {
-        Wheelchair::right();
-        out.printf("turning right");
-        if( out.readable()) {
-            out.printf("stopped\n");
-            Wheelchair::stop();
-            return;
+        out->printf("final %f \n", final);
+        while(imu->yaw() - 360 <= 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;
+            }
         }
     }
+    out->printf("done turning\n");
 }
 
 void Wheelchair::turn_left()