MME ELEC3810 / Mbed 2 deprecated Shared-1BNO055

fsdf

Dependencies:   BNO055_fusion MODSERIAL mbed

Fork of Shared-1BNO055 by Thomas Allen

main.cpp@14:418d1703806c, 2016-02-18 (annotated)

Committer:
tommyallen
Date:
Thu Feb 18 00:47:42 2016 +0000
Revision:
14:418d1703806c
Parent:
13:f5cccb8beac0
Child:
15:304c0fcf6575
TEst

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tommyallen 14:418d1703806c 1 //-----TOBY GALA 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;
el13ytg 12:c8019aca78d0 41 int TimeNow;
tommyallen 13:f5cccb8beac0 42 int sync = 1;
el13ytg 12:c8019aca78d0 43 bool direction = false;
tommyallen 6:0590c7ff8c34 44
tommyallen 8:41d1f9ab3be0 45 //string sdata = "";
tommyallen 8:41d1f9ab3be0 46
tommyallen 7:d4b5e83c7947 47 DigitalOut led1(LED1);
tommyallen 7:d4b5e83c7947 48 DigitalOut led2(LED2);
tommyallen 7:d4b5e83c7947 49 DigitalOut led3(LED3);
tommyallen 7:d4b5e83c7947 50 DigitalOut led4(LED4);
kenjiArai 2:cf77282aea7b 51 DigitalOut pwr_onoff(p30);
tommyallen 7:d4b5e83c7947 52
kenjiArai 2:cf77282aea7b 53 I2C i2c(p28, p27); // SDA, SCL
kenjiArai 2:cf77282aea7b 54 BNO055 imu(i2c, p29); // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default
tommyallen 7:d4b5e83c7947 55 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
tommyallen 6:0590c7ff8c34 56
kenjiArai 3:f5b5c4d795ce 57 Timer t;
kenjiArai 0:31451519d283 58
el13ytg 12:c8019aca78d0 59 PwmOut PWM (p21);
el13ytg 12:c8019aca78d0 60
kenjiArai 0:31451519d283 61 // RAM -------------------------------------------------------------------------------------------
kenjiArai 0:31451519d283 62 BNO055_ID_INF_TypeDef bno055_id_inf;
kenjiArai 0:31451519d283 63 BNO055_EULER_TypeDef euler_angles;
kenjiArai 0:31451519d283 64 BNO055_QUATERNION_TypeDef quaternion;
kenjiArai 0:31451519d283 65 BNO055_LIN_ACC_TypeDef linear_acc;
kenjiArai 0:31451519d283 66 BNO055_GRAVITY_TypeDef gravity;
kenjiArai 0:31451519d283 67 BNO055_TEMPERATURE_TypeDef chip_temp;
kenjiArai 0:31451519d283 68
kenjiArai 0:31451519d283 69 // ROM / Constant data ---------------------------------------------------------------------------
kenjiArai 0:31451519d283 70
kenjiArai 0:31451519d283 71 // Function prototypes ---------------------------------------------------------------------------
kenjiArai 0:31451519d283 72
tommyallen 7:d4b5e83c7947 73
tommyallen 7:d4b5e83c7947 74 //-------------------------------------------------------------------------------------------------
tommyallen 7:d4b5e83c7947 75 // Serial Interrupt
tommyallen 7:d4b5e83c7947 76 //-------------------------------------------------------------------------------------------------
tommyallen 7:d4b5e83c7947 77 void rxCallback(MODSERIAL_IRQ_INFO *q)
tommyallen 7:d4b5e83c7947 78 {
tommyallen 7:d4b5e83c7947 79 MODSERIAL *serial = q->serial;
tommyallen 8:41d1f9ab3be0 80 if ( serial->rxGetLastChar() == 'f') {
tommyallen 8:41d1f9ab3be0 81 imu.write_reg0(0x3d, 0x0c);
tommyallen 8:41d1f9ab3be0 82 RAW = false;
tommyallen 8:41d1f9ab3be0 83 FUS = true;
tommyallen 8:41d1f9ab3be0 84 pc.printf("\r\nSWITCHING TO FUSION DATA!\r\n");
tommyallen 8:41d1f9ab3be0 85 }
tommyallen 8:41d1f9ab3be0 86 if ( serial->rxGetLastChar() == 'r') {
tommyallen 8:41d1f9ab3be0 87 imu.write_reg0(0x3d, 0x07);
tommyallen 8:41d1f9ab3be0 88 RAW = true;
tommyallen 8:41d1f9ab3be0 89 FUS = false;
tommyallen 8:41d1f9ab3be0 90 pc.printf("\r\nSWITCHING TO RAW DATA!\r\n");
tommyallen 7:d4b5e83c7947 91 }
tommyallen 10:acbb851070c2 92 if ( serial->rxGetLastChar() == '*') {
el13ytg 12:c8019aca78d0 93 TimeOfStart = t.read_ms();
el13ytg 12:c8019aca78d0 94 TimeStamp = 0;
tommyallen 13:f5cccb8beac0 95 pc.printf("RESETTING TIME");
tommyallen 10:acbb851070c2 96 }
tommyallen 7:d4b5e83c7947 97 }
tommyallen 7:d4b5e83c7947 98
kenjiArai 0:31451519d283 99 //-------------------------------------------------------------------------------------------------
kenjiArai 0:31451519d283 100 // Control Program
kenjiArai 3:f5b5c4d795ce 101 //-------------------------------------------------------------------------------------------------
kenjiArai 4:6d1118089a36 102 // Calibration
kenjiArai 4:6d1118089a36 103 // Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data
tommyallen 7:d4b5e83c7947 104 void bno055_calbration(void)
tommyallen 7:d4b5e83c7947 105 {
kenjiArai 4:6d1118089a36 106 uint8_t d;
tommyallen 7:d4b5e83c7947 107 pc.printf("------ Enter BNO055 Manual Calibration Mode ------\r\n");
kenjiArai 4:6d1118089a36 108 //---------- Gyroscope Caliblation ------------------------------------------------------------
kenjiArai 4:6d1118089a36 109 // (a) Place the device in a single stable position for a period of few seconds to allow the
kenjiArai 4:6d1118089a36 110 // gyroscope to calibrate
kenjiArai 4:6d1118089a36 111 pc.printf("Step1) Please wait few seconds\r\n");
kenjiArai 4:6d1118089a36 112 t.start();
tommyallen 7:d4b5e83c7947 113 while (t.read() < 10) {
kenjiArai 4:6d1118089a36 114 d = imu.read_calib_status();
tommyallen 6:0590c7ff8c34 115 pc.printf("calibrating = 0x%x target = 0x30(at least)\r\n", d);
tommyallen 7:d4b5e83c7947 116 if ((d & 0x30) == 0x30) {
kenjiArai 4:6d1118089a36 117 break;
kenjiArai 4:6d1118089a36 118 }
kenjiArai 4:6d1118089a36 119 wait(1.0);
kenjiArai 4:6d1118089a36 120 }
kenjiArai 4:6d1118089a36 121 pc.printf("-> Step1) is done\r\n\r\n");
kenjiArai 4:6d1118089a36 122 //---------- Magnetometer Caliblation ---------------------------------------------------------
kenjiArai 4:6d1118089a36 123 // (a) Make some random movements (for example: writing the number ‘8’ on air) until the
kenjiArai 4:6d1118089a36 124 // CALIB_STAT register indicates fully calibrated.
kenjiArai 4:6d1118089a36 125 // (b) It takes more calibration movements to get the magnetometer calibrated than in the
kenjiArai 4:6d1118089a36 126 // NDOF mode.
kenjiArai 4:6d1118089a36 127 pc.printf("Step2) random moving (try to change the BNO055 axis)\r\n");
kenjiArai 4:6d1118089a36 128 t.start();
tommyallen 7:d4b5e83c7947 129 while (t.read() < 30) {
kenjiArai 4:6d1118089a36 130 d = imu.read_calib_status();
tommyallen 6:0590c7ff8c34 131 pc.printf("calibrating, = 0x%x target = 0x33(at least)\r\n", d);
tommyallen 7:d4b5e83c7947 132 if ((d & 0x03) == 0x03) {
kenjiArai 4:6d1118089a36 133 break;
kenjiArai 4:6d1118089a36 134 }
tommyallen 7:d4b5e83c7947 135 wait(1.0);
kenjiArai 4:6d1118089a36 136 }
kenjiArai 4:6d1118089a36 137 pc.printf("-> Step2) is done\r\n\r\n");
kenjiArai 4:6d1118089a36 138 //---------- Magnetometer Caliblation ---------------------------------------------------------
kenjiArai 4:6d1118089a36 139 // a) Place the device in 6 different stable positions for a period of few seconds
kenjiArai 4:6d1118089a36 140 // to allow the accelerometer to calibrate.
kenjiArai 4:6d1118089a36 141 // b) Make sure that there is slow movement between 2 stable positions
kenjiArai 4:6d1118089a36 142 // The 6 stable positions could be in any direction, but make sure that the device is
kenjiArai 4:6d1118089a36 143 // lying at least once perpendicular to the x, y and z axis.
kenjiArai 4:6d1118089a36 144 pc.printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!");
kenjiArai 4:6d1118089a36 145 pc.printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n");
kenjiArai 4:6d1118089a36 146 pc.printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,");
kenjiArai 4:6d1118089a36 147 pc.printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n");
kenjiArai 4:6d1118089a36 148 pc.printf(" If you will give up, hit any key.\r\n", d);
kenjiArai 4:6d1118089a36 149 t.stop();
tommyallen 7:d4b5e83c7947 150 while (true) {
kenjiArai 4:6d1118089a36 151 d = imu.read_calib_status();
kenjiArai 4:6d1118089a36 152 imu.get_gravity(&gravity);
tommyallen 6:0590c7ff8c34 153 pc.printf("calibrating = 0x%x target = 0xff ACC:X %4.1f, Y %4.1f, Z %4.1f\r\n",
tommyallen 7:d4b5e83c7947 154 d, gravity.x, gravity.y, gravity.z);
tommyallen 7:d4b5e83c7947 155 if (d == 0xff) {
tommyallen 7:d4b5e83c7947 156 break;
tommyallen 7:d4b5e83c7947 157 }
tommyallen 7:d4b5e83c7947 158 if (pc.readable()) {
tommyallen 7:d4b5e83c7947 159 break;
tommyallen 7:d4b5e83c7947 160 }
kenjiArai 4:6d1118089a36 161 wait(1.0);
kenjiArai 4:6d1118089a36 162 }
tommyallen 7:d4b5e83c7947 163 if (imu.read_calib_status() == 0xff) {
kenjiArai 4:6d1118089a36 164 pc.printf("-> All of Calibration steps are done successfully!\r\n\r\n");
kenjiArai 4:6d1118089a36 165 } else {
kenjiArai 4:6d1118089a36 166 pc.printf("-> Calibration steps are suspended!\r\n\r\n");
kenjiArai 4:6d1118089a36 167 }
kenjiArai 4:6d1118089a36 168 t.stop();
kenjiArai 4:6d1118089a36 169 }
tommyallen 7:d4b5e83c7947 170
tommyallen 8:41d1f9ab3be0 171 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 172 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 173 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 174 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 175 // Main Program
tommyallen 8:41d1f9ab3be0 176 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 177 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 178 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 179 //-------------------------------------------------------------------------------------------------
tommyallen 7:d4b5e83c7947 180 int main()
tommyallen 7:d4b5e83c7947 181 {
tommyallen 13:f5cccb8beac0 182 pc.baud(921600);
tommyallen 7:d4b5e83c7947 183 pc.attach(&rxCallback, MODSERIAL::RxIrq);
tommyallen 13:f5cccb8beac0 184 pc.printf("BNO055 Hello World/\r\n\r\n");
kenjiArai 4:6d1118089a36 185 imu.set_mounting_position(MT_P6);
kenjiArai 4:6d1118089a36 186 pwr_onoff = 0;
tommyallen 13:f5cccb8beac0 187 pc.printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!/\r\n");
kenjiArai 4:6d1118089a36 188 char c = pc.getc();
tommyallen 7:d4b5e83c7947 189 pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
kenjiArai 4:6d1118089a36 190 // Is BNO055 avairable?
tommyallen 7:d4b5e83c7947 191 if (imu.chip_ready() == 0) {
kenjiArai 4:6d1118089a36 192 do {
tommyallen 13:f5cccb8beac0 193 pc.printf("Bosch BNO055 is NOT avirable!!\r\n Reset/\r\n");
kenjiArai 4:6d1118089a36 194 pwr_onoff = 1; // Power off
kenjiArai 4:6d1118089a36 195 wait(0.1);
kenjiArai 4:6d1118089a36 196 pwr_onoff = 0; // Power on
kenjiArai 4:6d1118089a36 197 wait(0.02);
kenjiArai 4:6d1118089a36 198 } while(imu.reset());
kenjiArai 4:6d1118089a36 199 }
tommyallen 13:f5cccb8beac0 200 pc.printf("Bosch BNO055 is available now!!/\r\n");
tommyallen 13:f5cccb8beac0 201 pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x/\r\n",
tommyallen 7:d4b5e83c7947 202 imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN));
kenjiArai 4:6d1118089a36 203 imu.read_id_inf(&bno055_id_inf);
tommyallen 13:f5cccb8beac0 204 pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x,/",
tommyallen 7:d4b5e83c7947 205 bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
tommyallen 13:f5cccb8beac0 206 pc.printf("SW REV:0x%04x, BL REV:0x%02x/\r\n",
tommyallen 7:d4b5e83c7947 207 bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
tommyallen 13:f5cccb8beac0 208 pc.printf("If you would like to calibrate the BNO055, please hit 'y' (No: any other key)/\r\n");
kenjiArai 4:6d1118089a36 209 c = pc.getc();
tommyallen 7:d4b5e83c7947 210 if (c == 'y') {
kenjiArai 4:6d1118089a36 211 bno055_calbration();
kenjiArai 4:6d1118089a36 212 }
el13ytg 12:c8019aca78d0 213
tommyallen 13:f5cccb8beac0 214 pc.printf("If you would like to Synchronize the System, please hit 'y' (No: any other key)\r\n/");
tommyallen 13:f5cccb8beac0 215 c = pc.getc();
tommyallen 13:f5cccb8beac0 216 if (c == 'y') {
tommyallen 13:f5cccb8beac0 217 sync=0;
tommyallen 13:f5cccb8beac0 218 pc.printf("Synchronise/");
tommyallen 13:f5cccb8beac0 219 wait(0.5);
tommyallen 13:f5cccb8beac0 220 }
tommyallen 13:f5cccb8beac0 221
el13ytg 12:c8019aca78d0 222 while (sync == 0) {
el13ytg 12:c8019aca78d0 223 pc.printf("HELLO/");
el13ytg 12:c8019aca78d0 224 if (pc.readable()) {
el13ytg 12:c8019aca78d0 225 if (pc.getc() == 72) {
el13ytg 12:c8019aca78d0 226 while (sync == 0) {
el13ytg 12:c8019aca78d0 227 if (pc.readable()) {
el13ytg 12:c8019aca78d0 228 if (pc.getc() == 73) {
el13ytg 12:c8019aca78d0 229 sync = 1;
el13ytg 12:c8019aca78d0 230 }
el13ytg 12:c8019aca78d0 231 }
el13ytg 12:c8019aca78d0 232 }
el13ytg 12:c8019aca78d0 233 }
el13ytg 12:c8019aca78d0 234 }
el13ytg 12:c8019aca78d0 235 }
el13ytg 12:c8019aca78d0 236 pc.printf("START/");
kenjiArai 4:6d1118089a36 237 t.start();
tommyallen 8:41d1f9ab3be0 238 imu.write_reg0(0x3d, 0x07); // Default FUS
el13ytg 12:c8019aca78d0 239
tommyallen 8:41d1f9ab3be0 240 while (1) {
el13ytg 12:c8019aca78d0 241 while (RAW == true) {
tommyallen 9:7be40e42e578 242 Time = t.read_ms();
el13ytg 12:c8019aca78d0 243 if (Time > TimeStamp + 5) {
tommyallen 9:7be40e42e578 244 pc.printf("RAW,xLSB:,%d,xMSB:,%d,yLSB:%d,yMSB:,%d,zLSB:,%d,zMSB:,%d,"
tommyallen 9:7be40e42e578 245 ,imu.read_reg0(0x08),imu.read_reg0(0x09)
tommyallen 9:7be40e42e578 246 ,imu.read_reg0(0x0A),imu.read_reg0(0x0B)
tommyallen 9:7be40e42e578 247 ,imu.read_reg0(0x0C),imu.read_reg0(0x0D));
el13ytg 12:c8019aca78d0 248 pc.printf("Time:,%d,/",t.read_ms());
el13ytg 12:c8019aca78d0 249 TimeStamp = Time;
tommyallen 9:7be40e42e578 250 }
tommyallen 8:41d1f9ab3be0 251 }
tommyallen 8:41d1f9ab3be0 252 while (RAW == false) {
tommyallen 13:f5cccb8beac0 253 //Time = t.read_ms();
tommyallen 13:f5cccb8beac0 254 //TimeNow = + t.read_ms() - TimeOfStart;
tommyallen 13:f5cccb8beac0 255 if (t.read_ms() - TimeOfStart > TimeStamp + 5) {
tommyallen 13:f5cccb8beac0 256
tommyallen 13:f5cccb8beac0 257 //TimeStart = t.read_ms() - TimeOfStart;
el13ytg 12:c8019aca78d0 258
tommyallen 9:7be40e42e578 259 imu.get_linear_accel(&linear_acc);
el13ytg 12:c8019aca78d0 260 imu.get_Euler_Angles(&euler_angles);
el13ytg 12:c8019aca78d0 261 imu.get_quaternion(&quaternion);
el13ytg 12:c8019aca78d0 262 imu.get_gravity(&gravity);
el13ytg 12:c8019aca78d0 263 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 13:f5cccb8beac0 264 ,t.read_ms() - TimeOfStart,linear_acc.x ,linear_acc.y ,linear_acc.z,
el13ytg 12:c8019aca78d0 265 euler_angles.h, euler_angles.r, euler_angles.p,
el13ytg 12:c8019aca78d0 266 quaternion.w, quaternion.x, quaternion.y, quaternion.z,
tommyallen 13:f5cccb8beac0 267 gravity.x, gravity.y, gravity.z);
el13ytg 12:c8019aca78d0 268
tommyallen 13:f5cccb8beac0 269
tommyallen 13:f5cccb8beac0 270 //pc.printf("TimeStart:,%d,/",TimeStart);
tommyallen 13:f5cccb8beac0 271 //pc.printf("Timend:,%d,/",+ t.read_ms() - TimeOfStart);
tommyallen 13:f5cccb8beac0 272 TimeStamp = t.read_ms()- TimeOfStart;
tommyallen 9:7be40e42e578 273 }
el13ytg 12:c8019aca78d0 274
el13ytg 12:c8019aca78d0 275 /*if (Time > TimeStamp2 + 999) {
el13ytg 12:c8019aca78d0 276 if (direction == false) {
el13ytg 12:c8019aca78d0 277 PWM.pulsewidth_us(2400);
el13ytg 12:c8019aca78d0 278 //pc.printf("DIR, UP,");
el13ytg 12:c8019aca78d0 279 //pc.printf("Time:,%d,/",t.read_ms());
el13ytg 12:c8019aca78d0 280 direction = true;
el13ytg 12:c8019aca78d0 281 TimeStamp2 = Time;
el13ytg 12:c8019aca78d0 282 }
el13ytg 12:c8019aca78d0 283
el13ytg 12:c8019aca78d0 284 else if (direction == true) {
el13ytg 12:c8019aca78d0 285 PWM.pulsewidth_us(600);
el13ytg 12:c8019aca78d0 286 //pc.printf("DIR, DOWN,");
el13ytg 12:c8019aca78d0 287 //pc.printf("Time:,%d,/",t.read_ms());
el13ytg 12:c8019aca78d0 288 direction = false;
el13ytg 12:c8019aca78d0 289 TimeStamp2 = Time;
el13ytg 12:c8019aca78d0 290 }
el13ytg 12:c8019aca78d0 291 }
el13ytg 12:c8019aca78d0 292 */
tommyallen 8:41d1f9ab3be0 293 }
tommyallen 8:41d1f9ab3be0 294 }
kenjiArai 4:6d1118089a36 295 }
kenjiArai 4:6d1118089a36 296
tommyallen 8:41d1f9ab3be0 297 // imu.get_Euler_Angles(&euler_angles);
tommyallen 8:41d1f9ab3be0 298 // pc.printf("X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
tommyallen 8:41d1f9ab3be0 299 // euler_angles.h, euler_angles.r, euler_angles.p);
tommyallen 8:41d1f9ab3be0 300 // imu.get_linear_accel(&linear_acc);
tommyallen 8:41d1f9ab3be0 301 // pc.printf("Acc:,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
tommyallen 8:41d1f9ab3be0 302 // linear_acc.x, linear_acc.y, linear_acc.z);
tommyallen 8:41d1f9ab3be0 303 //
tommyallen 8:41d1f9ab3be0 304 // imu.get_gravity(&gravity);
tommyallen 8:41d1f9ab3be0 305 // pc.printf("Gravity,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
tommyallen 8:41d1f9ab3be0 306 // gravity.x, gravity.y, gravity.z);
tommyallen 8:41d1f9ab3be0 307 //
tommyallen 8:41d1f9ab3be0 308 // pc.printf("Time:,%d,\r\n",t.read_ms());
tommyallen 6:0590c7ff8c34 309 // imu.get_Euler_Angles(&euler_angles);
tommyallen 6:0590c7ff8c34 310 // pc.printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,",
tommyallen 6:0590c7ff8c34 311 // euler_angles.h, euler_angles.r, euler_angles.p);
tommyallen 6:0590c7ff8c34 312 // imu.get_quaternion(&quaternion);
tommyallen 6:0590c7ff8c34 313 // pc.printf("[Q],W,%d,X,%d,Y,%d,Z,%d,",
tommyallen 6:0590c7ff8c34 314 // quaternion.w, quaternion.x, quaternion.y, quaternion.z);
tommyallen 6:0590c7ff8c34 315 // imu.get_linear_accel(&linear_acc);
tommyallen 6:0590c7ff8c34 316 // pc.printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
tommyallen 6:0590c7ff8c34 317 // linear_acc.x, linear_acc.y, linear_acc.z);
tommyallen 6:0590c7ff8c34 318 // imu.get_gravity(&gravity);
tommyallen 6:0590c7ff8c34 319 // pc.printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
tommyallen 6:0590c7ff8c34 320 // gravity.x, gravity.y, gravity.z);
tommyallen 6:0590c7ff8c34 321 // imu.get_chip_temperature(&chip_temp);
tommyallen 6:0590c7ff8c34 322 // pc.printf("[T],%+d,%+d,",
tommyallen 6:0590c7ff8c34 323 // chip_temp.acc_chip, chip_temp.gyr_chip);
tommyallen 6:0590c7ff8c34 324 // pc.printf("[S],0x%x,[M],%d\r\n",
tommyallen 6:0590c7ff8c34 325 // imu.read_calib_status(), t.read_ms());
tommyallen 7:d4b5e83c7947 326 // pc.printf("Words\r\n");
kenjiArai 0:31451519d283 327