Khoa Bui Anh / Mbed 2 deprecated done

drum team

Dependencies:   BLE_API X_NUCLEO_IDB0XA1 mbed

Fork of BLE_HeartRate_IDB0XA1 by ST

main.cpp@22:65f63e2d06bd, 2016-10-30 (annotated)

Committer:
fxanhkhoa
Date:
Sun Oct 30 00:13:37 2016 +0000
Revision:
22:65f63e2d06bd
Parent:
21:0e7c08f5386f 
ok

Who changed what in which revision?

UserRevisionLine numberNew contents of line
screamer 0:eb7f02ad28a7 1 /* mbed Microcontroller Library
screamer 0:eb7f02ad28a7 2 * Copyright (c) 2006-2015 ARM Limited
screamer 0:eb7f02ad28a7 3 *
screamer 0:eb7f02ad28a7 4 * Licensed under the Apache License, Version 2.0 (the "License");
screamer 0:eb7f02ad28a7 5 * you may not use this file except in compliance with the License.
screamer 0:eb7f02ad28a7 6 * You may obtain a copy of the License at
screamer 0:eb7f02ad28a7 7 *
screamer 0:eb7f02ad28a7 8 * http://www.apache.org/licenses/LICENSE-2.0
screamer 0:eb7f02ad28a7 9 *
screamer 0:eb7f02ad28a7 10 * Unless required by applicable law or agreed to in writing, software
screamer 0:eb7f02ad28a7 11 * distributed under the License is distributed on an "AS IS" BASIS,
screamer 0:eb7f02ad28a7 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
screamer 0:eb7f02ad28a7 13 * See the License for the specific language governing permissions and
screamer 0:eb7f02ad28a7 14 * limitations under the License.
screamer 0:eb7f02ad28a7 15 */
screamer 0:eb7f02ad28a7 16
screamer 0:eb7f02ad28a7 17 #include "mbed.h"
fxanhkhoa 22:65f63e2d06bd 18 #include "MPU6050.h"
screamer 0:eb7f02ad28a7 19 #include "ble/BLE.h"
screamer 0:eb7f02ad28a7 20 #include "ble/services/HeartRateService.h"
fxanhkhoa 22:65f63e2d06bd 21 #define ratio 1
fxanhkhoa 22:65f63e2d06bd 22
fxanhkhoa 22:65f63e2d06bd 23 int i= 0,j = 0;
fxanhkhoa 22:65f63e2d06bd 24 float sum = 0;
fxanhkhoa 22:65f63e2d06bd 25 uint32_t sumCount = 0;
fxanhkhoa 22:65f63e2d06bd 26 volatile uint8_t hrmCounter;
fxanhkhoa 22:65f63e2d06bd 27
fxanhkhoa 22:65f63e2d06bd 28 float central1[3], central2[3];
fxanhkhoa 22:65f63e2d06bd 29 float drum1_min[3],drum2_min[3],drum3_min[3],drum4_min[3],drum5_min[3],drum6_min[3],drum7_min[3],drum8_min[3],drum9_min[3],drum10_min[3];
fxanhkhoa 22:65f63e2d06bd 30 float drum1_max[3],drum2_max[3],drum3_max[3],drum4_max[3],drum5_max[3],drum6_max[3],drum7_max[3],drum8_max[3],drum9_max[3],drum10_max[3];
fxanhkhoa 22:65f63e2d06bd 31 int flag = 0;
fxanhkhoa 22:65f63e2d06bd 32 int stt1 = 0, stt2 = 0;
fxanhkhoa 22:65f63e2d06bd 33 int drum1_stt1 = 0,drum2_stt1 = 0,drum3_stt1 = 0,drum4_stt1 = 0,drum5_stt1 = 0;
fxanhkhoa 22:65f63e2d06bd 34 int drum1_stt2 = 0,drum2_stt2 = 0,drum3_stt2 = 0,drum4_stt2 = 0,drum5_stt2 = 0;
fxanhkhoa 22:65f63e2d06bd 35
fxanhkhoa 22:65f63e2d06bd 36
fxanhkhoa 22:65f63e2d06bd 37 InterruptIn mybutton(USER_BUTTON);
fxanhkhoa 22:65f63e2d06bd 38
fxanhkhoa 22:65f63e2d06bd 39 MPU6050 mpu6050;
fxanhkhoa 22:65f63e2d06bd 40
fxanhkhoa 22:65f63e2d06bd 41 MPU6050 mpu6050_2;
fxanhkhoa 22:65f63e2d06bd 42
fxanhkhoa 22:65f63e2d06bd 43 Timer t;
fxanhkhoa 22:65f63e2d06bd 44
fxanhkhoa 22:65f63e2d06bd 45 Serial pc(USBTX, USBRX); // tx, rx
fxanhkhoa 22:65f63e2d06bd 46
fxanhkhoa 22:65f63e2d06bd 47 // VCC, SCE, RST, D/C, MOSI,S CLK, LED
fxanhkhoa 22:65f63e2d06bd 48 //N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
fxanhkhoa 22:65f63e2d06bd 49
fxanhkhoa 22:65f63e2d06bd 50 void get();
screamer 0:eb7f02ad28a7 51
apalmieri 13:227a0149b677 52 DigitalOut led1(LED1, 1);
screamer 0:eb7f02ad28a7 53
fxanhkhoa 22:65f63e2d06bd 54 const static char DEVICE_NAME[] = "IDB";
apalmieri 13:227a0149b677 55 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
apalmieri 13:227a0149b677 56
screamer 0:eb7f02ad28a7 57 static volatile bool triggerSensorPolling = false;
screamer 0:eb7f02ad28a7 58
apalmieri 2:bc0c0d442a24 59 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
screamer 0:eb7f02ad28a7 60 {
apalmieri 13:227a0149b677 61 (void)params;
apalmieri 13:227a0149b677 62 BLE::Instance().gap().startAdvertising(); // restart advertising
screamer 0:eb7f02ad28a7 63 }
screamer 0:eb7f02ad28a7 64
screamer 0:eb7f02ad28a7 65 void periodicCallback(void)
screamer 0:eb7f02ad28a7 66 {
screamer 0:eb7f02ad28a7 67 led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
screamer 0:eb7f02ad28a7 68 /* Note that the periodicCallback() executes in interrupt context, so it is safer to do
screamer 0:eb7f02ad28a7 69 * heavy-weight sensor polling from the main thread. */
screamer 0:eb7f02ad28a7 70 triggerSensorPolling = true;
screamer 0:eb7f02ad28a7 71 }
screamer 0:eb7f02ad28a7 72
apalmieri 13:227a0149b677 73 void onBleInitError(BLE &ble, ble_error_t error)
apalmieri 13:227a0149b677 74 {
apalmieri 13:227a0149b677 75 (void)ble;
apalmieri 13:227a0149b677 76 (void)error;
apalmieri 13:227a0149b677 77 /* Initialization error handling should go here */
apalmieri 13:227a0149b677 78 }
apalmieri 13:227a0149b677 79
apalmieri 13:227a0149b677 80 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
screamer 0:eb7f02ad28a7 81 {
apalmieri 13:227a0149b677 82 BLE& ble = params->ble;
apalmieri 13:227a0149b677 83 ble_error_t error = params->error;
screamer 0:eb7f02ad28a7 84
apalmieri 13:227a0149b677 85 if (error != BLE_ERROR_NONE) {
apalmieri 13:227a0149b677 86 onBleInitError(ble, error);
apalmieri 13:227a0149b677 87 return;
apalmieri 13:227a0149b677 88 }
apalmieri 13:227a0149b677 89
apalmieri 13:227a0149b677 90 if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
apalmieri 13:227a0149b677 91 return;
apalmieri 13:227a0149b677 92 }
apalmieri 13:227a0149b677 93
screamer 0:eb7f02ad28a7 94 ble.gap().onDisconnection(disconnectionCallback);
screamer 0:eb7f02ad28a7 95
screamer 0:eb7f02ad28a7 96 /* Setup primary service. */
fxanhkhoa 22:65f63e2d06bd 97 uint8_t hrmCounter = 'A'; // init HRM to 60bps
screamer 0:eb7f02ad28a7 98 HeartRateService hrService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
screamer 0:eb7f02ad28a7 99
screamer 0:eb7f02ad28a7 100 /* Setup advertising. */
screamer 0:eb7f02ad28a7 101 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
screamer 0:eb7f02ad28a7 102 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
screamer 0:eb7f02ad28a7 103 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
screamer 0:eb7f02ad28a7 104 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
screamer 0:eb7f02ad28a7 105 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
screamer 0:eb7f02ad28a7 106 ble.gap().setAdvertisingInterval(1000); /* 1000ms */
screamer 0:eb7f02ad28a7 107 ble.gap().startAdvertising();
fxanhkhoa 22:65f63e2d06bd 108
fxanhkhoa 22:65f63e2d06bd 109 pc.baud(9600);
screamer 0:eb7f02ad28a7 110
fxanhkhoa 22:65f63e2d06bd 111 //Set up I2C
fxanhkhoa 22:65f63e2d06bd 112 i2c.frequency(400000); // use fast (400 kHz) I2C
fxanhkhoa 22:65f63e2d06bd 113 i2c2.frequency(400000);
fxanhkhoa 22:65f63e2d06bd 114 t.start();
fxanhkhoa 22:65f63e2d06bd 115
fxanhkhoa 22:65f63e2d06bd 116 //lcd.init();
fxanhkhoa 22:65f63e2d06bd 117 //lcd.setBrightness(0.05);
fxanhkhoa 22:65f63e2d06bd 118
fxanhkhoa 22:65f63e2d06bd 119
fxanhkhoa 22:65f63e2d06bd 120 // Read the WHO_AM_I register, this is a good test of communication
fxanhkhoa 22:65f63e2d06bd 121 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
fxanhkhoa 22:65f63e2d06bd 122 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
fxanhkhoa 22:65f63e2d06bd 123
fxanhkhoa 22:65f63e2d06bd 124 if (whoami == 0x68) // WHO_AM_I should always be 0x68
fxanhkhoa 22:65f63e2d06bd 125 {
fxanhkhoa 22:65f63e2d06bd 126 pc.printf("MPU6050 is online...");
fxanhkhoa 22:65f63e2d06bd 127 wait(1);
fxanhkhoa 22:65f63e2d06bd 128 //lcd.clear();
fxanhkhoa 22:65f63e2d06bd 129 //lcd.printString("MPU6050 OK", 0, 0);
fxanhkhoa 22:65f63e2d06bd 130
fxanhkhoa 22:65f63e2d06bd 131
fxanhkhoa 22:65f63e2d06bd 132 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
fxanhkhoa 22:65f63e2d06bd 133 pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
fxanhkhoa 22:65f63e2d06bd 134 pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
fxanhkhoa 22:65f63e2d06bd 135 pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
fxanhkhoa 22:65f63e2d06bd 136 pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
fxanhkhoa 22:65f63e2d06bd 137 pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
fxanhkhoa 22:65f63e2d06bd 138 pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
fxanhkhoa 22:65f63e2d06bd 139 wait(1);
fxanhkhoa 22:65f63e2d06bd 140
fxanhkhoa 22:65f63e2d06bd 141 if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f)
fxanhkhoa 22:65f63e2d06bd 142 {
fxanhkhoa 22:65f63e2d06bd 143 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
fxanhkhoa 22:65f63e2d06bd 144 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
fxanhkhoa 22:65f63e2d06bd 145 mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
fxanhkhoa 22:65f63e2d06bd 146
fxanhkhoa 22:65f63e2d06bd 147 /*lcd.clear();
fxanhkhoa 22:65f63e2d06bd 148 lcd.printString("MPU6050", 0, 0);
fxanhkhoa 22:65f63e2d06bd 149 lcd.printString("pass self test", 0, 1);
fxanhkhoa 22:65f63e2d06bd 150 lcd.printString("initializing", 0, 2); */
fxanhkhoa 22:65f63e2d06bd 151 wait(2);
fxanhkhoa 22:65f63e2d06bd 152 }
fxanhkhoa 22:65f63e2d06bd 153 else
fxanhkhoa 22:65f63e2d06bd 154 {
fxanhkhoa 22:65f63e2d06bd 155 pc.printf("Device did not the pass self-test!\n\r");
fxanhkhoa 22:65f63e2d06bd 156
fxanhkhoa 22:65f63e2d06bd 157 /*lcd.clear();
fxanhkhoa 22:65f63e2d06bd 158 lcd.printString("MPU6050", 0, 0);
fxanhkhoa 22:65f63e2d06bd 159 lcd.printString("no pass", 0, 1);
fxanhkhoa 22:65f63e2d06bd 160 lcd.printString("self test", 0, 2);*/
fxanhkhoa 22:65f63e2d06bd 161 }
fxanhkhoa 22:65f63e2d06bd 162 }
fxanhkhoa 22:65f63e2d06bd 163 else
fxanhkhoa 22:65f63e2d06bd 164 {
fxanhkhoa 22:65f63e2d06bd 165 pc.printf("Could not connect to MPU6050: \n\r");
fxanhkhoa 22:65f63e2d06bd 166 pc.printf("%#x \n", whoami);
fxanhkhoa 22:65f63e2d06bd 167
fxanhkhoa 22:65f63e2d06bd 168 /*lcd.clear();
fxanhkhoa 22:65f63e2d06bd 169 lcd.printString("MPU6050", 0, 0);
fxanhkhoa 22:65f63e2d06bd 170 lcd.printString("no connection", 0, 1);
fxanhkhoa 22:65f63e2d06bd 171 lcd.printString("0x", 0, 2); lcd.setXYAddress(20, 2); lcd.printChar(whoami);*/
fxanhkhoa 22:65f63e2d06bd 172
fxanhkhoa 22:65f63e2d06bd 173 while(1) ; // Loop forever if communication doesn't happen
fxanhkhoa 22:65f63e2d06bd 174 }
fxanhkhoa 22:65f63e2d06bd 175
screamer 0:eb7f02ad28a7 176 // infinite loop
fxanhkhoa 22:65f63e2d06bd 177 while (1) {
screamer 0:eb7f02ad28a7 178 // check for trigger from periodicCallback()
screamer 0:eb7f02ad28a7 179 if (triggerSensorPolling && ble.getGapState().connected) {
screamer 0:eb7f02ad28a7 180 triggerSensorPolling = false;
screamer 0:eb7f02ad28a7 181
fxanhkhoa 22:65f63e2d06bd 182 // If data ready bit set, all data registers have new data
fxanhkhoa 22:65f63e2d06bd 183 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
fxanhkhoa 22:65f63e2d06bd 184 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
fxanhkhoa 22:65f63e2d06bd 185 mpu6050.getAres();
fxanhkhoa 22:65f63e2d06bd 186
fxanhkhoa 22:65f63e2d06bd 187 // Now we'll calculate the accleration value into actual g's
fxanhkhoa 22:65f63e2d06bd 188 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
fxanhkhoa 22:65f63e2d06bd 189 ay = (float)accelCount[1]*aRes - accelBias[1];
fxanhkhoa 22:65f63e2d06bd 190 az = (float)accelCount[2]*aRes - accelBias[2];
fxanhkhoa 22:65f63e2d06bd 191
fxanhkhoa 22:65f63e2d06bd 192 ax2 = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
fxanhkhoa 22:65f63e2d06bd 193 ay2 = (float)accelCount[1]*aRes - accelBias[1];
fxanhkhoa 22:65f63e2d06bd 194 az2 = (float)accelCount[2]*aRes - accelBias[2];
fxanhkhoa 22:65f63e2d06bd 195
fxanhkhoa 22:65f63e2d06bd 196 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
fxanhkhoa 22:65f63e2d06bd 197 mpu6050.getGres();
fxanhkhoa 22:65f63e2d06bd 198
fxanhkhoa 22:65f63e2d06bd 199 // Calculate the gyro value into actual degrees per second
fxanhkhoa 22:65f63e2d06bd 200 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
fxanhkhoa 22:65f63e2d06bd 201 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
fxanhkhoa 22:65f63e2d06bd 202 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
fxanhkhoa 22:65f63e2d06bd 203
fxanhkhoa 22:65f63e2d06bd 204 gx2 = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
fxanhkhoa 22:65f63e2d06bd 205 gy2 = (float)gyroCount[1]*gRes; // - gyroBias[1];
fxanhkhoa 22:65f63e2d06bd 206 gz2 = (float)gyroCount[2]*gRes; // - gyroBias[2];
fxanhkhoa 22:65f63e2d06bd 207
fxanhkhoa 22:65f63e2d06bd 208 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
fxanhkhoa 22:65f63e2d06bd 209 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
fxanhkhoa 22:65f63e2d06bd 210 }
fxanhkhoa 22:65f63e2d06bd 211
fxanhkhoa 22:65f63e2d06bd 212 Now = t.read_us();
fxanhkhoa 22:65f63e2d06bd 213 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
fxanhkhoa 22:65f63e2d06bd 214 lastUpdate = Now;
fxanhkhoa 22:65f63e2d06bd 215
fxanhkhoa 22:65f63e2d06bd 216 sum += deltat;
fxanhkhoa 22:65f63e2d06bd 217 sumCount++;
fxanhkhoa 22:65f63e2d06bd 218
fxanhkhoa 22:65f63e2d06bd 219 if(lastUpdate - firstUpdate > 10000000.0f) {
fxanhkhoa 22:65f63e2d06bd 220 beta = 0.04; // decrease filter gain after stabilized
fxanhkhoa 22:65f63e2d06bd 221 zeta = 0.015; // increasey bias drift gain after stabilized
fxanhkhoa 22:65f63e2d06bd 222 }
fxanhkhoa 22:65f63e2d06bd 223
fxanhkhoa 22:65f63e2d06bd 224 // Pass gyro rate as rad/s
fxanhkhoa 22:65f63e2d06bd 225 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
fxanhkhoa 22:65f63e2d06bd 226 mpu6050.MadgwickQuaternionUpdate(ax2, ay2, az2, gx2*PI/180.0f, gy2*PI/180.0f, gz2*PI/180.0f);
fxanhkhoa 22:65f63e2d06bd 227
fxanhkhoa 22:65f63e2d06bd 228 // Serial print and/or display at 0.5 s rate independent of data rates
fxanhkhoa 22:65f63e2d06bd 229 delt_t = t.read_ms() - count_mpu;
fxanhkhoa 22:65f63e2d06bd 230 //if (delt_t > 500) { // update LCD once per half-second independent of read rate
fxanhkhoa 22:65f63e2d06bd 231
fxanhkhoa 22:65f63e2d06bd 232 //pc.printf("ax = %f", 1000*ax);
fxanhkhoa 22:65f63e2d06bd 233 //pc.printf(" ay = %f", 1000*ay);
fxanhkhoa 22:65f63e2d06bd 234 //pc.printf(" az = %f mg\n\r", 1000*az);
fxanhkhoa 22:65f63e2d06bd 235
fxanhkhoa 22:65f63e2d06bd 236 //pc.printf("gx = %f", gx);
fxanhkhoa 22:65f63e2d06bd 237 //pc.printf(" gy = %f", gy);
fxanhkhoa 22:65f63e2d06bd 238 //pc.printf(" gz = %f deg/s\n\r", gz);
fxanhkhoa 22:65f63e2d06bd 239
fxanhkhoa 22:65f63e2d06bd 240 //pc.printf(" temperature = %f C\n\r", temperature);
fxanhkhoa 22:65f63e2d06bd 241
fxanhkhoa 22:65f63e2d06bd 242 //pc.printf("q0 = %f\n\r", q[0]);
fxanhkhoa 22:65f63e2d06bd 243 //pc.printf("q1 = %f\n\r", q[1]);
fxanhkhoa 22:65f63e2d06bd 244 //pc.printf("q2 = %f\n\r", q[2]);
fxanhkhoa 22:65f63e2d06bd 245 //pc.printf("q3 = %f\n\r", q[3]);
fxanhkhoa 22:65f63e2d06bd 246
fxanhkhoa 22:65f63e2d06bd 247 //lcd.clear();
fxanhkhoa 22:65f63e2d06bd 248 //lcd.printString("MPU6050", 0, 0);
fxanhkhoa 22:65f63e2d06bd 249 //lcd.printString("x y z", 0, 1);
fxanhkhoa 22:65f63e2d06bd 250 //lcd.setXYAddress(0, 2); lcd.printChar((char)(1000*ax));
fxanhkhoa 22:65f63e2d06bd 251 //lcd.setXYAddress(20, 2); lcd.printChar((char)(1000*ay));
fxanhkhoa 22:65f63e2d06bd 252 //lcd.setXYAddress(40, 2); lcd.printChar((char)(1000*az)); lcd.printString("mg", 66, 2);
fxanhkhoa 22:65f63e2d06bd 253
fxanhkhoa 22:65f63e2d06bd 254
fxanhkhoa 22:65f63e2d06bd 255 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
fxanhkhoa 22:65f63e2d06bd 256 // In this coordinate system, the positive z-axis is down toward Earth.
fxanhkhoa 22:65f63e2d06bd 257 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
fxanhkhoa 22:65f63e2d06bd 258 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
fxanhkhoa 22:65f63e2d06bd 259 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
fxanhkhoa 22:65f63e2d06bd 260 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
fxanhkhoa 22:65f63e2d06bd 261 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
fxanhkhoa 22:65f63e2d06bd 262 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
fxanhkhoa 22:65f63e2d06bd 263 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
fxanhkhoa 22:65f63e2d06bd 264 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
fxanhkhoa 22:65f63e2d06bd 265 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
fxanhkhoa 22:65f63e2d06bd 266 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
fxanhkhoa 22:65f63e2d06bd 267 pitch *= 180.0f / PI;
fxanhkhoa 22:65f63e2d06bd 268 yaw *= 180.0f / PI;
fxanhkhoa 22:65f63e2d06bd 269 roll *= 180.0f / PI;
fxanhkhoa 22:65f63e2d06bd 270
fxanhkhoa 22:65f63e2d06bd 271 yaw2 = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
fxanhkhoa 22:65f63e2d06bd 272 pitch2 = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
fxanhkhoa 22:65f63e2d06bd 273 roll2 = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
fxanhkhoa 22:65f63e2d06bd 274 pitch2 *= 180.0f / PI;
fxanhkhoa 22:65f63e2d06bd 275 yaw2 *= 180.0f / PI;
fxanhkhoa 22:65f63e2d06bd 276 roll2 *= 180.0f / PI;
fxanhkhoa 22:65f63e2d06bd 277
fxanhkhoa 22:65f63e2d06bd 278 // pc.printf("Yaw, Pitch, Roll: \n\r");
fxanhkhoa 22:65f63e2d06bd 279 // pc.printf("%f", yaw);
fxanhkhoa 22:65f63e2d06bd 280 // pc.printf(", ");
fxanhkhoa 22:65f63e2d06bd 281 // pc.printf("%f", pitch);
fxanhkhoa 22:65f63e2d06bd 282 // pc.printf(", ");
fxanhkhoa 22:65f63e2d06bd 283 // pc.printf("%f\n\r", roll);
fxanhkhoa 22:65f63e2d06bd 284 // pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r");
screamer 0:eb7f02ad28a7 285
fxanhkhoa 22:65f63e2d06bd 286 //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
fxanhkhoa 22:65f63e2d06bd 287 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
fxanhkhoa 22:65f63e2d06bd 288
fxanhkhoa 22:65f63e2d06bd 289 myled= !myled;
fxanhkhoa 22:65f63e2d06bd 290 count_mpu = t.read_ms();
fxanhkhoa 22:65f63e2d06bd 291 sum = 0;
fxanhkhoa 22:65f63e2d06bd 292 sumCount = 0;
fxanhkhoa 22:65f63e2d06bd 293 if ((yaw < 1) && (yaw > 0) && (flag == 0))
fxanhkhoa 22:65f63e2d06bd 294 {
fxanhkhoa 22:65f63e2d06bd 295
fxanhkhoa 22:65f63e2d06bd 296 central1[0] = yaw;
fxanhkhoa 22:65f63e2d06bd 297 central1[1] = pitch;
fxanhkhoa 22:65f63e2d06bd 298 central1[2] = roll;
fxanhkhoa 22:65f63e2d06bd 299 central2[0] = yaw2;
fxanhkhoa 22:65f63e2d06bd 300 central2[1] = pitch2;
fxanhkhoa 22:65f63e2d06bd 301 central2[2] = roll2;
fxanhkhoa 22:65f63e2d06bd 302
screamer 0:eb7f02ad28a7 303
fxanhkhoa 22:65f63e2d06bd 304 pc.printf("central x y z : %f %f %f \r\n", central1[0],central1[1],central1[2]);
fxanhkhoa 22:65f63e2d06bd 305 flag = 1;
fxanhkhoa 22:65f63e2d06bd 306 }
fxanhkhoa 22:65f63e2d06bd 307 //if (i == 2000) i = 0;
fxanhkhoa 22:65f63e2d06bd 308 if (flag == 1)
fxanhkhoa 22:65f63e2d06bd 309 {
fxanhkhoa 22:65f63e2d06bd 310 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
fxanhkhoa 22:65f63e2d06bd 311 //hrService.updateHeartRate((uint8_t)yaw);
fxanhkhoa 22:65f63e2d06bd 312 //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", drum1_min[0], drum1_max[0], drum1_min[2]);
fxanhkhoa 22:65f63e2d06bd 313 switch (stt1)
fxanhkhoa 22:65f63e2d06bd 314 {
fxanhkhoa 22:65f63e2d06bd 315 case 0:
fxanhkhoa 22:65f63e2d06bd 316 pc.printf("%d",stt1);
fxanhkhoa 22:65f63e2d06bd 317 if (/*((yaw > drum1_min[0]) ||*/ (yaw < drum1_max[0]) /*&& (pitch > drum1_min[1]) && (pitch < drum1_max[1]) && (roll > drum1_max[2]) && (roll < drum1_max[2])*/) stt1 = 1;
fxanhkhoa 22:65f63e2d06bd 318 else if (/*(yaw > drum2_min[0]) ||*/ (yaw < drum2_max[0])/* && (pitch > drum2_min[1]) && (pitch < drum2_max[1]) && (roll > drum2_max[2]) && (roll < drum2_max[2])*/) stt1 = 2;
fxanhkhoa 22:65f63e2d06bd 319 else if (/*(yaw > drum3_min[0]) ||*/ (yaw < drum3_max[0])/* && (pitch > drum3_min[1]) && (pitch < drum3_max[1]) && (roll > drum3_max[2]) && (roll < drum3_max[2])*/) stt1 = 3;
fxanhkhoa 22:65f63e2d06bd 320 hrService.updateHeartRate((uint8_t)96);
fxanhkhoa 22:65f63e2d06bd 321 break;
fxanhkhoa 22:65f63e2d06bd 322 case 1:
fxanhkhoa 22:65f63e2d06bd 323 pc.printf("%d",stt1);
fxanhkhoa 22:65f63e2d06bd 324 if (drum1_stt1 == 0)
fxanhkhoa 22:65f63e2d06bd 325 {
fxanhkhoa 22:65f63e2d06bd 326 pc.printf("drum 1_1\r\n");
fxanhkhoa 22:65f63e2d06bd 327 drum1_stt1 = 1;
fxanhkhoa 22:65f63e2d06bd 328 hrService.updateHeartRate((uint8_t)69);
fxanhkhoa 22:65f63e2d06bd 329 }
fxanhkhoa 22:65f63e2d06bd 330 else if ((yaw > drum1_max[0]) || (pitch > drum1_max[1]) /*&& (roll > drum1_max[2])*/) {
fxanhkhoa 22:65f63e2d06bd 331 stt1 = 0 ;
fxanhkhoa 22:65f63e2d06bd 332 drum1_stt1 = 0;
fxanhkhoa 22:65f63e2d06bd 333 pc.printf("up\r\n");
fxanhkhoa 22:65f63e2d06bd 334 }
fxanhkhoa 22:65f63e2d06bd 335 break;
fxanhkhoa 22:65f63e2d06bd 336 case 2:
fxanhkhoa 22:65f63e2d06bd 337 pc.printf("%d",stt1);
fxanhkhoa 22:65f63e2d06bd 338 if (drum2_stt1 == 0)
fxanhkhoa 22:65f63e2d06bd 339 {
fxanhkhoa 22:65f63e2d06bd 340 pc.printf("drum 2_2\r\n");
fxanhkhoa 22:65f63e2d06bd 341 drum1_stt1 = 1;
fxanhkhoa 22:65f63e2d06bd 342 hrService.updateHeartRate((uint8_t)88);
fxanhkhoa 22:65f63e2d06bd 343 }
fxanhkhoa 22:65f63e2d06bd 344 else if ((yaw > drum2_max[0]) && (pitch > drum2_max[1]) /*&& (roll > drum2_max[2])*/) {
fxanhkhoa 22:65f63e2d06bd 345 stt1 = 0 ;
fxanhkhoa 22:65f63e2d06bd 346 drum1_stt1 = 0;
fxanhkhoa 22:65f63e2d06bd 347 pc.printf("up\r\n");
fxanhkhoa 22:65f63e2d06bd 348 }
fxanhkhoa 22:65f63e2d06bd 349 break;
fxanhkhoa 22:65f63e2d06bd 350 case 3:
fxanhkhoa 22:65f63e2d06bd 351 pc.printf("%d",stt1);
fxanhkhoa 22:65f63e2d06bd 352 if (drum3_stt1 == 0)
fxanhkhoa 22:65f63e2d06bd 353 {
fxanhkhoa 22:65f63e2d06bd 354 pc.printf("drum 3_3\r\n");
fxanhkhoa 22:65f63e2d06bd 355 drum3_stt1 = 1;
fxanhkhoa 22:65f63e2d06bd 356 hrService.updateHeartRate((uint8_t)3);
fxanhkhoa 22:65f63e2d06bd 357 }
fxanhkhoa 22:65f63e2d06bd 358 else if ((yaw > drum3_max[0]) && (pitch > drum3_max[1]) /*&& (roll > drum2_max[2])*/) {
fxanhkhoa 22:65f63e2d06bd 359 stt1 = 0 ;
fxanhkhoa 22:65f63e2d06bd 360 drum3_stt1 = 0;
fxanhkhoa 22:65f63e2d06bd 361 pc.printf("up\r\n");
fxanhkhoa 22:65f63e2d06bd 362 }
fxanhkhoa 22:65f63e2d06bd 363 break;
fxanhkhoa 22:65f63e2d06bd 364 case 4:
fxanhkhoa 22:65f63e2d06bd 365 pc.printf("%d",stt1);
fxanhkhoa 22:65f63e2d06bd 366 if (drum4_stt1 == 0)
fxanhkhoa 22:65f63e2d06bd 367 {
fxanhkhoa 22:65f63e2d06bd 368 pc.printf("drum 4_4\r\n");
fxanhkhoa 22:65f63e2d06bd 369 drum4_stt1 = 1;
fxanhkhoa 22:65f63e2d06bd 370 hrService.updateHeartRate((uint8_t)4);
fxanhkhoa 22:65f63e2d06bd 371 }
fxanhkhoa 22:65f63e2d06bd 372 else if ((yaw > drum4_max[0]) && (pitch > drum4_max[1]) /*&& (roll > drum2_max[2])*/) {
fxanhkhoa 22:65f63e2d06bd 373 stt1 = 0 ;
fxanhkhoa 22:65f63e2d06bd 374 drum4_stt1 = 0;
fxanhkhoa 22:65f63e2d06bd 375 pc.printf("up\r\n");
fxanhkhoa 22:65f63e2d06bd 376 }
fxanhkhoa 22:65f63e2d06bd 377 break;
fxanhkhoa 22:65f63e2d06bd 378 case 5:
fxanhkhoa 22:65f63e2d06bd 379 pc.printf("%d",stt1);
fxanhkhoa 22:65f63e2d06bd 380 if (drum5_stt1 == 0)
fxanhkhoa 22:65f63e2d06bd 381 {
fxanhkhoa 22:65f63e2d06bd 382 pc.printf("drum 4_4\r\n");
fxanhkhoa 22:65f63e2d06bd 383 drum5_stt1 = 1;
fxanhkhoa 22:65f63e2d06bd 384 hrService.updateHeartRate((uint8_t)5);
fxanhkhoa 22:65f63e2d06bd 385 }
fxanhkhoa 22:65f63e2d06bd 386 else if ((yaw > drum5_max[0]) && (pitch > drum5_max[1]) /*&& (roll > drum2_max[2])*/) {
fxanhkhoa 22:65f63e2d06bd 387 stt1 = 0 ;
fxanhkhoa 22:65f63e2d06bd 388 drum5_stt1 = 0;
fxanhkhoa 22:65f63e2d06bd 389 pc.printf("up\r\n");
fxanhkhoa 22:65f63e2d06bd 390 }
fxanhkhoa 22:65f63e2d06bd 391 break;
fxanhkhoa 22:65f63e2d06bd 392 };
fxanhkhoa 22:65f63e2d06bd 393 }
fxanhkhoa 22:65f63e2d06bd 394 }
fxanhkhoa 22:65f63e2d06bd 395 //}
fxanhkhoa 22:65f63e2d06bd 396 else {
screamer 0:eb7f02ad28a7 397 ble.waitForEvent(); // low power wait for event
screamer 0:eb7f02ad28a7 398 }
screamer 0:eb7f02ad28a7 399 }
screamer 0:eb7f02ad28a7 400 }
apalmieri 13:227a0149b677 401
apalmieri 13:227a0149b677 402 int main(void)
apalmieri 13:227a0149b677 403 {
apalmieri 13:227a0149b677 404 Ticker ticker;
fxanhkhoa 22:65f63e2d06bd 405 ticker.attach(periodicCallback, 0.01); // blink LED every second
fxanhkhoa 22:65f63e2d06bd 406 mybutton.fall(get);
fxanhkhoa 22:65f63e2d06bd 407
apalmieri 13:227a0149b677 408
apalmieri 13:227a0149b677 409 BLE::Instance().init(bleInitComplete);
apalmieri 13:227a0149b677 410 }
apalmieri 14:f715c13eb84f 411
fxanhkhoa 22:65f63e2d06bd 412 void get()
fxanhkhoa 22:65f63e2d06bd 413 {
fxanhkhoa 22:65f63e2d06bd 414 j++;
fxanhkhoa 22:65f63e2d06bd 415 if (j == 1){
fxanhkhoa 22:65f63e2d06bd 416 drum1_min[0] = yaw - ratio;
fxanhkhoa 22:65f63e2d06bd 417 drum1_min[1] = pitch - ratio;
fxanhkhoa 22:65f63e2d06bd 418 drum1_min[2] = roll - ratio;
fxanhkhoa 22:65f63e2d06bd 419
fxanhkhoa 22:65f63e2d06bd 420 drum1_max[0] = yaw + ratio;
fxanhkhoa 22:65f63e2d06bd 421 drum1_max[1] = pitch + ratio;
fxanhkhoa 22:65f63e2d06bd 422 drum1_max[2] = roll + ratio;
fxanhkhoa 22:65f63e2d06bd 423 }
fxanhkhoa 22:65f63e2d06bd 424 else if (j == 2){
fxanhkhoa 22:65f63e2d06bd 425 drum2_min[0] = yaw - ratio;
fxanhkhoa 22:65f63e2d06bd 426 drum2_min[1] = pitch - ratio;
fxanhkhoa 22:65f63e2d06bd 427 drum2_min[2] = roll - ratio;
fxanhkhoa 22:65f63e2d06bd 428
fxanhkhoa 22:65f63e2d06bd 429 drum2_max[0] = yaw + ratio;
fxanhkhoa 22:65f63e2d06bd 430 drum2_max[1] = pitch + ratio;
fxanhkhoa 22:65f63e2d06bd 431 drum2_max[2] = roll + ratio;
fxanhkhoa 22:65f63e2d06bd 432 }
fxanhkhoa 22:65f63e2d06bd 433 else if (j == 3){
fxanhkhoa 22:65f63e2d06bd 434 drum3_min[0] = yaw - ratio;
fxanhkhoa 22:65f63e2d06bd 435 drum3_min[1] = pitch - ratio;
fxanhkhoa 22:65f63e2d06bd 436 drum3_min[2] = roll - ratio;
fxanhkhoa 22:65f63e2d06bd 437
fxanhkhoa 22:65f63e2d06bd 438 drum3_max[0] = yaw + ratio;
fxanhkhoa 22:65f63e2d06bd 439 drum3_max[1] = pitch + ratio;
fxanhkhoa 22:65f63e2d06bd 440 drum3_max[2] = roll + ratio;
fxanhkhoa 22:65f63e2d06bd 441 }
fxanhkhoa 22:65f63e2d06bd 442 else if (j == 4){
fxanhkhoa 22:65f63e2d06bd 443 drum4_min[0] = yaw - ratio;
fxanhkhoa 22:65f63e2d06bd 444 drum4_min[1] = pitch - ratio;
fxanhkhoa 22:65f63e2d06bd 445 drum4_min[2] = roll - ratio;
fxanhkhoa 22:65f63e2d06bd 446
fxanhkhoa 22:65f63e2d06bd 447 drum4_max[0] = yaw + ratio;
fxanhkhoa 22:65f63e2d06bd 448 drum4_max[1] = pitch + ratio;
fxanhkhoa 22:65f63e2d06bd 449 drum4_max[2] = roll + ratio;
fxanhkhoa 22:65f63e2d06bd 450 }
fxanhkhoa 22:65f63e2d06bd 451 else if (j == 5){
fxanhkhoa 22:65f63e2d06bd 452 drum5_min[0] = yaw - ratio;
fxanhkhoa 22:65f63e2d06bd 453 drum5_min[1] = pitch - ratio;
fxanhkhoa 22:65f63e2d06bd 454 drum5_min[2] = roll - ratio;
fxanhkhoa 22:65f63e2d06bd 455
fxanhkhoa 22:65f63e2d06bd 456 drum5_max[0] = yaw + ratio;
fxanhkhoa 22:65f63e2d06bd 457 drum5_max[1] = pitch + ratio;
fxanhkhoa 22:65f63e2d06bd 458 drum5_max[2] = roll + ratio;
fxanhkhoa 22:65f63e2d06bd 459 }
fxanhkhoa 22:65f63e2d06bd 460 else if (j == 6){
fxanhkhoa 22:65f63e2d06bd 461 drum6_min[0] = yaw - ratio;
fxanhkhoa 22:65f63e2d06bd 462 drum6_min[1] = pitch - ratio;
fxanhkhoa 22:65f63e2d06bd 463 drum6_min[2] = roll - ratio;
fxanhkhoa 22:65f63e2d06bd 464
fxanhkhoa 22:65f63e2d06bd 465 drum6_max[0] = yaw + ratio;
fxanhkhoa 22:65f63e2d06bd 466 drum6_max[1] = pitch + ratio;
fxanhkhoa 22:65f63e2d06bd 467 drum6_max[2] = roll + ratio;
fxanhkhoa 22:65f63e2d06bd 468 }
fxanhkhoa 22:65f63e2d06bd 469 else if (j == 7){
fxanhkhoa 22:65f63e2d06bd 470 drum7_min[0] = yaw - ratio;
fxanhkhoa 22:65f63e2d06bd 471 drum7_min[1] = pitch - ratio;
fxanhkhoa 22:65f63e2d06bd 472 drum7_min[2] = roll - ratio;
fxanhkhoa 22:65f63e2d06bd 473
fxanhkhoa 22:65f63e2d06bd 474 drum7_max[0] = yaw + ratio;
fxanhkhoa 22:65f63e2d06bd 475 drum7_max[1] = pitch + ratio;
fxanhkhoa 22:65f63e2d06bd 476 drum7_max[2] = roll + ratio;
fxanhkhoa 22:65f63e2d06bd 477 }
fxanhkhoa 22:65f63e2d06bd 478 else if (j == 8){
fxanhkhoa 22:65f63e2d06bd 479 drum8_min[0] = yaw - ratio;
fxanhkhoa 22:65f63e2d06bd 480 drum8_min[1] = pitch - ratio;
fxanhkhoa 22:65f63e2d06bd 481 drum8_min[2] = roll - ratio;
fxanhkhoa 22:65f63e2d06bd 482
fxanhkhoa 22:65f63e2d06bd 483 drum8_max[0] = yaw + ratio;
fxanhkhoa 22:65f63e2d06bd 484 drum8_max[1] = pitch + ratio;
fxanhkhoa 22:65f63e2d06bd 485 drum8_max[2] = roll + ratio;
fxanhkhoa 22:65f63e2d06bd 486 }
fxanhkhoa 22:65f63e2d06bd 487 else if (j == 9){
fxanhkhoa 22:65f63e2d06bd 488 drum9_min[0] = yaw - ratio;
fxanhkhoa 22:65f63e2d06bd 489 drum9_min[1] = pitch - ratio;
fxanhkhoa 22:65f63e2d06bd 490 drum9_min[2] = roll - ratio;
fxanhkhoa 22:65f63e2d06bd 491
fxanhkhoa 22:65f63e2d06bd 492 drum9_max[0] = yaw + ratio;
fxanhkhoa 22:65f63e2d06bd 493 drum9_max[1] = pitch + ratio;
fxanhkhoa 22:65f63e2d06bd 494 drum9_max[2] = roll + ratio;
fxanhkhoa 22:65f63e2d06bd 495 }
fxanhkhoa 22:65f63e2d06bd 496 else if (j == 10){
fxanhkhoa 22:65f63e2d06bd 497 drum10_min[0] = yaw - ratio;
fxanhkhoa 22:65f63e2d06bd 498 drum10_min[1] = pitch - ratio;
fxanhkhoa 22:65f63e2d06bd 499 drum10_min[2] = roll - ratio;
fxanhkhoa 22:65f63e2d06bd 500
fxanhkhoa 22:65f63e2d06bd 501 drum10_max[0] = yaw + ratio;
fxanhkhoa 22:65f63e2d06bd 502 drum10_max[1] = pitch + ratio;
fxanhkhoa 22:65f63e2d06bd 503 drum10_max[2] = roll + ratio;
fxanhkhoa 22:65f63e2d06bd 504 }
fxanhkhoa 22:65f63e2d06bd 505 if (j == 10) j = 0;
fxanhkhoa 22:65f63e2d06bd 506 pc.printf("x,y,z: %f %f %f \r\n",yaw,pitch,roll);
fxanhkhoa 22:65f63e2d06bd 507 }
fxanhkhoa 22:65f63e2d06bd 508