Edson Alcala
/
StepTracking
Simple step tracking
Fork of ST by
main.cpp@1:3eec9883598a, 2017-10-12 (annotated)
- Committer:
- alonsopg
- Date:
- Thu Oct 12 18:47:26 2017 +0000
- Revision:
- 1:3eec9883598a
- Parent:
- 0:30a995e45e2a
- Child:
- 2:02175845b24c
before integrating bluetooth
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
alonsopg | 0:30a995e45e2a | 1 | #include "mbed.h" |
alonsopg | 0:30a995e45e2a | 2 | #include "MPU9250.h" |
alonsopg | 0:30a995e45e2a | 3 | |
alonsopg | 0:30a995e45e2a | 4 | // Serial comms |
alonsopg | 0:30a995e45e2a | 5 | Serial pc(USBTX, USBRX); |
alonsopg | 0:30a995e45e2a | 6 | |
alonsopg | 0:30a995e45e2a | 7 | // Sensor board library |
alonsopg | 0:30a995e45e2a | 8 | MPU9250 mpu = MPU9250(p26, p27); |
alonsopg | 0:30a995e45e2a | 9 | |
alonsopg | 0:30a995e45e2a | 10 | // Configuration |
alonsopg | 1:3eec9883598a | 11 | bool testMPUConnection = true; |
alonsopg | 1:3eec9883598a | 12 | bool doSensorInitialization = false; |
alonsopg | 1:3eec9883598a | 13 | bool printAccelerometer = true; |
alonsopg | 1:3eec9883598a | 14 | bool printGyroscope = true; |
alonsopg | 0:30a995e45e2a | 15 | |
alonsopg | 0:30a995e45e2a | 16 | int main () { |
alonsopg | 1:3eec9883598a | 17 | int16_t accelerometer[3] = {0,0,0}; |
alonsopg | 1:3eec9883598a | 18 | int16_t gyroscope[3] = {0,0,0}; |
alonsopg | 1:3eec9883598a | 19 | |
alonsopg | 1:3eec9883598a | 20 | if (testMPUConnection) { |
alonsopg | 0:30a995e45e2a | 21 | uint8_t whoami = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 |
alonsopg | 0:30a995e45e2a | 22 | pc.printf("I AM 0x%x\n\r", whoami); |
alonsopg | 0:30a995e45e2a | 23 | pc.printf("I SHOULD BE 0x71\n\r"); |
alonsopg | 0:30a995e45e2a | 24 | wait(1); |
alonsopg | 0:30a995e45e2a | 25 | } |
alonsopg | 0:30a995e45e2a | 26 | |
alonsopg | 1:3eec9883598a | 27 | if (doSensorInitialization) { |
alonsopg | 1:3eec9883598a | 28 | // Initialise sensor board |
alonsopg | 1:3eec9883598a | 29 | pc.printf("Initializing sensor\n\r"); |
alonsopg | 1:3eec9883598a | 30 | mpu.initMPU9250(); |
alonsopg | 1:3eec9883598a | 31 | pc.printf("Initialization finished\n\r"); |
alonsopg | 1:3eec9883598a | 32 | wait(1); |
alonsopg | 1:3eec9883598a | 33 | } |
alonsopg | 1:3eec9883598a | 34 | |
alonsopg | 0:30a995e45e2a | 35 | while(1) { |
alonsopg | 1:3eec9883598a | 36 | pc.printf("Starting to stream data"); |
alonsopg | 1:3eec9883598a | 37 | if(printAccelerometer){ |
alonsopg | 1:3eec9883598a | 38 | mpu.readAccelData(accelerometer); |
alonsopg | 1:3eec9883598a | 39 | float ax = accelerometer[0] * 2.0 / 32768.0; |
alonsopg | 1:3eec9883598a | 40 | float ay = accelerometer[1] * 2.0 / 32768.0; |
alonsopg | 1:3eec9883598a | 41 | float az = accelerometer[2] * 2.0 / 32768.0; |
alonsopg | 1:3eec9883598a | 42 | |
alonsopg | 1:3eec9883598a | 43 | pc.printf("Acelerometer information, AX, AY, AZ \n"); |
alonsopg | 1:3eec9883598a | 44 | pc.printf("(%f, %f, %f)\n", ax,ay,az); |
alonsopg | 1:3eec9883598a | 45 | |
alonsopg | 1:3eec9883598a | 46 | float roll = float(atan2(ay, az) * 180/3.1416f); |
alonsopg | 1:3eec9883598a | 47 | float pitch = float(atan2(-ax, sqrt(ay*ay + az*az)) * 180/3.1416f); |
alonsopg | 1:3eec9883598a | 48 | float yaw = atan(ax/-ay); |
alonsopg | 1:3eec9883598a | 49 | |
alonsopg | 1:3eec9883598a | 50 | pc.printf("Roll/Pitch/Yaw: (%f, %f, %f)\n", roll, pitch, yaw); |
alonsopg | 1:3eec9883598a | 51 | } |
alonsopg | 1:3eec9883598a | 52 | |
alonsopg | 1:3eec9883598a | 53 | if(printGyroscope){ |
alonsopg | 1:3eec9883598a | 54 | mpu.readGyroData(gyroscope); |
alonsopg | 1:3eec9883598a | 55 | float gx = gyroscope[0] * 250.0 / 32768.0; |
alonsopg | 1:3eec9883598a | 56 | float gy = gyroscope[1] * 250.0 / 32768.0; |
alonsopg | 1:3eec9883598a | 57 | float gz = gyroscope[2] * 250.0 / 32768.0; |
alonsopg | 1:3eec9883598a | 58 | pc.printf("Gyroscope information, GX, GY, GZ \n"); |
alonsopg | 1:3eec9883598a | 59 | pc.printf("(:%f, %f, %f)\n", gx,gy,gz); |
alonsopg | 1:3eec9883598a | 60 | } |
alonsopg | 1:3eec9883598a | 61 | |
alonsopg | 1:3eec9883598a | 62 | wait(0.3); |
alonsopg | 0:30a995e45e2a | 63 | } |
alonsopg | 0:30a995e45e2a | 64 | } |