MME ELEC3810 / Mbed 2 deprecated Shared-1BNO055

fsdf

Dependencies:   BNO055_fusion MODSERIAL mbed

Fork of Shared-1BNO055 by Thomas Allen

main.cpp@16:6c95604185fc, 2016-02-22 (annotated)

Committer:
tommyallen
Date:
Mon Feb 22 18:55:35 2016 +0000
Revision:
16:6c95604185fc
Parent:
15:304c0fcf6575 
22/02/16;

Who changed what in which revision?

UserRevisionLine numberNew 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 }