File content as of revision 10:e5463c11e0a0:

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

}