fsdf
Dependencies: BNO055_fusion MODSERIAL mbed
Fork of Shared-1BNO055 by
Diff: main.cpp
- Revision:
- 15:304c0fcf6575
- Parent:
- 14:418d1703806c
--- a/main.cpp Thu Feb 18 00:47:42 2016 +0000 +++ b/main.cpp Mon Feb 22 18:54:27 2016 +0000 @@ -1,4 +1,4 @@ -//-----TOBY GALA AND TOM ALLEN FIGHTING CRIME-----// +//-----TOBY GALAL AND TOM ALLEN FIGHTING CRIME-----// /* * mbed Application program for the mbed Nucleo F401 @@ -38,6 +38,7 @@ int TimeStamp = 0; int TimeStamp2 = 0; int TimeOfStart = 0; +int TimeOfStartms = 0; int TimeNow; int sync = 1; bool direction = false; @@ -49,6 +50,7 @@ DigitalOut led3(LED3); DigitalOut led4(LED4); DigitalOut pwr_onoff(p30); +DigitalOut oscill(p20); I2C i2c(p28, p27); // SDA, SCL BNO055 imu(i2c, p29); // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default @@ -90,7 +92,8 @@ pc.printf("\r\nSWITCHING TO RAW DATA!\r\n"); } if ( serial->rxGetLastChar() == '*') { - TimeOfStart = t.read_ms(); + TimeOfStart = t.read_us(); + TimeOfStartms = t.read_ms(); TimeStamp = 0; pc.printf("RESETTING TIME"); } @@ -179,6 +182,7 @@ //------------------------------------------------------------------------------------------------- int main() { + oscill=0; pc.baud(921600); pc.attach(&rxCallback, MODSERIAL::RxIrq); pc.printf("BNO055 Hello World/\r\n\r\n"); @@ -236,7 +240,9 @@ pc.printf("START/"); t.start(); imu.write_reg0(0x3d, 0x07); // Default FUS - + imu.write_reg0(0x3b, 0x01); + imu.write_reg0(0x08, 0x03); + int units = imu.read_reg0(0x08); while (1) { while (RAW == true) { Time = t.read_ms(); @@ -252,24 +258,29 @@ while (RAW == false) { //Time = t.read_ms(); //TimeNow = + t.read_ms() - TimeOfStart; - if (t.read_ms() - TimeOfStart > TimeStamp + 5) { - - //TimeStart = t.read_ms() - TimeOfStart; + + //TimeStart = t.read_us() - TimeOfStart; + + if (t.read_us () - TimeOfStart > TimeStamp + 7060) { + + //oscill = 1; imu.get_linear_accel(&linear_acc); 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,/" - ,t.read_ms() - TimeOfStart,linear_acc.x ,linear_acc.y ,linear_acc.z, + ,t.read_ms() - TimeOfStartms,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); - - //pc.printf("TimeStart:,%d,/",TimeStart); - //pc.printf("Timend:,%d,/",+ t.read_ms() - TimeOfStart); - TimeStamp = t.read_ms()- TimeOfStart; + //oscill = 0; + //wait(1); + + TimeStamp = t.read_us()- TimeOfStart; + //pc.printf("TimeStart:,%d,/",TimeStart); + //pc.printf("Timend:,%d,/",+ t.read_us() - TimeOfStart); } /*if (Time > TimeStamp2 + 999) { @@ -292,36 +303,4 @@ */ } } -} - -// imu.get_Euler_Angles(&euler_angles); -// pc.printf("X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", -// euler_angles.h, euler_angles.r, euler_angles.p); -// imu.get_linear_accel(&linear_acc); -// pc.printf("Acc:,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", -// linear_acc.x, linear_acc.y, linear_acc.z); -// -// imu.get_gravity(&gravity); -// pc.printf("Gravity,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", -// gravity.x, gravity.y, gravity.z); -// -// pc.printf("Time:,%d,\r\n",t.read_ms()); -// imu.get_Euler_Angles(&euler_angles); -// pc.printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,", -// euler_angles.h, euler_angles.r, euler_angles.p); -// imu.get_quaternion(&quaternion); -// pc.printf("[Q],W,%d,X,%d,Y,%d,Z,%d,", -// quaternion.w, quaternion.x, quaternion.y, quaternion.z); -// imu.get_linear_accel(&linear_acc); -// pc.printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", -// linear_acc.x, linear_acc.y, linear_acc.z); -// imu.get_gravity(&gravity); -// pc.printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", -// gravity.x, gravity.y, gravity.z); -// imu.get_chip_temperature(&chip_temp); -// pc.printf("[T],%+d,%+d,", -// chip_temp.acc_chip, chip_temp.gyr_chip); -// pc.printf("[S],0x%x,[M],%d\r\n", -// imu.read_calib_status(), t.read_ms()); -// pc.printf("Words\r\n"); - +} \ No newline at end of file