Revision 11:d14a1f7f1297, committed 2018-08-01

Comitter:

ryanlin97

Date:

Wed Aug 01 22:39:22 2018 +0000

Parent:

10:e5463c11e0a0

Child:

12:921488918749

Commit message:

fiddling with the turning to correct itself

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/chair_BNO055.lib	Wed Aug 01 22:39:22 2018 +0000
@@ -0,0 +1,1 @@
+chair_BNO055#3258d62af038

--- a/chair_MPU9250.lib	Mon Jul 23 20:17:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/ryanlin97/code/chair_MPU9250/#66660dcf55fb

--- a/main.cpp	Mon Jul 23 20:17:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,129 +0,0 @@
-#include "wheelchair.h"
-
-AnalogIn x(A0);
-AnalogIn y(A1);
-
-DigitalOut off(D0);
-DigitalOut on(D1);
-DigitalOut up(D2);
-DigitalOut down(D3);
-
-bool manual = false;
-
-Serial pc(USBTX, USBRX, 57600);
-Timer t;
-
-MPU9250 imu(D14, D15);
-//Wheelchair smart(xDir,yDir, &pc, &t);
-
-int main(void)
-{
-
-    uint8_t whoami = imu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
-    pc.printf("I AM 0x%x\n\r", whoami);
-    pc.printf("I SHOULD BE 0x71\n\r");
-
-    if (whoami == 0x71) { // WHO_AM_I should always be 0x68
-        pc.printf("MPU9250 is online...\n\r");
-
-        wait(1);
-
-        imu.resetMPU9250(); // Reset registers to default in preparation for device calibration
-        imu.calibrateMPU9250(imu.gyroBias, imu.accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
-        imu.initMPU9250();
-        imu.initAK8963(imu.magCalibration);
-        wait(2);
-
-    } else {
-        pc.printf("Could not connect to MPU9250: \n\r");
-        pc.printf("%#x \n",  whoami);
-
-        while(1) ; // Loop forever if communication doesn't happen
-    }
-
-    imu.getAres(); // Get accelerometer sensitivity
-    imu.getGres(); // Get gyro sensitivity
-    imu.getMres(); // Get magnetometer sensitivity
-    pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/imu.aRes);
-    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/imu.gRes);
-    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/imu.mRes);
-    imu.magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
-    imu.magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
-    imu.magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
-
-
-    Wheelchair smart(xDir,yDir, &pc, &t);
-
-    while(1) {
-        if( pc.readable()) {
-            char c = pc.getc();
-
-            if( c == 'w') {
-                pc.printf("up \n");
-                smart.forward();
-            }
-
-            else if( c == 'd') {
-                pc.printf("left \n");
-                smart.left();
-            }
-
-            else if( c == 'a') {
-                pc.printf("right \n");
-                smart.right();
-            }
-
-            else if( c == 's') {
-                pc.printf("down \n");
-                smart.backward();
-            }
-
-            else if( c == 'r') {
-                smart.turn_right();
-            }
-
-            else if( c == 'l') {
-                smart.turn_left();
-            }
-
-            else if( c == 'm' || manual) {
-                pc.printf("turning on joystick\n");
-                manual = true;
-                while(manual) {
-                    smart.move(x,y);
-                    if( pc.readable()) {
-                        char d = pc.getc();
-                        if( d == 'm') {
-                            pc.printf("turning off joystick\n");
-                            manual = false;
-                        }
-                    }
-                }
-            }
-
-            else {
-                pc.printf("none \n");
-                smart.stop();
-            }
-        }
-
-        else {
-            //        pc.printf("Nothing pressed \n");
-            smart.stop();
-        }
-        /*
-        smart.move(x,y);
-        if( pc.readable()) {
-            char c = pc.getc();
-            if( c == 'r') {
-                smart.turn_right();
-            }
-            if( c == 'l') {
-                smart.turn_left();
-            }
-        }*/
-        wait(process);
-    }
-
-}
-

--- a/mbed.bld	Mon Jul 23 20:17:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/mbed_official/code/mbed/builds/a7c7b631e539
\ No newline at end of file

--- 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;
+        //} 
+    
+    
 }
 
 

--- a/wheelchair.h	Mon Jul 23 20:17:37 2018 +0000
+++ b/wheelchair.h	Wed Aug 01 22:39:22 2018 +0000
@@ -1,9 +1,10 @@
 #ifndef wheelchair
 #define wheelchair
 
-//#include "chair_BNO055.h"
-#include "chair_MPU9250.h"
+#include "chair_BNO055.h"
+//#include "chair_MPU9250.h"
 
+#define turn_precision 10
 #define def (2.5f/3.3f)
 #define high 3.3f
 #define offset .02f
@@ -17,20 +18,24 @@
 public:
     Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time);
     void move(float x_coor, float y_coor);
-    void turn_right();
-    void turn_left();
+    double turn_right(int deg);
+    double turn_left(int deg);
+    void turn(int deg);
     void forward();
     void backward();
     void right();
     void left();
     void stop();
+    void compass_thread();
+    chair_BNO055* imu;
 
 private:
     PwmOut* x;
     PwmOut* y;
     //chair_BNO055* imu;
-    chair_MPU9250* imu;
+    //chair_MPU9250* imu;
     Serial* out;
+    Timer* ti;
 
 };
 #endif
\ No newline at end of file