Cool-step / Mbed 2 deprecated cool_step_1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MPU9150_DMP MotorControl QuaternionMath SimpleIOMacros mbed

Fork of cool_step_new by Cool-step

main.cpp@8:63eff653d0ba, 2015-04-29 (annotated)

Committer:
heuristics
Date:
Wed Apr 29 15:12:57 2015 +0000
Revision:
8:63eff653d0ba
Parent:
7:7f9abf427b06
Child:
9:ca52462f9ebc
angle finished;

Who changed what in which revision?

UserRevisionLine numberNew 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 void initialize_connection(void);
heuristics 7:7f9abf427b06 23
heuristics 7:7f9abf427b06 24 void initialize_connection()
heuristics 7:7f9abf427b06 25 {
heuristics 7:7f9abf427b06 26 RN42.putc('$'); // Enter command mode
heuristics 7:7f9abf427b06 27 RN42.putc('$'); //
heuristics 7:7f9abf427b06 28 RN42.putc('$'); //
heuristics 7:7f9abf427b06 29 wait(0.5);
heuristics 7:7f9abf427b06 30 RN42.putc('C'); //
heuristics 7:7f9abf427b06 31 RN42.putc(','); // Send MAC address
heuristics 7:7f9abf427b06 32 RN42.putc('0'); //
heuristics 7:7f9abf427b06 33 RN42.putc('0'); //
heuristics 7:7f9abf427b06 34 RN42.putc('0'); //
heuristics 7:7f9abf427b06 35 RN42.putc('6'); //
heuristics 7:7f9abf427b06 36 RN42.putc('6'); //
heuristics 7:7f9abf427b06 37 RN42.putc('6'); //
heuristics 7:7f9abf427b06 38 RN42.putc('A'); //
heuristics 7:7f9abf427b06 39 RN42.putc('0'); //
heuristics 7:7f9abf427b06 40 RN42.putc('4'); //
heuristics 7:7f9abf427b06 41 RN42.putc('E'); //
heuristics 7:7f9abf427b06 42 RN42.putc('1'); //
heuristics 7:7f9abf427b06 43 RN42.putc('A'); //
heuristics 7:7f9abf427b06 44 wait(0.5);
heuristics 7:7f9abf427b06 45 RN42.putc('-'); // Exit command mode
heuristics 7:7f9abf427b06 46 RN42.putc('-'); //
heuristics 7:7f9abf427b06 47 RN42.putc('-'); //
heuristics 7:7f9abf427b06 48 RN42.putc(0x0D); //
heuristics 7:7f9abf427b06 49 wait(0.5);
heuristics 7:7f9abf427b06 50 }
heuristics 7:7f9abf427b06 51
heuristics 7:7f9abf427b06 52
heuristics 4:7ccb10039316 53 char buffer[200];
heuristics 1:7681221d0a5c 54 void infoTask(void)
heuristics 1:7681221d0a5c 55 {
heuristics 1:7681221d0a5c 56 led1=!led1;
heuristics 1:7681221d0a5c 57 }
heuristics 1:7681221d0a5c 58
heuristics 7:7f9abf427b06 59
heuristics 1:7681221d0a5c 60 int main()
heuristics 1:7681221d0a5c 61 {
heuristics 4:7ccb10039316 62 RN42.baud(9600);
heuristics 4:7ccb10039316 63 debug.baud(115200);
heuristics 6:3bd16fdf0b6e 64
heuristics 7:7f9abf427b06 65 // initialize_connection();
heuristics 7:7f9abf427b06 66
heuristics 4:7ccb10039316 67 infoTicker.attach(infoTask,0.2f);
heuristics 4:7ccb10039316 68
heuristics 4:7ccb10039316 69 if(imu.isReady()) {
heuristics 4:7ccb10039316 70 debug.printf("MPU9150 is ready\r\n");
heuristics 4:7ccb10039316 71 } else {
heuristics 4:7ccb10039316 72 debug.printf("MPU9150 initialisation failure\r\n");
heuristics 4:7ccb10039316 73 }
heuristics 4:7ccb10039316 74
heuristics 4:7ccb10039316 75 imu.initialiseDMP();
heuristics 4:7ccb10039316 76
heuristics 4:7ccb10039316 77 timer.start();
heuristics 5:3953111e9476 78 timer2.start();
heuristics 4:7ccb10039316 79
heuristics 4:7ccb10039316 80 imu.setFifoReset(true);
heuristics 4:7ccb10039316 81 imu.setDMPEnabled(true);
heuristics 4:7ccb10039316 82
heuristics 4:7ccb10039316 83 Quaternion q1;
heuristics 0:94acd21860e4 84 while(1) {
heuristics 6:3bd16fdf0b6e 85
heuristics 4:7ccb10039316 86 if(imu.getFifoCount() >= 48) {
heuristics 4:7ccb10039316 87 imu.getFifoBuffer(buffer, 48);
heuristics 4:7ccb10039316 88 led2 = !led2;
heuristics 4:7ccb10039316 89 }
heuristics 4:7ccb10039316 90
heuristics 7:7f9abf427b06 91 if(timer.read_ms() > 50) {
heuristics 4:7ccb10039316 92 timer.reset();
heuristics 4:7ccb10039316 93
heuristics 4:7ccb10039316 94 //This is the format of the data in the fifo,
heuristics 4:7ccb10039316 95 /* ================================================================================================ *
heuristics 4:7ccb10039316 96 | Default MotionApps v4.1 48-byte FIFO packet structure: |
heuristics 4:7ccb10039316 97 | |
heuristics 4:7ccb10039316 98 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
heuristics 4:7ccb10039316 99 | 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 100 | |
heuristics 4:7ccb10039316 101 | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
heuristics 4:7ccb10039316 102 | 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 103 * ================================================================================================ */
heuristics 4:7ccb10039316 104
heuristics 4:7ccb10039316 105 /*
heuristics 4:7ccb10039316 106 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 107 (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]),
heuristics 4:7ccb10039316 108 (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45]));
heuristics 4:7ccb10039316 109
heuristics 4:7ccb10039316 110 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 111 (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]),
heuristics 4:7ccb10039316 112 (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27]));
heuristics 4:7ccb10039316 113
heuristics 4:7ccb10039316 114 debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28],
heuristics 4:7ccb10039316 115 (int16_t)(buffer[31] << 8) + buffer[30],
heuristics 4:7ccb10039316 116 (int16_t)(buffer[33] << 8) + buffer[32]);
heuristics 4:7ccb10039316 117
heuristics 4:7ccb10039316 118 debug.printf("%f, %f, %f, %f\r\n",
heuristics 4:7ccb10039316 119 (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 120 (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 121 (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 122 (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 123 */
heuristics 4:7ccb10039316 124
heuristics 4:7ccb10039316 125 q1.decode(buffer);
heuristics 7:7f9abf427b06 126 debug.printf("%f, %f, %f, %f ", q1.w, q1.v.x, q1.v.y, q1.v.z);
heuristics 7:7f9abf427b06 127
heuristics 7:7f9abf427b06 128 Vector3 vector_x(1,0,0),vector_y(0,1,0),vector_z(0,0,1);
heuristics 8:63eff653d0ba 129 Vector3 pry=q1.getEulerAngles();
heuristics 8:63eff653d0ba 130 /*Vector3 vector_x_local=q1.rotate(vector_x);
heuristics 7:7f9abf427b06 131 Vector3 vector_y_local=q1.rotate(vector_y);
heuristics 7:7f9abf427b06 132 Vector3 vector_z_local=q1.rotate(vector_z);
heuristics 7:7f9abf427b06 133 Vector3 direction_plane_normal=vector_z_local.cross_product(vector_z);
heuristics 7:7f9abf427b06 134 Vector3 bar_projection=vector_z-vector_x_local*(vector_z.dot_product(vector_x_local)/vector_x_local.length_squared());
heuristics 7:7f9abf427b06 135 Vector3 bar_projection_g=bar_projection-vector_z*(bar_projection.dot_product(vector_z)/vector_z.length_squared());
heuristics 8:63eff653d0ba 136 float angle=acos(vector_y_local.dot_product(vector_z)/vector_y_local.length()/vector_z.length());*/
heuristics 8:63eff653d0ba 137 //debug.printf("angle: %f ", angle/PI*180);
heuristics 8:63eff653d0ba 138 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 139
heuristics 4:7ccb10039316 140 //TeaPot Demo Packet for MotionFit SDK
heuristics 4:7ccb10039316 141 /*
heuristics 4:7ccb10039316 142 debug.putc('$'); //packet start
heuristics 4:7ccb10039316 143 debug.putc((char)2); //assume packet type constant
heuristics 4:7ccb10039316 144 debug.putc((char)0); //count seems unused
heuristics 4:7ccb10039316 145 for(int i = 0; i < 16; i++){ //16 bytes for 4 32bit floats
heuristics 4:7ccb10039316 146 debug.putc((char)(buffer[i]));
heuristics 4:7ccb10039316 147 }
heuristics 4:7ccb10039316 148 for(int i = 0; i < 5; i++){ //no idea, padded with 0
heuristics 4:7ccb10039316 149 debug.putc((char)0);
heuristics 4:7ccb10039316 150 }
heuristics 4:7ccb10039316 151 */
heuristics 4:7ccb10039316 152 }
heuristics 0:94acd21860e4 153 }
heuristics 0:94acd21860e4 154 }