first publish
Dependencies: MPU9150_DMP MotorControl QuaternionMath SimpleIOMacros mbed
Fork of cool_step by
Diff: main.cpp
- Revision:
- 13:75892580d4ff
- Parent:
- 12:410ebe8573ce
- Child:
- 14:94a02b617085
diff -r 410ebe8573ce -r 75892580d4ff main.cpp --- a/main.cpp Thu May 14 09:04:28 2015 +0000 +++ b/main.cpp Thu May 14 13:32:39 2015 +0000 @@ -10,10 +10,10 @@ DigitalOut led2(P2_7); DigitalOut led3(P2_8); AnalogIn hallSensor(P0_26); -//InterruptIn hall1_(P0_5), hall2_(P0_4); -//Hallsensor hall1(P1_26, P1_27); +InterruptIn hall1_(P0_5), hall2_(P0_4); +Hallsensor hall1(P1_26, P1_27); -//Motor motor1(P2_0, P2_1,&hall1); +Motor motor1(P2_0, P2_1,&hall1); //Serial RN42(P0_10, P0_11); @@ -28,7 +28,7 @@ Hallsensor hall2(P0_4, P0_5); Motor motor2(P2_2, P2_3, &hall2); -MPU9150 imu(P0_28, P0_27, P2_13); +//MPU9150 imu(P0_28, P0_27, P2_13); char buffer[200]; void infoTask(void) @@ -50,31 +50,31 @@ infoTicker.attach(infoTask,0.2f); - if(imu.isReady()) { + /* if(imu.isReady()) { debug.printf("MPU9150 is ready\r\n"); } else { debug.printf("MPU9150 initialisation failure\r\n"); } - - imu.initialiseDMP(); +*/ + // imu.initialiseDMP(); timer.start(); timer2.start(); - imu.setFifoReset(true); - imu.setDMPEnabled(true); + // imu.setFifoReset(true); + // imu.setDMPEnabled(true); - Quaternion q1; + // Quaternion q1; while(1) { - if(imu.getFifoCount() >= 48) { - imu.getFifoBuffer(buffer, 48); - led2 = !led2; + // 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) { + /* if(timer.read_ms() > 50) { timer.reset(); //This is the format of the data in the fifo, @@ -108,18 +108,18 @@ (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); + // 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); + // 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()); + // 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(0.7); } - } -} + +