fsdf
Dependencies: BNO055_fusion MODSERIAL mbed
Fork of Shared-1BNO055 by
Diff: main.cpp
- Revision:
- 12:c8019aca78d0
- Parent:
- 11:d68282e48fd4
- Child:
- 13:f5cccb8beac0
diff -r d68282e48fd4 -r c8019aca78d0 main.cpp --- a/main.cpp Wed Feb 17 12:41:10 2016 +0000 +++ b/main.cpp Wed Feb 17 18:03:04 2016 +0000 @@ -32,9 +32,12 @@ volatile bool FUS = false; bool printed = false; int Time; -int timeStamp = 0; -int timeOfStart; -int timeNow; +int TimeStamp = 0; +int TimeStamp2=0; +int TimeOfStart; +int TimeNow; +int sync = 0; +bool direction = false; //string sdata = ""; @@ -50,6 +53,8 @@ Timer t; +PwmOut PWM (p21); + // RAM ------------------------------------------------------------------------------------------- BNO055_ID_INF_TypeDef bno055_id_inf; BNO055_EULER_TypeDef euler_angles; @@ -82,8 +87,8 @@ pc.printf("\r\nSWITCHING TO RAW DATA!\r\n"); } if ( serial->rxGetLastChar() == '*') { - timeOfStart = t.read_ms(); - timeStamp = 0; + TimeOfStart = t.read_ms(); + TimeStamp = 0; } } @@ -201,31 +206,88 @@ if (c == 'y') { bno055_calbration(); } + + while (sync == 0) { + pc.printf("HELLO/"); + if (pc.readable()) { + if (pc.getc() == 72) { + while (sync == 0) { + if (pc.readable()) { + if (pc.getc() == 73) { + sync = 1; + } + } + } + } + } + } + pc.printf("START/"); t.start(); imu.write_reg0(0x3d, 0x07); // Default FUS - + while (1) { - while (RAW == true) { + while (RAW == true) { Time = t.read_ms(); - timeNow = Time + timeOfStart; - if (timeNow > timeStamp + 9) { + if (Time > TimeStamp + 5) { pc.printf("RAW,xLSB:,%d,xMSB:,%d,yLSB:%d,yMSB:,%d,zLSB:,%d,zMSB:,%d," ,imu.read_reg0(0x08),imu.read_reg0(0x09) ,imu.read_reg0(0x0A),imu.read_reg0(0x0B) ,imu.read_reg0(0x0C),imu.read_reg0(0x0D)); - pc.printf("Time:,%d,/",timeNow); - timeStamp = timeNow; + pc.printf("Time:,%d,/",t.read_ms()); + TimeStamp = Time; } } while (RAW == false) { Time = t.read_ms(); - timeNow = + Time - timeOfStart; - if (timeNow > timeStamp + 9) { + TimeNow = + Time - TimeOfStart; + if (TimeNow > TimeStamp + 5) { + TimeStamp = TimeNow; + //imu.get_linear_accel(&linear_acc); +// pc.printf("FUS,T:,%d,x:,%+6.1f,y:,%+6.1f,z:,%+6.1f, , , , ," +// ,TimeNow,linear_acc.x ,linear_acc.y ,linear_acc.z); +// +// imu.get_Euler_Angles(&euler_angles); +// pc.printf("Y,%+6.1f,R,%+6.1f,P,%+6.1f,", +// euler_angles.h, euler_angles.r, euler_angles.p); +// +// imu.get_quaternion(&quaternion); +// pc.printf("W,%d,X,%d,Y,%d,Z,%d,", +// quaternion.w, quaternion.x, quaternion.y, quaternion.z); +// +// imu.get_gravity(&gravity); +// pc.printf("X,%+6.1f,Y,%+6.1f,Z,%+6.1f,/", +// gravity.x, gravity.y, gravity.z); + imu.get_linear_accel(&linear_acc); - pc.printf("FUS,Time:,%d,x:,%+6.1f,y:,%+6.1f,z:,%+6.1f, , , , , , , , , /" - ,timeNow,linear_acc.x ,linear_acc.y ,linear_acc.z); - timeStamp = timeNow; + imu.get_Euler_Angles(&euler_angles); + imu.get_quaternion(&quaternion); + imu.get_gravity(&gravity); + pc.printf("FUS,T:,%d,x:,%+6.1f,y:,%+6.1f,z:,%+6.1f, , ,Y,%+6.1f,R,%+6.1f,P,%+6.1f,W,%d,X,%d,Y,%d,Z,%d,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,/" + ,TimeNow,linear_acc.x ,linear_acc.y ,linear_acc.z, + euler_angles.h, euler_angles.r, euler_angles.p, + quaternion.w, quaternion.x, quaternion.y, quaternion.z, + gravity.x, gravity.y, gravity.z); + } + + /*if (Time > TimeStamp2 + 999) { + if (direction == false) { + PWM.pulsewidth_us(2400); + //pc.printf("DIR, UP,"); + //pc.printf("Time:,%d,/",t.read_ms()); + direction = true; + TimeStamp2 = Time; + } + + else if (direction == true) { + PWM.pulsewidth_us(600); + //pc.printf("DIR, DOWN,"); + //pc.printf("Time:,%d,/",t.read_ms()); + direction = false; + TimeStamp2 = Time; + } + } + */ } } }