Diff: wheelchair.cpp

Revision:

6:0cd57bdd8fbc

Parent:

5:e0ccaab3959a

Child:

7:5e38d43fbce3

--- a/wheelchair.cpp	Tue Jul 17 07:19:04 2018 +0000
+++ b/wheelchair.cpp	Fri Jul 20 17:54:43 2018 +0000
@@ -1,44 +1,25 @@
 #include "wheelchair.h"
 
-Wheelchair::Wheelchair(PinName xPin, PinName yPin)
+Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc)
 {
     x = new PwmOut(xPin);
     y = new PwmOut(yPin);
-    imu = new chair_imu();
+    //imu = new chair_BNO055();
+    imu = new chair_MPU9250(pc);
+    
 }
+
 /*
 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
 */
 void Wheelchair::move(float x_coor, float y_coor)
 {
-     
+
     float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
     float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
     x->write(scaled_x);
     y->write(scaled_y);
-    
-}
 
-void Wheelchair::turn_right(){
-    double start = imu->yaw();
-    double final = start + 90;
-    if(final > 360)
-        final -= 360;
-   
-    while(imu->yaw() <= final) {
-        Wheelchair::right();
-        } 
-}
-
-void Wheelchair::turn_left(){
-    double start = imu->yaw();
-    double final = start - 90;
-    if(final <0)
-        final += 360;
-   
-    while(imu->yaw() >= final) {
-        Wheelchair::left();
-        } 
 }
 void Wheelchair::forward()
 {
@@ -68,4 +49,35 @@
 {
     x->write(def);
     y->write(def);
-}
\ No newline at end of file
+}
+
+void Wheelchair::turn_right(Serial out)
+{
+    bool stop = false;
+    double start = imu->yaw();
+    double final = start + 90;
+    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;
+        }
+    }
+}
+
+void Wheelchair::turn_left()
+{
+    double start = imu->yaw();
+    double final = start - 90;
+    if(final <0)
+        final += 360;
+
+    while(imu->yaw() >= final) {
+        Wheelchair::left();
+    }
+}