first publish
Dependencies: MPU9150_DMP MotorControl QuaternionMath SimpleIOMacros mbed
Fork of cool_step by
main.cpp
- Committer:
- heuristics
- Date:
- 2015-05-13
- Revision:
- 11:ba1a630e56c4
- Parent:
- 10:09dbd00164b9
- Child:
- 12:410ebe8573ce
File content as of revision 11:ba1a630e56c4:
#include "mbed.h" #include "MPU9150.h" #include "Quaternion.h" #include "Motor.h" void InitTimer0(void); //led pins DigitalOut led1(P2_6); DigitalOut led2(P2_7); DigitalOut led3(P2_8); AnalogIn hallSensor(P0_26); volatile int pulses_ = 0; //InterruptIn hall1_(P0_5), hall2_(P0_4); //Hallsensor hall1(P1_26, P1_27); //Hallsensor hall2(P0_4, P0_5); //Motor motor1(P2_0, P2_1,&hall1); //Motor motor2(P2_2, P2_3, &hall2); MPU9150 imu(P0_28, P0_27, P2_13); Serial RN42(P0_10, P0_11); Serial debug(P0_2, P0_3); Ticker infoTicker; Timer timer; Timer timer2; char buffer[200]; void infoTask(void) { led1=!led1; } void deneme(void) { led3=!led3; } int main() { RN42.baud(9600); debug.baud(115200); // InitTimer0(); // initialize_connection(); infoTicker.attach(infoTask,0.2f); if(imu.isReady()) { debug.printf("MPU9150 is ready\r\n"); } else { debug.printf("MPU9150 initialisation failure\r\n"); } imu.initialiseDMP(); timer.start(); timer2.start(); imu.setFifoReset(true); imu.setDMPEnabled(true); Quaternion q1; while(1) { if(imu.getFifoCount() >= 48) { imu.getFifoBuffer(buffer, 48); led2 = !led2; //debug.printf("%d\r\n",timer2.read_ms()); timer2.reset(); } if(timer.read_ms() > 50) { timer.reset(); //This is the format of the data in the fifo, /* ================================================================================================ * | Default MotionApps v4.1 48-byte FIFO packet structure: | | | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | | | | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | * ================================================================================================ */ /* 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]), (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]), (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45])); 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]), (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]), (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27])); debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28], (int16_t)(buffer[31] << 8) + buffer[30], (int16_t)(buffer[33] << 8) + buffer[32]); debug.printf("%f, %f, %f, %f\r\n", (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)), (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)), (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)), (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30))); */ q1.decode(buffer); debug.printf("%f, ",hallSensor.read()); debug.printf("%f, %f, %f, %f ", q1.w, q1.v.x, q1.v.y, q1.v.z); Vector3 vector_x(1,0,0),vector_y(0,1,0),vector_z(0,0,1); Vector3 pry=q1.getEulerAngles(); debug.printf("p: %f r: %f y: %f ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180); debug.printf("m1: %d m2: %d \r\n ",motor2.getPulses(),motor2.getPulses()); //motor1.setVelocity(pry.x/90); motor2.setVelocity(pry.x/90); } } }