Diff: wheelchair.cpp

Revision:

11:75f0f13ff6c1

Parent:

10:e5463c11e0a0

Child:

12:0e5a0571b497

--- a/wheelchair.cpp	Mon Jul 23 20:17:37 2018 +0000
+++ b/wheelchair.cpp	Thu Aug 16 16:42:45 2018 +0000
@@ -1,16 +1,24 @@
 #include "wheelchair.h"
+
 bool manual_drive = false;
+volatile float north;
+//volatile double curr_yaw;
+double curr_yaw;
+double Setpoint, Output;
 
+void Wheelchair::compass_thread() {
+    
+    }
+    
 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, time);
-    imu->setup();
+    //imu = new chair_MPU9250(pc, time);
+    Wheelchair::stop();
     out = pc;
     out->printf("wheelchair setup done \n");
-
+    ti = time;
 }
 
 /*
@@ -21,10 +29,17 @@
 
     float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
     float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
+    
+   // lowPass(scaled_x);
+    //lowPass(scaled_y);
+    
     x->write(scaled_x);
     y->write(scaled_y);
+    
+    //out->printf("yaw %f\n", imu->yaw());
 
 }
+
 void Wheelchair::forward()
 {
     x->write(high);
@@ -40,13 +55,13 @@
 void Wheelchair::right()
 {
     x->write(def);
-    y->write(high);
+    y->write(low);
 }
 
 void Wheelchair::left()
 {
     x->write(def);
-    y->write(low);
+    y->write(high);
 }
 
 void Wheelchair::stop()
@@ -54,76 +69,20 @@
     x->write(def);
     y->write(def);
 }
-// counter clockwise is +
-// clockwise is -
-
-void Wheelchair::turn_right()
+// counter clockwise is -
+// clockwise is +
+double Wheelchair::turn_right(int deg)
 {
-    bool overturn = false;
-    out->printf("turning right\n");
-
-    double start = imu->yaw();
-    double final = start - 90;
-    
-    if(final < 0) {
-        final += 360;
-        overturn = true;
-    }
-
-    out->printf("start %f, final %f\n", start, final);
-    
-    double curr = 361;
-    while(curr >= final) {
-        Wheelchair::right();
-        if( out->readable()) {
-            out->printf("stopped\n");
-            Wheelchair::stop();
-            return;
-        }
-        curr = imu->yaw();
-        
-        
-        if(overturn && curr >= 0 && curr <= start ) {
-            curr = 361;
-        }
-    }
-
-    out->printf("done turning\n");
-    Wheelchair::stop();
-
+    return;
 }
 
-void Wheelchair::turn_left()
+double Wheelchair::turn_left(int deg)
 {
-    bool overturn = false;
-    out->printf("turning left\n");
-
-    double start = imu->yaw();
-    double final = start + 90;
-
-    if(final > 360) {
-        final -= 360;
-        overturn = true;
-    }
-
-out->printf("start %f, final %f\n", start, 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("done turning\n");
-    Wheelchair::stop();
+    return;
 }
 
+void Wheelchair::turn(int deg)
+{
+ return;   
+}