first publish
Dependencies: MPU9150_DMP MotorControl QuaternionMath SimpleIOMacros mbed
Fork of cool_step by
main.cpp@9:ca52462f9ebc, 2015-05-05 (annotated)
- Committer:
- heuristics
- Date:
- Tue May 05 09:07:11 2015 +0000
- Revision:
- 9:ca52462f9ebc
- Parent:
- 8:63eff653d0ba
- Child:
- 10:09dbd00164b9
imu_rate_inc;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
heuristics | 0:94acd21860e4 | 1 | #include "mbed.h" |
heuristics | 4:7ccb10039316 | 2 | #include "MPU9150.h" |
heuristics | 4:7ccb10039316 | 3 | #include "Quaternion.h" |
heuristics | 0:94acd21860e4 | 4 | |
heuristics | 4:7ccb10039316 | 5 | //led pins |
heuristics | 1:7681221d0a5c | 6 | DigitalOut led1(P2_6); |
heuristics | 1:7681221d0a5c | 7 | DigitalOut led2(P2_7); |
heuristics | 1:7681221d0a5c | 8 | DigitalOut led3(P2_8); |
heuristics | 0:94acd21860e4 | 9 | |
heuristics | 4:7ccb10039316 | 10 | |
heuristics | 4:7ccb10039316 | 11 | MPU9150 imu(P0_28, P0_27, P2_13); |
heuristics | 4:7ccb10039316 | 12 | |
heuristics | 4:7ccb10039316 | 13 | |
heuristics | 4:7ccb10039316 | 14 | Serial RN42(P0_10, P0_11); |
heuristics | 4:7ccb10039316 | 15 | Serial debug(P0_2, P0_3); |
heuristics | 4:7ccb10039316 | 16 | |
heuristics | 1:7681221d0a5c | 17 | Ticker infoTicker; |
heuristics | 4:7ccb10039316 | 18 | |
heuristics | 5:3953111e9476 | 19 | Timer timer; |
heuristics | 5:3953111e9476 | 20 | Timer timer2; |
heuristics | 5:3953111e9476 | 21 | |
heuristics | 7:7f9abf427b06 | 22 | |
heuristics | 7:7f9abf427b06 | 23 | |
heuristics | 4:7ccb10039316 | 24 | char buffer[200]; |
heuristics | 1:7681221d0a5c | 25 | void infoTask(void) |
heuristics | 1:7681221d0a5c | 26 | { |
heuristics | 1:7681221d0a5c | 27 | led1=!led1; |
heuristics | 1:7681221d0a5c | 28 | } |
heuristics | 1:7681221d0a5c | 29 | |
heuristics | 7:7f9abf427b06 | 30 | |
heuristics | 1:7681221d0a5c | 31 | int main() |
heuristics | 1:7681221d0a5c | 32 | { |
heuristics | 4:7ccb10039316 | 33 | RN42.baud(9600); |
heuristics | 4:7ccb10039316 | 34 | debug.baud(115200); |
heuristics | 6:3bd16fdf0b6e | 35 | |
heuristics | 7:7f9abf427b06 | 36 | // initialize_connection(); |
heuristics | 7:7f9abf427b06 | 37 | |
heuristics | 4:7ccb10039316 | 38 | infoTicker.attach(infoTask,0.2f); |
heuristics | 4:7ccb10039316 | 39 | |
heuristics | 4:7ccb10039316 | 40 | if(imu.isReady()) { |
heuristics | 4:7ccb10039316 | 41 | debug.printf("MPU9150 is ready\r\n"); |
heuristics | 4:7ccb10039316 | 42 | } else { |
heuristics | 4:7ccb10039316 | 43 | debug.printf("MPU9150 initialisation failure\r\n"); |
heuristics | 4:7ccb10039316 | 44 | } |
heuristics | 4:7ccb10039316 | 45 | |
heuristics | 4:7ccb10039316 | 46 | imu.initialiseDMP(); |
heuristics | 4:7ccb10039316 | 47 | |
heuristics | 4:7ccb10039316 | 48 | timer.start(); |
heuristics | 5:3953111e9476 | 49 | timer2.start(); |
heuristics | 4:7ccb10039316 | 50 | |
heuristics | 4:7ccb10039316 | 51 | imu.setFifoReset(true); |
heuristics | 4:7ccb10039316 | 52 | imu.setDMPEnabled(true); |
heuristics | 4:7ccb10039316 | 53 | |
heuristics | 4:7ccb10039316 | 54 | Quaternion q1; |
heuristics | 0:94acd21860e4 | 55 | while(1) { |
heuristics | 6:3bd16fdf0b6e | 56 | |
heuristics | 4:7ccb10039316 | 57 | if(imu.getFifoCount() >= 48) { |
heuristics | 4:7ccb10039316 | 58 | imu.getFifoBuffer(buffer, 48); |
heuristics | 4:7ccb10039316 | 59 | led2 = !led2; |
heuristics | 9:ca52462f9ebc | 60 | debug.printf("%d\r\n",timer2.read_ms()); |
heuristics | 9:ca52462f9ebc | 61 | timer2.reset(); |
heuristics | 4:7ccb10039316 | 62 | } |
heuristics | 4:7ccb10039316 | 63 | |
heuristics | 7:7f9abf427b06 | 64 | if(timer.read_ms() > 50) { |
heuristics | 4:7ccb10039316 | 65 | timer.reset(); |
heuristics | 4:7ccb10039316 | 66 | |
heuristics | 4:7ccb10039316 | 67 | //This is the format of the data in the fifo, |
heuristics | 4:7ccb10039316 | 68 | /* ================================================================================================ * |
heuristics | 4:7ccb10039316 | 69 | | Default MotionApps v4.1 48-byte FIFO packet structure: | |
heuristics | 4:7ccb10039316 | 70 | | | |
heuristics | 4:7ccb10039316 | 71 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | |
heuristics | 4:7ccb10039316 | 72 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | |
heuristics | 4:7ccb10039316 | 73 | | | |
heuristics | 4:7ccb10039316 | 74 | | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | |
heuristics | 4:7ccb10039316 | 75 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | |
heuristics | 4:7ccb10039316 | 76 | * ================================================================================================ */ |
heuristics | 4:7ccb10039316 | 77 | |
heuristics | 4:7ccb10039316 | 78 | /* |
heuristics | 4:7ccb10039316 | 79 | debug.printf("%d, %d, %d\r\n", (int32_t)(((int32_t)buffer[34] << 24) + ((int32_t)buffer[35] << 16) + ((int32_t)buffer[36] << 8) + (int32_t)buffer[37]), |
heuristics | 4:7ccb10039316 | 80 | (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]), |
heuristics | 4:7ccb10039316 | 81 | (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45])); |
heuristics | 4:7ccb10039316 | 82 | |
heuristics | 4:7ccb10039316 | 83 | debug.printf("%d, %d, %d\r\n", (int32_t)(((int32_t)buffer[16] << 24) + ((int32_t)buffer[17] << 16) + ((int32_t)buffer[18] << 8) + (int32_t)buffer[19]), |
heuristics | 4:7ccb10039316 | 84 | (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]), |
heuristics | 4:7ccb10039316 | 85 | (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27])); |
heuristics | 4:7ccb10039316 | 86 | |
heuristics | 4:7ccb10039316 | 87 | debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28], |
heuristics | 4:7ccb10039316 | 88 | (int16_t)(buffer[31] << 8) + buffer[30], |
heuristics | 4:7ccb10039316 | 89 | (int16_t)(buffer[33] << 8) + buffer[32]); |
heuristics | 4:7ccb10039316 | 90 | |
heuristics | 4:7ccb10039316 | 91 | debug.printf("%f, %f, %f, %f\r\n", |
heuristics | 4:7ccb10039316 | 92 | (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)), |
heuristics | 4:7ccb10039316 | 93 | (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)), |
heuristics | 4:7ccb10039316 | 94 | (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)), |
heuristics | 4:7ccb10039316 | 95 | (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30))); |
heuristics | 4:7ccb10039316 | 96 | */ |
heuristics | 4:7ccb10039316 | 97 | |
heuristics | 4:7ccb10039316 | 98 | q1.decode(buffer); |
heuristics | 7:7f9abf427b06 | 99 | debug.printf("%f, %f, %f, %f ", q1.w, q1.v.x, q1.v.y, q1.v.z); |
heuristics | 7:7f9abf427b06 | 100 | |
heuristics | 7:7f9abf427b06 | 101 | Vector3 vector_x(1,0,0),vector_y(0,1,0),vector_z(0,0,1); |
heuristics | 8:63eff653d0ba | 102 | Vector3 pry=q1.getEulerAngles(); |
heuristics | 8:63eff653d0ba | 103 | /*Vector3 vector_x_local=q1.rotate(vector_x); |
heuristics | 7:7f9abf427b06 | 104 | Vector3 vector_y_local=q1.rotate(vector_y); |
heuristics | 7:7f9abf427b06 | 105 | Vector3 vector_z_local=q1.rotate(vector_z); |
heuristics | 7:7f9abf427b06 | 106 | Vector3 direction_plane_normal=vector_z_local.cross_product(vector_z); |
heuristics | 7:7f9abf427b06 | 107 | Vector3 bar_projection=vector_z-vector_x_local*(vector_z.dot_product(vector_x_local)/vector_x_local.length_squared()); |
heuristics | 7:7f9abf427b06 | 108 | Vector3 bar_projection_g=bar_projection-vector_z*(bar_projection.dot_product(vector_z)/vector_z.length_squared()); |
heuristics | 8:63eff653d0ba | 109 | float angle=acos(vector_y_local.dot_product(vector_z)/vector_y_local.length()/vector_z.length());*/ |
heuristics | 8:63eff653d0ba | 110 | //debug.printf("angle: %f ", angle/PI*180); |
heuristics | 8:63eff653d0ba | 111 | debug.printf("p: %f r: %f y: %f \r\n ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180); |
heuristics | 4:7ccb10039316 | 112 | |
heuristics | 4:7ccb10039316 | 113 | //TeaPot Demo Packet for MotionFit SDK |
heuristics | 4:7ccb10039316 | 114 | /* |
heuristics | 4:7ccb10039316 | 115 | debug.putc('$'); //packet start |
heuristics | 4:7ccb10039316 | 116 | debug.putc((char)2); //assume packet type constant |
heuristics | 4:7ccb10039316 | 117 | debug.putc((char)0); //count seems unused |
heuristics | 4:7ccb10039316 | 118 | for(int i = 0; i < 16; i++){ //16 bytes for 4 32bit floats |
heuristics | 4:7ccb10039316 | 119 | debug.putc((char)(buffer[i])); |
heuristics | 4:7ccb10039316 | 120 | } |
heuristics | 4:7ccb10039316 | 121 | for(int i = 0; i < 5; i++){ //no idea, padded with 0 |
heuristics | 4:7ccb10039316 | 122 | debug.putc((char)0); |
heuristics | 4:7ccb10039316 | 123 | } |
heuristics | 4:7ccb10039316 | 124 | */ |
heuristics | 4:7ccb10039316 | 125 | } |
heuristics | 0:94acd21860e4 | 126 | } |
heuristics | 0:94acd21860e4 | 127 | } |