fsdf
Dependencies: BNO055_fusion MODSERIAL mbed
Fork of Shared-1BNO055 by
main.cpp@15:304c0fcf6575, 2016-02-22 (annotated)
- Committer:
- tommyallen
- Date:
- Mon Feb 22 18:54:27 2016 +0000
- Revision:
- 15:304c0fcf6575
- Parent:
- 14:418d1703806c
wdszhds
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tommyallen | 15:304c0fcf6575 | 1 | //-----TOBY GALAL AND TOM ALLEN FIGHTING CRIME-----// |
tommyallen | 14:418d1703806c | 2 | |
kenjiArai | 0:31451519d283 | 3 | /* |
kenjiArai | 0:31451519d283 | 4 | * mbed Application program for the mbed Nucleo F401 |
kenjiArai | 0:31451519d283 | 5 | * BNO055 Intelligent 9-axis absolute orientation sensor |
kenjiArai | 0:31451519d283 | 6 | * by Bosch Sensortec |
kenjiArai | 0:31451519d283 | 7 | * |
kenjiArai | 0:31451519d283 | 8 | * Copyright (c) 2015 Kenji Arai / JH1PJL |
kenjiArai | 0:31451519d283 | 9 | * http://www.page.sannet.ne.jp/kenjia/index.html |
kenjiArai | 0:31451519d283 | 10 | * http://mbed.org/users/kenjiArai/ |
kenjiArai | 0:31451519d283 | 11 | * Created: March 30th, 2015 |
kenjiArai | 5:9594519c9462 | 12 | * Revised: April 16th, 2015 |
kenjiArai | 0:31451519d283 | 13 | * |
kenjiArai | 0:31451519d283 | 14 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, |
kenjiArai | 0:31451519d283 | 15 | * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE |
kenjiArai | 0:31451519d283 | 16 | * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, |
kenjiArai | 0:31451519d283 | 17 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
kenjiArai | 0:31451519d283 | 18 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. |
kenjiArai | 0:31451519d283 | 19 | */ |
kenjiArai | 0:31451519d283 | 20 | |
kenjiArai | 0:31451519d283 | 21 | // Include --------------------------------------------------------------------------------------- |
kenjiArai | 0:31451519d283 | 22 | #include "mbed.h" |
kenjiArai | 0:31451519d283 | 23 | #include "BNO055.h" |
tommyallen | 7:d4b5e83c7947 | 24 | #include "MODSERIAL.h" |
tommyallen | 11:d68282e48fd4 | 25 | //#include "arm_math.h" |
kenjiArai | 0:31451519d283 | 26 | |
kenjiArai | 0:31451519d283 | 27 | // Definition ------------------------------------------------------------------------------------ |
kenjiArai | 3:f5b5c4d795ce | 28 | #define NUM_LOOP 100 |
kenjiArai | 3:f5b5c4d795ce | 29 | |
kenjiArai | 0:31451519d283 | 30 | // Object ---------------------------------------------------------------------------------------- |
tommyallen | 7:d4b5e83c7947 | 31 | MODSERIAL pc(USBTX,USBRX); |
tommyallen | 7:d4b5e83c7947 | 32 | |
tommyallen | 8:41d1f9ab3be0 | 33 | volatile bool RAW = true; |
tommyallen | 8:41d1f9ab3be0 | 34 | volatile bool FUS = false; |
tommyallen | 7:d4b5e83c7947 | 35 | bool printed = false; |
tommyallen | 9:7be40e42e578 | 36 | int Time; |
tommyallen | 13:f5cccb8beac0 | 37 | int TimeStart; |
el13ytg | 12:c8019aca78d0 | 38 | int TimeStamp = 0; |
tommyallen | 13:f5cccb8beac0 | 39 | int TimeStamp2 = 0; |
tommyallen | 13:f5cccb8beac0 | 40 | int TimeOfStart = 0; |
tommyallen | 15:304c0fcf6575 | 41 | int TimeOfStartms = 0; |
el13ytg | 12:c8019aca78d0 | 42 | int TimeNow; |
tommyallen | 13:f5cccb8beac0 | 43 | int sync = 1; |
el13ytg | 12:c8019aca78d0 | 44 | bool direction = false; |
tommyallen | 6:0590c7ff8c34 | 45 | |
tommyallen | 8:41d1f9ab3be0 | 46 | //string sdata = ""; |
tommyallen | 8:41d1f9ab3be0 | 47 | |
tommyallen | 7:d4b5e83c7947 | 48 | DigitalOut led1(LED1); |
tommyallen | 7:d4b5e83c7947 | 49 | DigitalOut led2(LED2); |
tommyallen | 7:d4b5e83c7947 | 50 | DigitalOut led3(LED3); |
tommyallen | 7:d4b5e83c7947 | 51 | DigitalOut led4(LED4); |
kenjiArai | 2:cf77282aea7b | 52 | DigitalOut pwr_onoff(p30); |
tommyallen | 15:304c0fcf6575 | 53 | DigitalOut oscill(p20); |
tommyallen | 7:d4b5e83c7947 | 54 | |
kenjiArai | 2:cf77282aea7b | 55 | I2C i2c(p28, p27); // SDA, SCL |
kenjiArai | 2:cf77282aea7b | 56 | BNO055 imu(i2c, p29); // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default |
tommyallen | 7:d4b5e83c7947 | 57 | LocalFileSystem local("local"); // Create the local filesystem under the name "local" |
tommyallen | 6:0590c7ff8c34 | 58 | |
kenjiArai | 3:f5b5c4d795ce | 59 | Timer t; |
kenjiArai | 0:31451519d283 | 60 | |
el13ytg | 12:c8019aca78d0 | 61 | PwmOut PWM (p21); |
el13ytg | 12:c8019aca78d0 | 62 | |
kenjiArai | 0:31451519d283 | 63 | // RAM ------------------------------------------------------------------------------------------- |
kenjiArai | 0:31451519d283 | 64 | BNO055_ID_INF_TypeDef bno055_id_inf; |
kenjiArai | 0:31451519d283 | 65 | BNO055_EULER_TypeDef euler_angles; |
kenjiArai | 0:31451519d283 | 66 | BNO055_QUATERNION_TypeDef quaternion; |
kenjiArai | 0:31451519d283 | 67 | BNO055_LIN_ACC_TypeDef linear_acc; |
kenjiArai | 0:31451519d283 | 68 | BNO055_GRAVITY_TypeDef gravity; |
kenjiArai | 0:31451519d283 | 69 | BNO055_TEMPERATURE_TypeDef chip_temp; |
kenjiArai | 0:31451519d283 | 70 | |
kenjiArai | 0:31451519d283 | 71 | // ROM / Constant data --------------------------------------------------------------------------- |
kenjiArai | 0:31451519d283 | 72 | |
kenjiArai | 0:31451519d283 | 73 | // Function prototypes --------------------------------------------------------------------------- |
kenjiArai | 0:31451519d283 | 74 | |
tommyallen | 7:d4b5e83c7947 | 75 | |
tommyallen | 7:d4b5e83c7947 | 76 | //------------------------------------------------------------------------------------------------- |
tommyallen | 7:d4b5e83c7947 | 77 | // Serial Interrupt |
tommyallen | 7:d4b5e83c7947 | 78 | //------------------------------------------------------------------------------------------------- |
tommyallen | 7:d4b5e83c7947 | 79 | void rxCallback(MODSERIAL_IRQ_INFO *q) |
tommyallen | 7:d4b5e83c7947 | 80 | { |
tommyallen | 7:d4b5e83c7947 | 81 | MODSERIAL *serial = q->serial; |
tommyallen | 8:41d1f9ab3be0 | 82 | if ( serial->rxGetLastChar() == 'f') { |
tommyallen | 8:41d1f9ab3be0 | 83 | imu.write_reg0(0x3d, 0x0c); |
tommyallen | 8:41d1f9ab3be0 | 84 | RAW = false; |
tommyallen | 8:41d1f9ab3be0 | 85 | FUS = true; |
tommyallen | 8:41d1f9ab3be0 | 86 | pc.printf("\r\nSWITCHING TO FUSION DATA!\r\n"); |
tommyallen | 8:41d1f9ab3be0 | 87 | } |
tommyallen | 8:41d1f9ab3be0 | 88 | if ( serial->rxGetLastChar() == 'r') { |
tommyallen | 8:41d1f9ab3be0 | 89 | imu.write_reg0(0x3d, 0x07); |
tommyallen | 8:41d1f9ab3be0 | 90 | RAW = true; |
tommyallen | 8:41d1f9ab3be0 | 91 | FUS = false; |
tommyallen | 8:41d1f9ab3be0 | 92 | pc.printf("\r\nSWITCHING TO RAW DATA!\r\n"); |
tommyallen | 7:d4b5e83c7947 | 93 | } |
tommyallen | 10:acbb851070c2 | 94 | if ( serial->rxGetLastChar() == '*') { |
tommyallen | 15:304c0fcf6575 | 95 | TimeOfStart = t.read_us(); |
tommyallen | 15:304c0fcf6575 | 96 | TimeOfStartms = t.read_ms(); |
el13ytg | 12:c8019aca78d0 | 97 | TimeStamp = 0; |
tommyallen | 13:f5cccb8beac0 | 98 | pc.printf("RESETTING TIME"); |
tommyallen | 10:acbb851070c2 | 99 | } |
tommyallen | 7:d4b5e83c7947 | 100 | } |
tommyallen | 7:d4b5e83c7947 | 101 | |
kenjiArai | 0:31451519d283 | 102 | //------------------------------------------------------------------------------------------------- |
kenjiArai | 0:31451519d283 | 103 | // Control Program |
kenjiArai | 3:f5b5c4d795ce | 104 | //------------------------------------------------------------------------------------------------- |
kenjiArai | 4:6d1118089a36 | 105 | // Calibration |
kenjiArai | 4:6d1118089a36 | 106 | // Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data |
tommyallen | 7:d4b5e83c7947 | 107 | void bno055_calbration(void) |
tommyallen | 7:d4b5e83c7947 | 108 | { |
kenjiArai | 4:6d1118089a36 | 109 | uint8_t d; |
tommyallen | 7:d4b5e83c7947 | 110 | pc.printf("------ Enter BNO055 Manual Calibration Mode ------\r\n"); |
kenjiArai | 4:6d1118089a36 | 111 | //---------- Gyroscope Caliblation ------------------------------------------------------------ |
kenjiArai | 4:6d1118089a36 | 112 | // (a) Place the device in a single stable position for a period of few seconds to allow the |
kenjiArai | 4:6d1118089a36 | 113 | // gyroscope to calibrate |
kenjiArai | 4:6d1118089a36 | 114 | pc.printf("Step1) Please wait few seconds\r\n"); |
kenjiArai | 4:6d1118089a36 | 115 | t.start(); |
tommyallen | 7:d4b5e83c7947 | 116 | while (t.read() < 10) { |
kenjiArai | 4:6d1118089a36 | 117 | d = imu.read_calib_status(); |
tommyallen | 6:0590c7ff8c34 | 118 | pc.printf("calibrating = 0x%x target = 0x30(at least)\r\n", d); |
tommyallen | 7:d4b5e83c7947 | 119 | if ((d & 0x30) == 0x30) { |
kenjiArai | 4:6d1118089a36 | 120 | break; |
kenjiArai | 4:6d1118089a36 | 121 | } |
kenjiArai | 4:6d1118089a36 | 122 | wait(1.0); |
kenjiArai | 4:6d1118089a36 | 123 | } |
kenjiArai | 4:6d1118089a36 | 124 | pc.printf("-> Step1) is done\r\n\r\n"); |
kenjiArai | 4:6d1118089a36 | 125 | //---------- Magnetometer Caliblation --------------------------------------------------------- |
kenjiArai | 4:6d1118089a36 | 126 | // (a) Make some random movements (for example: writing the number ‘8’ on air) until the |
kenjiArai | 4:6d1118089a36 | 127 | // CALIB_STAT register indicates fully calibrated. |
kenjiArai | 4:6d1118089a36 | 128 | // (b) It takes more calibration movements to get the magnetometer calibrated than in the |
kenjiArai | 4:6d1118089a36 | 129 | // NDOF mode. |
kenjiArai | 4:6d1118089a36 | 130 | pc.printf("Step2) random moving (try to change the BNO055 axis)\r\n"); |
kenjiArai | 4:6d1118089a36 | 131 | t.start(); |
tommyallen | 7:d4b5e83c7947 | 132 | while (t.read() < 30) { |
kenjiArai | 4:6d1118089a36 | 133 | d = imu.read_calib_status(); |
tommyallen | 6:0590c7ff8c34 | 134 | pc.printf("calibrating, = 0x%x target = 0x33(at least)\r\n", d); |
tommyallen | 7:d4b5e83c7947 | 135 | if ((d & 0x03) == 0x03) { |
kenjiArai | 4:6d1118089a36 | 136 | break; |
kenjiArai | 4:6d1118089a36 | 137 | } |
tommyallen | 7:d4b5e83c7947 | 138 | wait(1.0); |
kenjiArai | 4:6d1118089a36 | 139 | } |
kenjiArai | 4:6d1118089a36 | 140 | pc.printf("-> Step2) is done\r\n\r\n"); |
kenjiArai | 4:6d1118089a36 | 141 | //---------- Magnetometer Caliblation --------------------------------------------------------- |
kenjiArai | 4:6d1118089a36 | 142 | // a) Place the device in 6 different stable positions for a period of few seconds |
kenjiArai | 4:6d1118089a36 | 143 | // to allow the accelerometer to calibrate. |
kenjiArai | 4:6d1118089a36 | 144 | // b) Make sure that there is slow movement between 2 stable positions |
kenjiArai | 4:6d1118089a36 | 145 | // The 6 stable positions could be in any direction, but make sure that the device is |
kenjiArai | 4:6d1118089a36 | 146 | // lying at least once perpendicular to the x, y and z axis. |
kenjiArai | 4:6d1118089a36 | 147 | pc.printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!"); |
kenjiArai | 4:6d1118089a36 | 148 | pc.printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n"); |
kenjiArai | 4:6d1118089a36 | 149 | pc.printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,"); |
kenjiArai | 4:6d1118089a36 | 150 | pc.printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n"); |
kenjiArai | 4:6d1118089a36 | 151 | pc.printf(" If you will give up, hit any key.\r\n", d); |
kenjiArai | 4:6d1118089a36 | 152 | t.stop(); |
tommyallen | 7:d4b5e83c7947 | 153 | while (true) { |
kenjiArai | 4:6d1118089a36 | 154 | d = imu.read_calib_status(); |
kenjiArai | 4:6d1118089a36 | 155 | imu.get_gravity(&gravity); |
tommyallen | 6:0590c7ff8c34 | 156 | pc.printf("calibrating = 0x%x target = 0xff ACC:X %4.1f, Y %4.1f, Z %4.1f\r\n", |
tommyallen | 7:d4b5e83c7947 | 157 | d, gravity.x, gravity.y, gravity.z); |
tommyallen | 7:d4b5e83c7947 | 158 | if (d == 0xff) { |
tommyallen | 7:d4b5e83c7947 | 159 | break; |
tommyallen | 7:d4b5e83c7947 | 160 | } |
tommyallen | 7:d4b5e83c7947 | 161 | if (pc.readable()) { |
tommyallen | 7:d4b5e83c7947 | 162 | break; |
tommyallen | 7:d4b5e83c7947 | 163 | } |
kenjiArai | 4:6d1118089a36 | 164 | wait(1.0); |
kenjiArai | 4:6d1118089a36 | 165 | } |
tommyallen | 7:d4b5e83c7947 | 166 | if (imu.read_calib_status() == 0xff) { |
kenjiArai | 4:6d1118089a36 | 167 | pc.printf("-> All of Calibration steps are done successfully!\r\n\r\n"); |
kenjiArai | 4:6d1118089a36 | 168 | } else { |
kenjiArai | 4:6d1118089a36 | 169 | pc.printf("-> Calibration steps are suspended!\r\n\r\n"); |
kenjiArai | 4:6d1118089a36 | 170 | } |
kenjiArai | 4:6d1118089a36 | 171 | t.stop(); |
kenjiArai | 4:6d1118089a36 | 172 | } |
tommyallen | 7:d4b5e83c7947 | 173 | |
tommyallen | 8:41d1f9ab3be0 | 174 | //------------------------------------------------------------------------------------------------- |
tommyallen | 8:41d1f9ab3be0 | 175 | //------------------------------------------------------------------------------------------------- |
tommyallen | 8:41d1f9ab3be0 | 176 | //------------------------------------------------------------------------------------------------- |
tommyallen | 8:41d1f9ab3be0 | 177 | //------------------------------------------------------------------------------------------------- |
tommyallen | 8:41d1f9ab3be0 | 178 | // Main Program |
tommyallen | 8:41d1f9ab3be0 | 179 | //------------------------------------------------------------------------------------------------- |
tommyallen | 8:41d1f9ab3be0 | 180 | //------------------------------------------------------------------------------------------------- |
tommyallen | 8:41d1f9ab3be0 | 181 | //------------------------------------------------------------------------------------------------- |
tommyallen | 8:41d1f9ab3be0 | 182 | //------------------------------------------------------------------------------------------------- |
tommyallen | 7:d4b5e83c7947 | 183 | int main() |
tommyallen | 7:d4b5e83c7947 | 184 | { |
tommyallen | 15:304c0fcf6575 | 185 | oscill=0; |
tommyallen | 13:f5cccb8beac0 | 186 | pc.baud(921600); |
tommyallen | 7:d4b5e83c7947 | 187 | pc.attach(&rxCallback, MODSERIAL::RxIrq); |
tommyallen | 13:f5cccb8beac0 | 188 | pc.printf("BNO055 Hello World/\r\n\r\n"); |
kenjiArai | 4:6d1118089a36 | 189 | imu.set_mounting_position(MT_P6); |
kenjiArai | 4:6d1118089a36 | 190 | pwr_onoff = 0; |
tommyallen | 13:f5cccb8beac0 | 191 | pc.printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!/\r\n"); |
kenjiArai | 4:6d1118089a36 | 192 | char c = pc.getc(); |
tommyallen | 7:d4b5e83c7947 | 193 | pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n"); |
kenjiArai | 4:6d1118089a36 | 194 | // Is BNO055 avairable? |
tommyallen | 7:d4b5e83c7947 | 195 | if (imu.chip_ready() == 0) { |
kenjiArai | 4:6d1118089a36 | 196 | do { |
tommyallen | 13:f5cccb8beac0 | 197 | pc.printf("Bosch BNO055 is NOT avirable!!\r\n Reset/\r\n"); |
kenjiArai | 4:6d1118089a36 | 198 | pwr_onoff = 1; // Power off |
kenjiArai | 4:6d1118089a36 | 199 | wait(0.1); |
kenjiArai | 4:6d1118089a36 | 200 | pwr_onoff = 0; // Power on |
kenjiArai | 4:6d1118089a36 | 201 | wait(0.02); |
kenjiArai | 4:6d1118089a36 | 202 | } while(imu.reset()); |
kenjiArai | 4:6d1118089a36 | 203 | } |
tommyallen | 13:f5cccb8beac0 | 204 | pc.printf("Bosch BNO055 is available now!!/\r\n"); |
tommyallen | 13:f5cccb8beac0 | 205 | pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x/\r\n", |
tommyallen | 7:d4b5e83c7947 | 206 | imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN)); |
kenjiArai | 4:6d1118089a36 | 207 | imu.read_id_inf(&bno055_id_inf); |
tommyallen | 13:f5cccb8beac0 | 208 | pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x,/", |
tommyallen | 7:d4b5e83c7947 | 209 | bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id); |
tommyallen | 13:f5cccb8beac0 | 210 | pc.printf("SW REV:0x%04x, BL REV:0x%02x/\r\n", |
tommyallen | 7:d4b5e83c7947 | 211 | bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); |
tommyallen | 13:f5cccb8beac0 | 212 | pc.printf("If you would like to calibrate the BNO055, please hit 'y' (No: any other key)/\r\n"); |
kenjiArai | 4:6d1118089a36 | 213 | c = pc.getc(); |
tommyallen | 7:d4b5e83c7947 | 214 | if (c == 'y') { |
kenjiArai | 4:6d1118089a36 | 215 | bno055_calbration(); |
kenjiArai | 4:6d1118089a36 | 216 | } |
el13ytg | 12:c8019aca78d0 | 217 | |
tommyallen | 13:f5cccb8beac0 | 218 | pc.printf("If you would like to Synchronize the System, please hit 'y' (No: any other key)\r\n/"); |
tommyallen | 13:f5cccb8beac0 | 219 | c = pc.getc(); |
tommyallen | 13:f5cccb8beac0 | 220 | if (c == 'y') { |
tommyallen | 13:f5cccb8beac0 | 221 | sync=0; |
tommyallen | 13:f5cccb8beac0 | 222 | pc.printf("Synchronise/"); |
tommyallen | 13:f5cccb8beac0 | 223 | wait(0.5); |
tommyallen | 13:f5cccb8beac0 | 224 | } |
tommyallen | 13:f5cccb8beac0 | 225 | |
el13ytg | 12:c8019aca78d0 | 226 | while (sync == 0) { |
el13ytg | 12:c8019aca78d0 | 227 | pc.printf("HELLO/"); |
el13ytg | 12:c8019aca78d0 | 228 | if (pc.readable()) { |
el13ytg | 12:c8019aca78d0 | 229 | if (pc.getc() == 72) { |
el13ytg | 12:c8019aca78d0 | 230 | while (sync == 0) { |
el13ytg | 12:c8019aca78d0 | 231 | if (pc.readable()) { |
el13ytg | 12:c8019aca78d0 | 232 | if (pc.getc() == 73) { |
el13ytg | 12:c8019aca78d0 | 233 | sync = 1; |
el13ytg | 12:c8019aca78d0 | 234 | } |
el13ytg | 12:c8019aca78d0 | 235 | } |
el13ytg | 12:c8019aca78d0 | 236 | } |
el13ytg | 12:c8019aca78d0 | 237 | } |
el13ytg | 12:c8019aca78d0 | 238 | } |
el13ytg | 12:c8019aca78d0 | 239 | } |
el13ytg | 12:c8019aca78d0 | 240 | pc.printf("START/"); |
kenjiArai | 4:6d1118089a36 | 241 | t.start(); |
tommyallen | 8:41d1f9ab3be0 | 242 | imu.write_reg0(0x3d, 0x07); // Default FUS |
tommyallen | 15:304c0fcf6575 | 243 | imu.write_reg0(0x3b, 0x01); |
tommyallen | 15:304c0fcf6575 | 244 | imu.write_reg0(0x08, 0x03); |
tommyallen | 15:304c0fcf6575 | 245 | int units = imu.read_reg0(0x08); |
tommyallen | 8:41d1f9ab3be0 | 246 | while (1) { |
el13ytg | 12:c8019aca78d0 | 247 | while (RAW == true) { |
tommyallen | 9:7be40e42e578 | 248 | Time = t.read_ms(); |
el13ytg | 12:c8019aca78d0 | 249 | if (Time > TimeStamp + 5) { |
tommyallen | 9:7be40e42e578 | 250 | pc.printf("RAW,xLSB:,%d,xMSB:,%d,yLSB:%d,yMSB:,%d,zLSB:,%d,zMSB:,%d," |
tommyallen | 9:7be40e42e578 | 251 | ,imu.read_reg0(0x08),imu.read_reg0(0x09) |
tommyallen | 9:7be40e42e578 | 252 | ,imu.read_reg0(0x0A),imu.read_reg0(0x0B) |
tommyallen | 9:7be40e42e578 | 253 | ,imu.read_reg0(0x0C),imu.read_reg0(0x0D)); |
el13ytg | 12:c8019aca78d0 | 254 | pc.printf("Time:,%d,/",t.read_ms()); |
el13ytg | 12:c8019aca78d0 | 255 | TimeStamp = Time; |
tommyallen | 9:7be40e42e578 | 256 | } |
tommyallen | 8:41d1f9ab3be0 | 257 | } |
tommyallen | 8:41d1f9ab3be0 | 258 | while (RAW == false) { |
tommyallen | 13:f5cccb8beac0 | 259 | //Time = t.read_ms(); |
tommyallen | 13:f5cccb8beac0 | 260 | //TimeNow = + t.read_ms() - TimeOfStart; |
tommyallen | 15:304c0fcf6575 | 261 | |
tommyallen | 15:304c0fcf6575 | 262 | //TimeStart = t.read_us() - TimeOfStart; |
tommyallen | 15:304c0fcf6575 | 263 | |
tommyallen | 15:304c0fcf6575 | 264 | if (t.read_us () - TimeOfStart > TimeStamp + 7060) { |
tommyallen | 15:304c0fcf6575 | 265 | |
tommyallen | 15:304c0fcf6575 | 266 | //oscill = 1; |
el13ytg | 12:c8019aca78d0 | 267 | |
tommyallen | 9:7be40e42e578 | 268 | imu.get_linear_accel(&linear_acc); |
el13ytg | 12:c8019aca78d0 | 269 | imu.get_Euler_Angles(&euler_angles); |
el13ytg | 12:c8019aca78d0 | 270 | imu.get_quaternion(&quaternion); |
el13ytg | 12:c8019aca78d0 | 271 | imu.get_gravity(&gravity); |
el13ytg | 12:c8019aca78d0 | 272 | 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,/" |
tommyallen | 15:304c0fcf6575 | 273 | ,t.read_ms() - TimeOfStartms,linear_acc.x ,linear_acc.y ,linear_acc.z, |
el13ytg | 12:c8019aca78d0 | 274 | euler_angles.h, euler_angles.r, euler_angles.p, |
el13ytg | 12:c8019aca78d0 | 275 | quaternion.w, quaternion.x, quaternion.y, quaternion.z, |
tommyallen | 13:f5cccb8beac0 | 276 | gravity.x, gravity.y, gravity.z); |
el13ytg | 12:c8019aca78d0 | 277 | |
tommyallen | 15:304c0fcf6575 | 278 | //oscill = 0; |
tommyallen | 15:304c0fcf6575 | 279 | //wait(1); |
tommyallen | 15:304c0fcf6575 | 280 | |
tommyallen | 15:304c0fcf6575 | 281 | TimeStamp = t.read_us()- TimeOfStart; |
tommyallen | 15:304c0fcf6575 | 282 | //pc.printf("TimeStart:,%d,/",TimeStart); |
tommyallen | 15:304c0fcf6575 | 283 | //pc.printf("Timend:,%d,/",+ t.read_us() - TimeOfStart); |
tommyallen | 9:7be40e42e578 | 284 | } |
el13ytg | 12:c8019aca78d0 | 285 | |
el13ytg | 12:c8019aca78d0 | 286 | /*if (Time > TimeStamp2 + 999) { |
el13ytg | 12:c8019aca78d0 | 287 | if (direction == false) { |
el13ytg | 12:c8019aca78d0 | 288 | PWM.pulsewidth_us(2400); |
el13ytg | 12:c8019aca78d0 | 289 | //pc.printf("DIR, UP,"); |
el13ytg | 12:c8019aca78d0 | 290 | //pc.printf("Time:,%d,/",t.read_ms()); |
el13ytg | 12:c8019aca78d0 | 291 | direction = true; |
el13ytg | 12:c8019aca78d0 | 292 | TimeStamp2 = Time; |
el13ytg | 12:c8019aca78d0 | 293 | } |
el13ytg | 12:c8019aca78d0 | 294 | |
el13ytg | 12:c8019aca78d0 | 295 | else if (direction == true) { |
el13ytg | 12:c8019aca78d0 | 296 | PWM.pulsewidth_us(600); |
el13ytg | 12:c8019aca78d0 | 297 | //pc.printf("DIR, DOWN,"); |
el13ytg | 12:c8019aca78d0 | 298 | //pc.printf("Time:,%d,/",t.read_ms()); |
el13ytg | 12:c8019aca78d0 | 299 | direction = false; |
el13ytg | 12:c8019aca78d0 | 300 | TimeStamp2 = Time; |
el13ytg | 12:c8019aca78d0 | 301 | } |
el13ytg | 12:c8019aca78d0 | 302 | } |
el13ytg | 12:c8019aca78d0 | 303 | */ |
tommyallen | 8:41d1f9ab3be0 | 304 | } |
tommyallen | 8:41d1f9ab3be0 | 305 | } |
tommyallen | 15:304c0fcf6575 | 306 | } |