Edson Alcala
/
StepTracking
Simple step tracking
Fork of ST by
Diff: main.cpp
- Revision:
- 1:3eec9883598a
- Parent:
- 0:30a995e45e2a
- Child:
- 2:02175845b24c
--- a/main.cpp Thu Oct 12 18:06:49 2017 +0000 +++ b/main.cpp Thu Oct 12 18:47:26 2017 +0000 @@ -8,27 +8,57 @@ MPU9250 mpu = MPU9250(p26, p27); // Configuration -bool test_comms = true; -bool do_sensor_init = false; -bool do_sensor_self_test = true; -bool print_accel = true; -bool print_gyro = true; +bool testMPUConnection = true; +bool doSensorInitialization = false; +bool printAccelerometer = true; +bool printGyroscope = true; int main () { - float test_result[6] = {0.0,0.0,0.0,0.0,0.0,0.0}; - int16_t accel[3] = {0,0,0}; - int16_t gyro[3] = {0,0,0}; - int16_t temp = 0; - - // Test MPU connection - if (test_comms) { + int16_t accelerometer[3] = {0,0,0}; + int16_t gyroscope[3] = {0,0,0}; + + if (testMPUConnection) { uint8_t whoami = mpu.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"); wait(1); } + if (doSensorInitialization) { + // Initialise sensor board + pc.printf("Initializing sensor\n\r"); + mpu.initMPU9250(); + pc.printf("Initialization finished\n\r"); + wait(1); + } + while(1) { - pc.printf("Hi hi"); + pc.printf("Starting to stream data"); + if(printAccelerometer){ + mpu.readAccelData(accelerometer); + float ax = accelerometer[0] * 2.0 / 32768.0; + float ay = accelerometer[1] * 2.0 / 32768.0; + float az = accelerometer[2] * 2.0 / 32768.0; + + pc.printf("Acelerometer information, AX, AY, AZ \n"); + pc.printf("(%f, %f, %f)\n", ax,ay,az); + + float roll = float(atan2(ay, az) * 180/3.1416f); + float pitch = float(atan2(-ax, sqrt(ay*ay + az*az)) * 180/3.1416f); + float yaw = atan(ax/-ay); + + pc.printf("Roll/Pitch/Yaw: (%f, %f, %f)\n", roll, pitch, yaw); + } + + if(printGyroscope){ + mpu.readGyroData(gyroscope); + float gx = gyroscope[0] * 250.0 / 32768.0; + float gy = gyroscope[1] * 250.0 / 32768.0; + float gz = gyroscope[2] * 250.0 / 32768.0; + pc.printf("Gyroscope information, GX, GY, GZ \n"); + pc.printf("(:%f, %f, %f)\n", gx,gy,gz); + } + + wait(0.3); } } \ No newline at end of file