File content as of revision 8:381a4ec3fef8:

#include "wheelchair.h"

AnalogIn x(A0);
AnalogIn y(A1);

DigitalOut off(D0);
DigitalOut on(D1);
DigitalOut up(D2);
DigitalOut down(D3);

Serial pc(USBTX, USBRX, 57600);
Timer t;
Timer other;

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 {
                   pc.printf("none \n");
                   smart.stop();
                   if( c == 'o') {
                      pc.printf("turning on");
                      on = 0;
                      wait(process);
                      on = 1;
                       }

                   else if( c == 'k') {
                       off = 0;
                       wait(process);
                       off = 1;
                       }

                   else if( c == 'u') {
                       up = 0;
                       wait(process);
                       up = 1;
                       }

                   else if( c == 'p') {
                       down = 0;
                       wait(process);
                       down = 1;
                       }
                   }
                  }

           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);
    }

}