Diff: main.cpp

Revision:

7:5e38d43fbce3

Parent:

6:0cd57bdd8fbc

Child:

8:381a4ec3fef8

diff -r 0cd57bdd8fbc -r 5e38d43fbce3 main.cpp
--- a/main.cpp	Fri Jul 20 17:54:43 2018 +0000
+++ b/main.cpp	Sun Jul 22 06:15:03 2018 +0000
@@ -8,15 +8,53 @@
 DigitalOut up(D2);
 DigitalOut down(D3);
 
-Serial pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX, 57600);
+Timer t;
+Timer other;
 
-Wheelchair smart(xDir,yDir, &pc);
+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
+    
+    
     on = 1;
     while(1) {
-        /*   if( pc.readable()) {
+   /*        if( pc.readable()) {
                char c = pc.getc();
                if( c == 'w') {
                    pc.printf("up \n");
@@ -72,12 +110,12 @@
               pc.printf("Nothing pressed \n");
               smart.stop();
                    }
-          */
+     */     
         smart.move(x,y);
         if( pc.readable()) {
             char c = pc.getc();
             if( c == 'r') {
-                smart.turn_right(pc);
+                smart.turn_right();
             }
         }
         wait(process);