ryan lin
/
wheeldriveassistance
wheelchair code for driver assitance
Fork of wheelchairalexa by
Diff: main.cpp
- Revision:
- 7:5e38d43fbce3
- Parent:
- 6:0cd57bdd8fbc
- Child:
- 8:381a4ec3fef8
--- 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);