Diff: wheelchair.cpp

Revision:

11:d14a1f7f1297

Parent:

10:e5463c11e0a0

Child:

12:921488918749

--- a/wheelchair.cpp	Mon Jul 23 20:17:37 2018 +0000
+++ b/wheelchair.cpp	Wed Aug 01 22:39:22 2018 +0000
@@ -1,15 +1,35 @@
 #include "wheelchair.h"
+
 bool manual_drive = false;
+volatile float north;
+volatile float curr_yaw;
+volatile float comp_yn;
 
+void Wheelchair::compass_thread() {
+     //north = lowPass(imu->angle_north());
+     curr_yaw = imu->yaw();
+     north = boxcar(imu->angle_north());
+     //out->printf("curr_yaw %f\n", curr_yaw);
+     comp_yn = complement(north,curr_yaw,.5);
+     out->printf("%f curr_yaw\n", curr_yaw);
+     out->printf("%f gyroz\n",imu->gyro_z());
+     //out->printf("yaw is %f, north is %f, curr_yaw is %f\n", comp_yn, north, curr_yaw);
+    
+        //out->printf("Yaw is %f\n", imu->yaw());
+        //out->printf("north is %f\n", imu->angle_north());
+    }
+    
 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 = new chair_BNO055(pc, time);
+    //imu = new chair_MPU9250(pc, time);
+    Wheelchair::stop();
     imu->setup();
     out = pc;
     out->printf("wheelchair setup done \n");
+    ti = time;
 
 }
 
@@ -21,10 +41,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 +67,15 @@
 void Wheelchair::right()
 {
     x->write(def);
-    y->write(high);
+    y->write(low);
+    //y->write(1.9);
+
 }
 
 void Wheelchair::left()
 {
     x->write(def);
-    y->write(low);
+    y->write(high);
 }
 
 void Wheelchair::stop()
@@ -54,76 +83,115 @@
     x->write(def);
     y->write(def);
 }
-// counter clockwise is +
-// clockwise is -
+// counter clockwise is -
+// clockwise is +
 
-void Wheelchair::turn_right()
+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;
+    double start = curr_yaw;
+    double final = start + deg;
+
+    if(final > 360) {
+        final -= 360;
         overturn = true;
     }
 
     out->printf("start %f, final %f\n", start, final);
-    
-    double curr = 361;
-    while(curr >= final) {
+
+    double curr = -1;
+    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;
+        curr = curr_yaw;
+        if(overturn && curr > (360 - deg) ) {
+            curr = 0;
         }
     }
-
-    out->printf("done turning\n");
+    
+    out->printf("done turning start %f final %f\n", start, final);
     Wheelchair::stop();
-
+    
+    return final;
 }
 
-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;
+    double start = comp_yn;
+    double final = start - deg;
 
-    if(final > 360) {
-        final -= 360;
+    if(final < 0) {
+        final += 360;
         overturn = true;
     }
 
-out->printf("start %f, final %f\n", start, final);
+    out->printf("start %f, final %f\n", start, final);
 
-    double curr = -1;
-    while(curr <= final) {
+    double curr = 361;
+    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;
+        curr = curr_yaw;
+
+        if(overturn && curr >= 0 && curr <= start ) {
+            curr = 361;
         }
     }
 
-    out->printf("done turning\n");
+    out->printf("done turning start %f final %f\n", start, final);
     Wheelchair::stop();
+
+    return final;
+}
+
+void Wheelchair::turn(int deg)
+{
+    if(deg > 180) {
+        deg -= 360;
+    }
+
+    else if(deg < -180) {
+        deg+=360;
+    }  
+    
+    double finalpos;
+    int turnAmt = abs(deg);
+    //ti->reset();
+    /*
+    if(deg >= 0){
+        finalpos = Wheelchair::turn_right(turnAmt);
+        }
+    else {
+        finalpos = Wheelchair::turn_left(turnAmt);
+        }
+    */
+    wait(2);
+    
+    float correction = finalpos - curr_yaw;
+    out->printf("final pos %f actual pos %f\n", finalpos, curr_yaw);
+    
+    
+    //if(abs(correction) > turn_precision) {
+        out->printf("correcting %f\n", correction);
+        //ti->reset();
+        Wheelchair::turn_left(curr_yaw - finalpos);
+        return;
+        //} 
+    
+    
 }