Khoa Bui Anh / Mbed 2 deprecated WAND_PROJECT

Wireless auto note device

Dependencies:   BLE_API invisdrum X_NUCLEO_IDB0XA1 kalman mbed

main.cpp@0:ffd0caf3db9f, 2016-11-22 (annotated)

Committer:
fxanhkhoa
Date:
Tue Nov 22 02:57:33 2016 +0000
Revision:
0:ffd0caf3db9f
WAND PROJECT

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fxanhkhoa 0:ffd0caf3db9f 1 /* mbed Microcontroller Library
fxanhkhoa 0:ffd0caf3db9f 2 * Copyright (c) 2006-2015 ARM Limited
fxanhkhoa 0:ffd0caf3db9f 3 *
fxanhkhoa 0:ffd0caf3db9f 4 * Licensed under the Apache License, Version 2.0 (the "License");
fxanhkhoa 0:ffd0caf3db9f 5 * you may not use this file except in compliance with the License.
fxanhkhoa 0:ffd0caf3db9f 6 * You may obtain a copy of the License at
fxanhkhoa 0:ffd0caf3db9f 7 *
fxanhkhoa 0:ffd0caf3db9f 8 * http://www.apache.org/licenses/LICENSE-2.0
fxanhkhoa 0:ffd0caf3db9f 9 *
fxanhkhoa 0:ffd0caf3db9f 10 * Unless required by applicable law or agreed to in writing, software
fxanhkhoa 0:ffd0caf3db9f 11 * distributed under the License is distributed on an "AS IS" BASIS,
fxanhkhoa 0:ffd0caf3db9f 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
fxanhkhoa 0:ffd0caf3db9f 13 * See the License for the specific language governing permissions and
fxanhkhoa 0:ffd0caf3db9f 14 * limitations under the License.
fxanhkhoa 0:ffd0caf3db9f 15 */
fxanhkhoa 0:ffd0caf3db9f 16
fxanhkhoa 0:ffd0caf3db9f 17 #include "mbed.h"
fxanhkhoa 0:ffd0caf3db9f 18 #include "MPU6050.h"
fxanhkhoa 0:ffd0caf3db9f 19 #include "MPU60501.h"
fxanhkhoa 0:ffd0caf3db9f 20 #include "HMC5883L.h"
fxanhkhoa 0:ffd0caf3db9f 21 #include "ble/BLE.h"
fxanhkhoa 0:ffd0caf3db9f 22 #include "ble/services/HeartRateService.h"
fxanhkhoa 0:ffd0caf3db9f 23 #include "kalman.c"
fxanhkhoa 0:ffd0caf3db9f 24 #include <math.h>
fxanhkhoa 0:ffd0caf3db9f 25
fxanhkhoa 0:ffd0caf3db9f 26 #define ratio 5
fxanhkhoa 0:ffd0caf3db9f 27 #define ratioy 9
fxanhkhoa 0:ffd0caf3db9f 28 #define pitch_ratio 6
fxanhkhoa 0:ffd0caf3db9f 29 #define PI 3.1415926535897932384626433832795
fxanhkhoa 0:ffd0caf3db9f 30 #define Rad2Dree 57.295779513082320876798154814105
fxanhkhoa 0:ffd0caf3db9f 31
fxanhkhoa 0:ffd0caf3db9f 32 #define wait_time 0.02
fxanhkhoa 0:ffd0caf3db9f 33
fxanhkhoa 0:ffd0caf3db9f 34 int i= 0,j = 0;
fxanhkhoa 0:ffd0caf3db9f 35 float sum = 0;
fxanhkhoa 0:ffd0caf3db9f 36 uint32_t sumCount = 0;
fxanhkhoa 0:ffd0caf3db9f 37 volatile uint8_t hrmCounter;
fxanhkhoa 0:ffd0caf3db9f 38
fxanhkhoa 0:ffd0caf3db9f 39 float central1[3], central2[3];
fxanhkhoa 0:ffd0caf3db9f 40 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 0:ffd0caf3db9f 41 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 0:ffd0caf3db9f 42 int flag = 0;
fxanhkhoa 0:ffd0caf3db9f 43 int stt1 = 0, stt2 = 0;
fxanhkhoa 0:ffd0caf3db9f 44 int drum1_stt1 = 0,drum2_stt1 = 0,drum3_stt1 = 0,drum4_stt1 = 0,drum5_stt1 = 0;
fxanhkhoa 0:ffd0caf3db9f 45 int drum1_stt2 = 0,drum2_stt2 = 0,drum3_stt2 = 0,drum4_stt2 = 0,drum5_stt2 = 0;
fxanhkhoa 0:ffd0caf3db9f 46
fxanhkhoa 0:ffd0caf3db9f 47 float Acc[3];
fxanhkhoa 0:ffd0caf3db9f 48 float Gyro[3];
fxanhkhoa 0:ffd0caf3db9f 49 float angle[3];
fxanhkhoa 0:ffd0caf3db9f 50 int Mag[3];
fxanhkhoa 0:ffd0caf3db9f 51 float R,R2;
fxanhkhoa 0:ffd0caf3db9f 52 unsigned long timer;
fxanhkhoa 0:ffd0caf3db9f 53 long loopStartTime;
fxanhkhoa 0:ffd0caf3db9f 54
fxanhkhoa 0:ffd0caf3db9f 55 Timer GlobalTime;
fxanhkhoa 0:ffd0caf3db9f 56 Timer ProgramTimer;
fxanhkhoa 0:ffd0caf3db9f 57 kalman filter_pitch;
fxanhkhoa 0:ffd0caf3db9f 58 kalman filter_roll;
fxanhkhoa 0:ffd0caf3db9f 59 kalman filter_yaw;
fxanhkhoa 0:ffd0caf3db9f 60
fxanhkhoa 0:ffd0caf3db9f 61
fxanhkhoa 0:ffd0caf3db9f 62 InterruptIn mybutton(USER_BUTTON);
fxanhkhoa 0:ffd0caf3db9f 63
fxanhkhoa 0:ffd0caf3db9f 64 MPU60501 mpu6050;
fxanhkhoa 0:ffd0caf3db9f 65
fxanhkhoa 0:ffd0caf3db9f 66 MPU60501 mpu6050_2;
fxanhkhoa 0:ffd0caf3db9f 67
fxanhkhoa 0:ffd0caf3db9f 68 MPU6050 ark(PB_9,PB_8);
fxanhkhoa 0:ffd0caf3db9f 69
fxanhkhoa 0:ffd0caf3db9f 70 HMC5883L compass(PB_9, PB_8);
fxanhkhoa 0:ffd0caf3db9f 71
fxanhkhoa 0:ffd0caf3db9f 72 Timer tmpu;
fxanhkhoa 0:ffd0caf3db9f 73
fxanhkhoa 0:ffd0caf3db9f 74 Timer gettime;
fxanhkhoa 0:ffd0caf3db9f 75
fxanhkhoa 0:ffd0caf3db9f 76 Serial pc(USBTX, USBRX); // tx, rx
fxanhkhoa 0:ffd0caf3db9f 77
fxanhkhoa 0:ffd0caf3db9f 78 // VCC, SCE, RST, D/C, MOSI,S CLK, LED
fxanhkhoa 0:ffd0caf3db9f 79 //N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
fxanhkhoa 0:ffd0caf3db9f 80
fxanhkhoa 0:ffd0caf3db9f 81 void get();
fxanhkhoa 0:ffd0caf3db9f 82
fxanhkhoa 0:ffd0caf3db9f 83 DigitalOut led1(LED1, 1);
fxanhkhoa 0:ffd0caf3db9f 84
fxanhkhoa 0:ffd0caf3db9f 85 const static char DEVICE_NAME[] = "Drum";
fxanhkhoa 0:ffd0caf3db9f 86 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
fxanhkhoa 0:ffd0caf3db9f 87
fxanhkhoa 0:ffd0caf3db9f 88 static volatile bool triggerSensorPolling = false;
fxanhkhoa 0:ffd0caf3db9f 89
fxanhkhoa 0:ffd0caf3db9f 90 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
fxanhkhoa 0:ffd0caf3db9f 91 {
fxanhkhoa 0:ffd0caf3db9f 92 (void)params;
fxanhkhoa 0:ffd0caf3db9f 93 BLE::Instance().gap().startAdvertising(); // restart advertising
fxanhkhoa 0:ffd0caf3db9f 94 }
fxanhkhoa 0:ffd0caf3db9f 95
fxanhkhoa 0:ffd0caf3db9f 96 void periodicCallback(void)
fxanhkhoa 0:ffd0caf3db9f 97 {
fxanhkhoa 0:ffd0caf3db9f 98 //led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
fxanhkhoa 0:ffd0caf3db9f 99 /* Note that the periodicCallback() executes in interrupt context, so it is safer to do
fxanhkhoa 0:ffd0caf3db9f 100 * heavy-weight sensor polling from the main thread. */
fxanhkhoa 0:ffd0caf3db9f 101 triggerSensorPolling = true;
fxanhkhoa 0:ffd0caf3db9f 102 }
fxanhkhoa 0:ffd0caf3db9f 103
fxanhkhoa 0:ffd0caf3db9f 104 void onBleInitError(BLE &ble, ble_error_t error)
fxanhkhoa 0:ffd0caf3db9f 105 {
fxanhkhoa 0:ffd0caf3db9f 106 (void)ble;
fxanhkhoa 0:ffd0caf3db9f 107 (void)error;
fxanhkhoa 0:ffd0caf3db9f 108 /* Initialization error handling should go here */
fxanhkhoa 0:ffd0caf3db9f 109 }
fxanhkhoa 0:ffd0caf3db9f 110
fxanhkhoa 0:ffd0caf3db9f 111 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
fxanhkhoa 0:ffd0caf3db9f 112 {
fxanhkhoa 0:ffd0caf3db9f 113 BLE& ble = params->ble;
fxanhkhoa 0:ffd0caf3db9f 114 ble_error_t error = params->error;
fxanhkhoa 0:ffd0caf3db9f 115
fxanhkhoa 0:ffd0caf3db9f 116 if (error != BLE_ERROR_NONE) {
fxanhkhoa 0:ffd0caf3db9f 117 onBleInitError(ble, error);
fxanhkhoa 0:ffd0caf3db9f 118 return;
fxanhkhoa 0:ffd0caf3db9f 119 }
fxanhkhoa 0:ffd0caf3db9f 120
fxanhkhoa 0:ffd0caf3db9f 121 if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
fxanhkhoa 0:ffd0caf3db9f 122 return;
fxanhkhoa 0:ffd0caf3db9f 123 }
fxanhkhoa 0:ffd0caf3db9f 124
fxanhkhoa 0:ffd0caf3db9f 125 ble.gap().onDisconnection(disconnectionCallback);
fxanhkhoa 0:ffd0caf3db9f 126
fxanhkhoa 0:ffd0caf3db9f 127 /* Setup primary service. */
fxanhkhoa 0:ffd0caf3db9f 128 uint8_t hrmCounter = 'A'; // init HRM to 60bps
fxanhkhoa 0:ffd0caf3db9f 129 HeartRateService hrService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
fxanhkhoa 0:ffd0caf3db9f 130
fxanhkhoa 0:ffd0caf3db9f 131 /* Setup advertising. */
fxanhkhoa 0:ffd0caf3db9f 132 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
fxanhkhoa 0:ffd0caf3db9f 133 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
fxanhkhoa 0:ffd0caf3db9f 134 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
fxanhkhoa 0:ffd0caf3db9f 135 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
fxanhkhoa 0:ffd0caf3db9f 136 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
fxanhkhoa 0:ffd0caf3db9f 137 ble.gap().setAdvertisingInterval(1000); /* 1000ms */
fxanhkhoa 0:ffd0caf3db9f 138 ble.gap().startAdvertising();
fxanhkhoa 0:ffd0caf3db9f 139
fxanhkhoa 0:ffd0caf3db9f 140 pc.baud(9600);
fxanhkhoa 0:ffd0caf3db9f 141
fxanhkhoa 0:ffd0caf3db9f 142 //Set up I2C
fxanhkhoa 0:ffd0caf3db9f 143 i2c.frequency(400000); // use fast (400 kHz) I2C
fxanhkhoa 0:ffd0caf3db9f 144 i2c2.frequency(400000);
fxanhkhoa 0:ffd0caf3db9f 145
fxanhkhoa 0:ffd0caf3db9f 146 compass.setDefault();
fxanhkhoa 0:ffd0caf3db9f 147 wait(0.1);
fxanhkhoa 0:ffd0caf3db9f 148
fxanhkhoa 0:ffd0caf3db9f 149
fxanhkhoa 0:ffd0caf3db9f 150 //lcd.init();
fxanhkhoa 0:ffd0caf3db9f 151 //lcd.setBrightness(0.05);
fxanhkhoa 0:ffd0caf3db9f 152
fxanhkhoa 0:ffd0caf3db9f 153
fxanhkhoa 0:ffd0caf3db9f 154 // Read the WHO_AM_I register, this is a good test of communication
fxanhkhoa 0:ffd0caf3db9f 155 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
fxanhkhoa 0:ffd0caf3db9f 156 //uint8_t whoami2 = mpu6050_2.readByte2(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
fxanhkhoa 0:ffd0caf3db9f 157 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
fxanhkhoa 0:ffd0caf3db9f 158
fxanhkhoa 0:ffd0caf3db9f 159 if ((whoami == 0x68) /*|| (whoami2 == 0x68)*/) // WHO_AM_I should always be 0x68
fxanhkhoa 0:ffd0caf3db9f 160 {
fxanhkhoa 0:ffd0caf3db9f 161 pc.printf("MPU6050 is online...");
fxanhkhoa 0:ffd0caf3db9f 162 wait(1);
fxanhkhoa 0:ffd0caf3db9f 163 //lcd.clear();
fxanhkhoa 0:ffd0caf3db9f 164 //lcd.printString("MPU6050 OK", 0, 0);
fxanhkhoa 0:ffd0caf3db9f 165
fxanhkhoa 0:ffd0caf3db9f 166
fxanhkhoa 0:ffd0caf3db9f 167 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
fxanhkhoa 0:ffd0caf3db9f 168 mpu6050.MPU6050SelfTest2(SelfTest2); // Start by performing self test and reporting values
fxanhkhoa 0:ffd0caf3db9f 169 pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
fxanhkhoa 0:ffd0caf3db9f 170 pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
fxanhkhoa 0:ffd0caf3db9f 171 pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
fxanhkhoa 0:ffd0caf3db9f 172 pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
fxanhkhoa 0:ffd0caf3db9f 173 pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
fxanhkhoa 0:ffd0caf3db9f 174 pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
fxanhkhoa 0:ffd0caf3db9f 175 wait(1);
fxanhkhoa 0:ffd0caf3db9f 176
fxanhkhoa 0:ffd0caf3db9f 177 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 0:ffd0caf3db9f 178 {
fxanhkhoa 0:ffd0caf3db9f 179 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
fxanhkhoa 0:ffd0caf3db9f 180 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
fxanhkhoa 0:ffd0caf3db9f 181 mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
fxanhkhoa 0:ffd0caf3db9f 182
fxanhkhoa 0:ffd0caf3db9f 183 /*lcd.clear();
fxanhkhoa 0:ffd0caf3db9f 184 lcd.printString("MPU6050", 0, 0);
fxanhkhoa 0:ffd0caf3db9f 185 lcd.printString("pass self test", 0, 1);
fxanhkhoa 0:ffd0caf3db9f 186 lcd.printString("initializing", 0, 2); */
fxanhkhoa 0:ffd0caf3db9f 187 wait(2);
fxanhkhoa 0:ffd0caf3db9f 188 }
fxanhkhoa 0:ffd0caf3db9f 189 else
fxanhkhoa 0:ffd0caf3db9f 190 {
fxanhkhoa 0:ffd0caf3db9f 191 pc.printf("Device did not the pass self-test!\n\r");
fxanhkhoa 0:ffd0caf3db9f 192
fxanhkhoa 0:ffd0caf3db9f 193 /*lcd.clear();
fxanhkhoa 0:ffd0caf3db9f 194 lcd.printString("MPU6050", 0, 0);
fxanhkhoa 0:ffd0caf3db9f 195 lcd.printString("no pass", 0, 1);
fxanhkhoa 0:ffd0caf3db9f 196 lcd.printString("self test", 0, 2);*/
fxanhkhoa 0:ffd0caf3db9f 197 }
fxanhkhoa 0:ffd0caf3db9f 198 }
fxanhkhoa 0:ffd0caf3db9f 199 else
fxanhkhoa 0:ffd0caf3db9f 200 {
fxanhkhoa 0:ffd0caf3db9f 201 pc.printf("Could not connect to MPU6050: \n\r");
fxanhkhoa 0:ffd0caf3db9f 202 pc.printf("%#x \n", whoami);
fxanhkhoa 0:ffd0caf3db9f 203
fxanhkhoa 0:ffd0caf3db9f 204 /*lcd.clear();
fxanhkhoa 0:ffd0caf3db9f 205 lcd.printString("MPU6050", 0, 0);
fxanhkhoa 0:ffd0caf3db9f 206 lcd.printString("no connection", 0, 1);
fxanhkhoa 0:ffd0caf3db9f 207 lcd.printString("0x", 0, 2); lcd.setXYAddress(20, 2); lcd.printChar(whoami);*/
fxanhkhoa 0:ffd0caf3db9f 208
fxanhkhoa 0:ffd0caf3db9f 209 while(1) ; // Loop forever if communication doesn't happen
fxanhkhoa 0:ffd0caf3db9f 210 }
fxanhkhoa 0:ffd0caf3db9f 211 i++;
fxanhkhoa 0:ffd0caf3db9f 212 tmpu.start();
fxanhkhoa 0:ffd0caf3db9f 213
fxanhkhoa 0:ffd0caf3db9f 214 kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
fxanhkhoa 0:ffd0caf3db9f 215 kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
fxanhkhoa 0:ffd0caf3db9f 216 kalman_init(&filter_yaw, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
fxanhkhoa 0:ffd0caf3db9f 217
fxanhkhoa 0:ffd0caf3db9f 218 ProgramTimer.start();
fxanhkhoa 0:ffd0caf3db9f 219 loopStartTime = ProgramTimer.read_us();
fxanhkhoa 0:ffd0caf3db9f 220 timer = loopStartTime;
fxanhkhoa 0:ffd0caf3db9f 221 // infinite loop
fxanhkhoa 0:ffd0caf3db9f 222 while (1) {
fxanhkhoa 0:ffd0caf3db9f 223 ark.getAccelero(Acc);
fxanhkhoa 0:ffd0caf3db9f 224 ark.getGyro(Gyro);
fxanhkhoa 0:ffd0caf3db9f 225 Mag[0] = compass.getMx();
fxanhkhoa 0:ffd0caf3db9f 226 Mag[1] = compass.getMy();
fxanhkhoa 0:ffd0caf3db9f 227 Mag[2] = compass.getMz();
fxanhkhoa 0:ffd0caf3db9f 228 R = sqrt(std::pow(Acc[0] , 2) + std::pow(Acc[1] , 2) + std::pow(Acc[2] , 2));
fxanhkhoa 0:ffd0caf3db9f 229 //R2 = sqrt(std::pow(Acc2[0] , 2) + std::pow(Acc2[1] , 2) + std::pow(Acc2[2] , 2));
fxanhkhoa 0:ffd0caf3db9f 230
fxanhkhoa 0:ffd0caf3db9f 231 kalman_predict(&filter_pitch, Gyro[0], (ProgramTimer.read_us() - timer));
fxanhkhoa 0:ffd0caf3db9f 232 kalman_update(&filter_pitch, acos(Acc[0]/R));
fxanhkhoa 0:ffd0caf3db9f 233 kalman_predict(&filter_roll, Gyro[1], (ProgramTimer.read_us() - timer));
fxanhkhoa 0:ffd0caf3db9f 234 kalman_update(&filter_roll, acos(Acc[1]/R));
fxanhkhoa 0:ffd0caf3db9f 235 kalman_predict(&filter_yaw, (float)Mag[1]/Mag[0], (ProgramTimer.read_us() - timer));
fxanhkhoa 0:ffd0caf3db9f 236 kalman_update(&filter_yaw, atan((float)Mag[1]/Mag[0]));
fxanhkhoa 0:ffd0caf3db9f 237
fxanhkhoa 0:ffd0caf3db9f 238 angle[0] = kalman_get_angle(&filter_pitch);
fxanhkhoa 0:ffd0caf3db9f 239 angle[1] = kalman_get_angle(&filter_roll);
fxanhkhoa 0:ffd0caf3db9f 240 angle[2] = kalman_get_angle(&filter_yaw);
fxanhkhoa 0:ffd0caf3db9f 241
fxanhkhoa 0:ffd0caf3db9f 242 //yaw = atan2( (-Mag[1]*cos(angle[0]*Rad2Dree) + Mag[2]*sin(angle[0]*Rad2Dree)) , (Mag[0]*cos(angle[0] * Rad2Dree) + Mag[1]*sin(angle[0]*Rad2Dree)*sin(angle[1]*Rad2Dree)+ Mag[2]*sin(angle[1]*Rad2Dree)*cos(angle[0]*Rad2Dree)) );
fxanhkhoa 0:ffd0caf3db9f 243 timer = ProgramTimer.read_us();
fxanhkhoa 0:ffd0caf3db9f 244
fxanhkhoa 0:ffd0caf3db9f 245 // If data ready bit set, all data registers have new data
fxanhkhoa 0:ffd0caf3db9f 246 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
fxanhkhoa 0:ffd0caf3db9f 247 mpu6050.readAccelData(accelCount);
fxanhkhoa 0:ffd0caf3db9f 248 }
fxanhkhoa 0:ffd0caf3db9f 249
fxanhkhoa 0:ffd0caf3db9f 250 yaw = angle[2] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 251 if ((yaw < 46) && (yaw > 44) && (flag == 0))
fxanhkhoa 0:ffd0caf3db9f 252 {
fxanhkhoa 0:ffd0caf3db9f 253
fxanhkhoa 0:ffd0caf3db9f 254 central1[0] = yaw;
fxanhkhoa 0:ffd0caf3db9f 255 central1[1] = pitch;
fxanhkhoa 0:ffd0caf3db9f 256 central1[2] = roll;
fxanhkhoa 0:ffd0caf3db9f 257 central2[0] = yaw2;
fxanhkhoa 0:ffd0caf3db9f 258 central2[1] = pitch2;
fxanhkhoa 0:ffd0caf3db9f 259 central2[2] = roll2;
fxanhkhoa 0:ffd0caf3db9f 260
fxanhkhoa 0:ffd0caf3db9f 261
fxanhkhoa 0:ffd0caf3db9f 262 pc.printf("central x y z : %f %f %f \r\n", central1[0],central1[1],central1[2]);
fxanhkhoa 0:ffd0caf3db9f 263 flag = 1;
fxanhkhoa 0:ffd0caf3db9f 264 }
fxanhkhoa 0:ffd0caf3db9f 265 if (i == 1000) i = 0;
fxanhkhoa 0:ffd0caf3db9f 266 if (flag == 1)
fxanhkhoa 0:ffd0caf3db9f 267 {
fxanhkhoa 0:ffd0caf3db9f 268 pitch = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 269 roll = angle[0] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 270 yaw = angle[2] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 271 //yaw = atan((float) Mag[1]/Mag[0])*Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 272 //yaw =atan2( (-Mag[1]*cos(roll) + Mag[2]*sin(roll) ) , (Mag[0]*cos(pitch) + Mag[1]*sin(pitch)*sin(roll)+ Mag[2]*sin(pitch)*cos(roll)) );
fxanhkhoa 0:ffd0caf3db9f 273 //pc.printf("Pitch, Roll, Yaw: %f %f %f %f \n\r", angle[1] * Rad2Dree, angle[0] * Rad2Dree,yaw, atan((float)Mag[1]/Mag[0]) * Rad2Dree);// pitch, roll, yaw
fxanhkhoa 0:ffd0caf3db9f 274 //pc.printf("%d %d %d \r\n",Mag[0],Mag[1],Mag[2]);
fxanhkhoa 0:ffd0caf3db9f 275 //pc.printf("Magx Magy Magz : %d %d %d \r\n",Mag[0],Mag[1],Mag[2]);
fxanhkhoa 0:ffd0caf3db9f 276 //wait(0.1);
fxanhkhoa 0:ffd0caf3db9f 277 //hrService.updateHeartRate((uint8_t)yaw);
fxanhkhoa 0:ffd0caf3db9f 278 //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", drum1_min[0], drum1_max[0], drum1_min[2]);
fxanhkhoa 0:ffd0caf3db9f 279 int step = 0, step2 = 0;
fxanhkhoa 0:ffd0caf3db9f 280 //while (triggerSensorPolling && ble.getGapState().connected == 1) {};
fxanhkhoa 0:ffd0caf3db9f 281 if (triggerSensorPolling && ble.getGapState().connected) {
fxanhkhoa 0:ffd0caf3db9f 282 triggerSensorPolling = false;
fxanhkhoa 0:ffd0caf3db9f 283 switch (stt1)
fxanhkhoa 0:ffd0caf3db9f 284 {
fxanhkhoa 0:ffd0caf3db9f 285 case 0:
fxanhkhoa 0:ffd0caf3db9f 286 //pc.printf("%d\n",stt1);
fxanhkhoa 0:ffd0caf3db9f 287 step = 0;
fxanhkhoa 0:ffd0caf3db9f 288 if ((yaw < drum2_max[0]) && (yaw > drum2_min[0]) && (pitch > drum2_max[1])) stt1 = 2;
fxanhkhoa 0:ffd0caf3db9f 289 else if ((yaw < drum1_max[0]) && (yaw > drum1_min[0]) && (pitch > drum1_max[1])) stt1 = 1;
fxanhkhoa 0:ffd0caf3db9f 290 else if ((yaw < drum3_max[0]) && (yaw > drum3_min[0]) && (pitch > drum3_max[1])) stt1 = 3;
fxanhkhoa 0:ffd0caf3db9f 291 else if ((yaw < drum4_max[0]) && (yaw > drum4_min[0]) && (pitch < drum4_max[1])) stt1 = 4;
fxanhkhoa 0:ffd0caf3db9f 292 else if ((yaw < drum5_max[0]) && (yaw > drum5_min[0]) && (pitch < drum5_max[1])) stt1 = 5;
fxanhkhoa 0:ffd0caf3db9f 293 //hrService.updateHeartRate((uint8_t)96);
fxanhkhoa 0:ffd0caf3db9f 294 break;
fxanhkhoa 0:ffd0caf3db9f 295 case 1:
fxanhkhoa 0:ffd0caf3db9f 296 //pc.printf("%d\n",stt1);
fxanhkhoa 0:ffd0caf3db9f 297 if ((drum1_stt1 == 0) && (yaw < drum1_max[0]) && (yaw > drum1_min[0]) && (pitch > drum1_max[1]))
fxanhkhoa 0:ffd0caf3db9f 298 {
fxanhkhoa 0:ffd0caf3db9f 299 //pc.printf("drum 1_1\r\n");
fxanhkhoa 0:ffd0caf3db9f 300 drum1_stt1 = 1;
fxanhkhoa 0:ffd0caf3db9f 301 hrService.updateHeartRate((uint8_t)1);
fxanhkhoa 0:ffd0caf3db9f 302 wait(wait_time);
fxanhkhoa 0:ffd0caf3db9f 303 }
fxanhkhoa 0:ffd0caf3db9f 304 //else if ((yaw > drum1_max[0] + 2)|| (yaw < drum1_min[0] - 2) || (pitch < (drum1_max[1] - pitch_ratio)) /*&& (roll > drum1_max[2])*/) {
fxanhkhoa 0:ffd0caf3db9f 305 else if (pitch < (drum1_max[1] - pitch_ratio)){
fxanhkhoa 0:ffd0caf3db9f 306 stt1 = 0 ;
fxanhkhoa 0:ffd0caf3db9f 307 drum1_stt1 = 0;
fxanhkhoa 0:ffd0caf3db9f 308 //pc.printf("up\r\n");
fxanhkhoa 0:ffd0caf3db9f 309 }
fxanhkhoa 0:ffd0caf3db9f 310 break;
fxanhkhoa 0:ffd0caf3db9f 311 case 2:
fxanhkhoa 0:ffd0caf3db9f 312 //pc.printf("%d\n",stt1);
fxanhkhoa 0:ffd0caf3db9f 313 if ((drum2_stt1 == 0) && (yaw < drum2_max[0]) && (yaw > drum2_min[0]) && (pitch > drum2_max[1]))
fxanhkhoa 0:ffd0caf3db9f 314 {
fxanhkhoa 0:ffd0caf3db9f 315 //pc.printf("drum 2_2\r\n");
fxanhkhoa 0:ffd0caf3db9f 316 drum2_stt1 = 1;
fxanhkhoa 0:ffd0caf3db9f 317 hrService.updateHeartRate((uint8_t)2);
fxanhkhoa 0:ffd0caf3db9f 318 wait(wait_time);
fxanhkhoa 0:ffd0caf3db9f 319 }
fxanhkhoa 0:ffd0caf3db9f 320 //else if ((yaw > drum2_max[0])|| (yaw < drum2_min[0]) || (pitch < (drum2_max[1] - pitch_ratio)) /*&& (roll > drum2_max[2])*/) {
fxanhkhoa 0:ffd0caf3db9f 321 else if (pitch < (drum2_max[1] - pitch_ratio)){
fxanhkhoa 0:ffd0caf3db9f 322 stt1 = 0 ;
fxanhkhoa 0:ffd0caf3db9f 323 drum2_stt1 = 0;
fxanhkhoa 0:ffd0caf3db9f 324 //pc.printf("up\r\n");
fxanhkhoa 0:ffd0caf3db9f 325 }
fxanhkhoa 0:ffd0caf3db9f 326 break;
fxanhkhoa 0:ffd0caf3db9f 327 case 3:
fxanhkhoa 0:ffd0caf3db9f 328 //pc.printf("%d\n",stt1);
fxanhkhoa 0:ffd0caf3db9f 329 if ((drum3_stt1 == 0) && (yaw < drum3_max[0]) && (yaw > drum3_min[0]) && (pitch > drum3_max[1]))
fxanhkhoa 0:ffd0caf3db9f 330 {
fxanhkhoa 0:ffd0caf3db9f 331 //pc.printf("drum 3_3\r\n");
fxanhkhoa 0:ffd0caf3db9f 332 drum3_stt1 = 1;
fxanhkhoa 0:ffd0caf3db9f 333 hrService.updateHeartRate((uint8_t)3);
fxanhkhoa 0:ffd0caf3db9f 334 wait(wait_time);
fxanhkhoa 0:ffd0caf3db9f 335 }
fxanhkhoa 0:ffd0caf3db9f 336 //else if ((yaw > drum3_max[0])|| (yaw < drum3_min[0]) || (pitch < (drum3_max[1] - pitch_ratio)) /*&& (roll > drum2_max[2])*/) {
fxanhkhoa 0:ffd0caf3db9f 337 else if (pitch < (drum3_max[1] - pitch_ratio)){
fxanhkhoa 0:ffd0caf3db9f 338 stt1 = 0 ;
fxanhkhoa 0:ffd0caf3db9f 339 drum3_stt1 = 0;
fxanhkhoa 0:ffd0caf3db9f 340 //pc.printf("up\r\n");
fxanhkhoa 0:ffd0caf3db9f 341 }
fxanhkhoa 0:ffd0caf3db9f 342 break;
fxanhkhoa 0:ffd0caf3db9f 343 case 4:
fxanhkhoa 0:ffd0caf3db9f 344 //pc.printf("%d\n",stt1);
fxanhkhoa 0:ffd0caf3db9f 345 if (drum4_stt1 == 0)
fxanhkhoa 0:ffd0caf3db9f 346 {
fxanhkhoa 0:ffd0caf3db9f 347 if ((pitch > drum4_max[1]) && (step == 0))
fxanhkhoa 0:ffd0caf3db9f 348 {
fxanhkhoa 0:ffd0caf3db9f 349 //pc.printf("drum 4_4\r\n");
fxanhkhoa 0:ffd0caf3db9f 350 drum4_stt1 = 1;
fxanhkhoa 0:ffd0caf3db9f 351 hrService.updateHeartRate((uint8_t)4);
fxanhkhoa 0:ffd0caf3db9f 352 step = 1;
fxanhkhoa 0:ffd0caf3db9f 353 wait(wait_time);
fxanhkhoa 0:ffd0caf3db9f 354 }
fxanhkhoa 0:ffd0caf3db9f 355 }
fxanhkhoa 0:ffd0caf3db9f 356 //else if ((yaw > drum4_max[0])|| (yaw < drum4_min[0]) || (pitch > (drum4_max[1] + pitch_ratio)) /*&& (roll > drum2_max[2])*/) {
fxanhkhoa 0:ffd0caf3db9f 357 else if (pitch > (drum4_max[1] + pitch_ratio)){
fxanhkhoa 0:ffd0caf3db9f 358 stt1 = 0 ;
fxanhkhoa 0:ffd0caf3db9f 359 drum4_stt1 = 0;
fxanhkhoa 0:ffd0caf3db9f 360 //pc.printf("up\r\n");
fxanhkhoa 0:ffd0caf3db9f 361 }
fxanhkhoa 0:ffd0caf3db9f 362 break;
fxanhkhoa 0:ffd0caf3db9f 363 case 5:
fxanhkhoa 0:ffd0caf3db9f 364 //pc.printf("%d\n",stt1);
fxanhkhoa 0:ffd0caf3db9f 365 if (drum5_stt1 == 0)
fxanhkhoa 0:ffd0caf3db9f 366 {
fxanhkhoa 0:ffd0caf3db9f 367 if ((pitch > drum5_max[1]) && (step == 0))
fxanhkhoa 0:ffd0caf3db9f 368 {
fxanhkhoa 0:ffd0caf3db9f 369 pc.printf("drum 5_5\r\n");
fxanhkhoa 0:ffd0caf3db9f 370 drum5_stt1 = 1;
fxanhkhoa 0:ffd0caf3db9f 371 hrService.updateHeartRate((uint8_t)5);
fxanhkhoa 0:ffd0caf3db9f 372 step = 1;
fxanhkhoa 0:ffd0caf3db9f 373 wait(wait_time);
fxanhkhoa 0:ffd0caf3db9f 374 }
fxanhkhoa 0:ffd0caf3db9f 375 }
fxanhkhoa 0:ffd0caf3db9f 376 //else if ((yaw > drum5_max[0])|| (yaw < drum5_min[0]) || (pitch > (drum5_max[1] + pitch_ratio)) /*&& (roll > drum2_max[2])*/) {
fxanhkhoa 0:ffd0caf3db9f 377 else if (pitch > (drum5_max[1] + pitch_ratio)){
fxanhkhoa 0:ffd0caf3db9f 378 stt1 = 0 ;
fxanhkhoa 0:ffd0caf3db9f 379 drum5_stt1 = 0;
fxanhkhoa 0:ffd0caf3db9f 380 //pc.printf("up\r\n");
fxanhkhoa 0:ffd0caf3db9f 381 }
fxanhkhoa 0:ffd0caf3db9f 382 break;
fxanhkhoa 0:ffd0caf3db9f 383 default:
fxanhkhoa 0:ffd0caf3db9f 384 break;
fxanhkhoa 0:ffd0caf3db9f 385 };
fxanhkhoa 0:ffd0caf3db9f 386 triggerSensorPolling = false;
fxanhkhoa 0:ffd0caf3db9f 387 switch (stt2){
fxanhkhoa 0:ffd0caf3db9f 388 case 0:
fxanhkhoa 0:ffd0caf3db9f 389 //hrService.updateHeartRate((uint8_t)69);
fxanhkhoa 0:ffd0caf3db9f 390 //pc.printf("%d\r\n",stt2);
fxanhkhoa 0:ffd0caf3db9f 391 step2 = 0;
fxanhkhoa 0:ffd0caf3db9f 392 /*
fxanhkhoa 0:ffd0caf3db9f 393 if ((yaw < drum6_max[0]) && (yaw > drum6_min[0]) && (pitch > drum6_max[1])) stt1 = 1;
fxanhkhoa 0:ffd0caf3db9f 394 else if ((yaw < drum7_max[0]) && (yaw > drum7_min[0]) && (pitch > drum7_max[1])) stt1 = 2;
fxanhkhoa 0:ffd0caf3db9f 395 else if ((yaw < drum8_max[0]) && (yaw > drum8_min[0]) && (pitch > drum8_max[1])) stt1 = 3;
fxanhkhoa 0:ffd0caf3db9f 396 else if ((yaw < drum9_max[0]) && (yaw > drum9_min[0]) && (pitch < drum9_max[1])) stt1 = 4;
fxanhkhoa 0:ffd0caf3db9f 397 else if ((yaw < drum10_max[0]) && (yaw > drum10_min[0]) && (pitch < drum10_max[1])) stt1 = 5;
fxanhkhoa 0:ffd0caf3db9f 398 */
fxanhkhoa 0:ffd0caf3db9f 399 //hrService.updateHeartRate((uint8_t)35);
fxanhkhoa 0:ffd0caf3db9f 400 break;
fxanhkhoa 0:ffd0caf3db9f 401 case 1:
fxanhkhoa 0:ffd0caf3db9f 402 //pc.printf("stt 2: %d ",stt2);
fxanhkhoa 0:ffd0caf3db9f 403 if ((drum1_stt2 == 0) && (yaw < drum6_max[0]) && (yaw > drum6_min[0]) && (pitch > drum6_max[1]))
fxanhkhoa 0:ffd0caf3db9f 404 {
fxanhkhoa 0:ffd0caf3db9f 405 pc.printf("drum 1_1\r\n");
fxanhkhoa 0:ffd0caf3db9f 406 drum1_stt2 = 1;
fxanhkhoa 0:ffd0caf3db9f 407 hrService.updateHeartRate((uint8_t)1);
fxanhkhoa 0:ffd0caf3db9f 408 wait(wait_time);
fxanhkhoa 0:ffd0caf3db9f 409 }
fxanhkhoa 0:ffd0caf3db9f 410 //else if ((yaw > drum1_max[0]) || (yaw < drum6_min[0]) || (pitch < drum6_max[1] - pitch_ratio) /*&& (roll > drum1_max[2])*/) {
fxanhkhoa 0:ffd0caf3db9f 411 else if (pitch < (drum6_max[1] - pitch_ratio)){
fxanhkhoa 0:ffd0caf3db9f 412 stt2 = 0 ;
fxanhkhoa 0:ffd0caf3db9f 413 drum1_stt2 = 0;
fxanhkhoa 0:ffd0caf3db9f 414 //pc.printf("up\r\n");
fxanhkhoa 0:ffd0caf3db9f 415 }
fxanhkhoa 0:ffd0caf3db9f 416 break;
fxanhkhoa 0:ffd0caf3db9f 417 case 2:
fxanhkhoa 0:ffd0caf3db9f 418 //pc.printf("stt 2:%d ",stt2);
fxanhkhoa 0:ffd0caf3db9f 419 if (drum2_stt2 == 0)
fxanhkhoa 0:ffd0caf3db9f 420 {
fxanhkhoa 0:ffd0caf3db9f 421 pc.printf("drum 2_2\r\n");
fxanhkhoa 0:ffd0caf3db9f 422 drum2_stt2 = 1;
fxanhkhoa 0:ffd0caf3db9f 423 hrService.updateHeartRate((uint8_t)2);
fxanhkhoa 0:ffd0caf3db9f 424 wait(wait_time);
fxanhkhoa 0:ffd0caf3db9f 425 }
fxanhkhoa 0:ffd0caf3db9f 426 //else if ((yaw > drum2_max[0])|| (yaw < drum7_min[0]) || (pitch < drum7_max[1] - pitch_ratio) /*&& (roll > drum2_max[2])*/) {
fxanhkhoa 0:ffd0caf3db9f 427 else if (pitch < (drum7_max[1] - pitch_ratio)){
fxanhkhoa 0:ffd0caf3db9f 428 stt2 = 0 ;
fxanhkhoa 0:ffd0caf3db9f 429 drum2_stt2 = 0;
fxanhkhoa 0:ffd0caf3db9f 430 //pc.printf("up\r\n");
fxanhkhoa 0:ffd0caf3db9f 431 }
fxanhkhoa 0:ffd0caf3db9f 432 break;
fxanhkhoa 0:ffd0caf3db9f 433 case 3:
fxanhkhoa 0:ffd0caf3db9f 434 //pc.printf("stt 2: %d ",stt2);
fxanhkhoa 0:ffd0caf3db9f 435 if (drum3_stt2 == 0)
fxanhkhoa 0:ffd0caf3db9f 436 {
fxanhkhoa 0:ffd0caf3db9f 437 pc.printf("drum 3_3\r\n");
fxanhkhoa 0:ffd0caf3db9f 438 drum3_stt2 = 1;
fxanhkhoa 0:ffd0caf3db9f 439 hrService.updateHeartRate((uint8_t)3);
fxanhkhoa 0:ffd0caf3db9f 440 wait(wait_time);
fxanhkhoa 0:ffd0caf3db9f 441 }
fxanhkhoa 0:ffd0caf3db9f 442 //else if ((yaw > drum8_max[0])|| (yaw < drum8_min[0]) || (pitch < drum8_max[1] - pitch_ratio) /*&& (roll > drum2_max[2])*/) {
fxanhkhoa 0:ffd0caf3db9f 443 else if (pitch > (drum8_max[1] + pitch_ratio)){
fxanhkhoa 0:ffd0caf3db9f 444 stt1 = 0 ;
fxanhkhoa 0:ffd0caf3db9f 445 drum3_stt2 = 0;
fxanhkhoa 0:ffd0caf3db9f 446 //pc.printf("up\r\n");
fxanhkhoa 0:ffd0caf3db9f 447 }
fxanhkhoa 0:ffd0caf3db9f 448 break;
fxanhkhoa 0:ffd0caf3db9f 449 case 4:
fxanhkhoa 0:ffd0caf3db9f 450 pc.printf("stt 2: %d ",stt2);
fxanhkhoa 0:ffd0caf3db9f 451 if (drum4_stt2 == 0)
fxanhkhoa 0:ffd0caf3db9f 452 {
fxanhkhoa 0:ffd0caf3db9f 453 if ((pitch > drum9_max[1]) && (step2 == 0) && (yaw < drum9_max[0]) && (yaw > drum9_min[0]) && (pitch < drum9_max[1]))
fxanhkhoa 0:ffd0caf3db9f 454 {
fxanhkhoa 0:ffd0caf3db9f 455 pc.printf("drum 4_4\r\n");
fxanhkhoa 0:ffd0caf3db9f 456 drum4_stt2 = 1;
fxanhkhoa 0:ffd0caf3db9f 457 hrService.updateHeartRate((uint8_t)4);
fxanhkhoa 0:ffd0caf3db9f 458 step2 = 1;
fxanhkhoa 0:ffd0caf3db9f 459 wait(wait_time);
fxanhkhoa 0:ffd0caf3db9f 460 }
fxanhkhoa 0:ffd0caf3db9f 461 }
fxanhkhoa 0:ffd0caf3db9f 462 //else if ((yaw > drum9_max[0])|| (yaw < drum9_min[0]) || (pitch > drum9_max[1] + pitch_ratio) /*&& (roll > drum2_max[2])*/) {
fxanhkhoa 0:ffd0caf3db9f 463 else if (pitch > (drum9_max[1] + pitch_ratio)){
fxanhkhoa 0:ffd0caf3db9f 464 stt2 = 0 ;
fxanhkhoa 0:ffd0caf3db9f 465 drum4_stt2 = 0;
fxanhkhoa 0:ffd0caf3db9f 466 pc.printf("up\r\n");
fxanhkhoa 0:ffd0caf3db9f 467 }
fxanhkhoa 0:ffd0caf3db9f 468 break;
fxanhkhoa 0:ffd0caf3db9f 469 case 5:
fxanhkhoa 0:ffd0caf3db9f 470 //pc.printf("stt 2: %d ",stt2);
fxanhkhoa 0:ffd0caf3db9f 471 if (drum5_stt2 == 0)
fxanhkhoa 0:ffd0caf3db9f 472 {
fxanhkhoa 0:ffd0caf3db9f 473 if ((pitch > drum10_max[1]) && (step2 == 0) && (yaw < drum10_max[0]) && (yaw > drum10_min[0]))
fxanhkhoa 0:ffd0caf3db9f 474 {
fxanhkhoa 0:ffd0caf3db9f 475 pc.printf("drum 5_5\r\n");
fxanhkhoa 0:ffd0caf3db9f 476 drum5_stt2 = 1;
fxanhkhoa 0:ffd0caf3db9f 477 hrService.updateHeartRate((uint8_t)5);
fxanhkhoa 0:ffd0caf3db9f 478 step2 = 1;
fxanhkhoa 0:ffd0caf3db9f 479 wait(wait_time);
fxanhkhoa 0:ffd0caf3db9f 480 }
fxanhkhoa 0:ffd0caf3db9f 481 }
fxanhkhoa 0:ffd0caf3db9f 482 //else if ((yaw > drum10_max[0])|| (yaw < drum10_min[0]) || (pitch > drum10_max[1] + pitch_ratio) /*&& (roll > drum2_max[2])*/) {
fxanhkhoa 0:ffd0caf3db9f 483 else if (pitch < (drum10_max[1] - pitch_ratio)){
fxanhkhoa 0:ffd0caf3db9f 484 stt2 = 0 ;
fxanhkhoa 0:ffd0caf3db9f 485 drum5_stt2 = 0;
fxanhkhoa 0:ffd0caf3db9f 486 pc.printf("up\r\n");
fxanhkhoa 0:ffd0caf3db9f 487 }
fxanhkhoa 0:ffd0caf3db9f 488 break;
fxanhkhoa 0:ffd0caf3db9f 489 default:
fxanhkhoa 0:ffd0caf3db9f 490 break;
fxanhkhoa 0:ffd0caf3db9f 491 };
fxanhkhoa 0:ffd0caf3db9f 492 }
fxanhkhoa 0:ffd0caf3db9f 493 else {ble.waitForEvent();} // low power wait for event
fxanhkhoa 0:ffd0caf3db9f 494 }
fxanhkhoa 0:ffd0caf3db9f 495 //}
fxanhkhoa 0:ffd0caf3db9f 496 }
fxanhkhoa 0:ffd0caf3db9f 497 }
fxanhkhoa 0:ffd0caf3db9f 498
fxanhkhoa 0:ffd0caf3db9f 499 int main(void)
fxanhkhoa 0:ffd0caf3db9f 500 {
fxanhkhoa 0:ffd0caf3db9f 501
fxanhkhoa 0:ffd0caf3db9f 502 drum1_min[0] = 15 - ratioy;
fxanhkhoa 0:ffd0caf3db9f 503 drum1_max[0] = 15 + ratioy;
fxanhkhoa 0:ffd0caf3db9f 504 drum2_min[0] = 45 - ratioy;
fxanhkhoa 0:ffd0caf3db9f 505 drum2_max[0] = 45 + ratioy;
fxanhkhoa 0:ffd0caf3db9f 506 drum3_min[0] = 75 - ratioy;
fxanhkhoa 0:ffd0caf3db9f 507 drum3_max[0] = 75 + ratioy;
fxanhkhoa 0:ffd0caf3db9f 508 drum4_min[0] = 16 - ratioy;
fxanhkhoa 0:ffd0caf3db9f 509 drum4_max[0] = 16 + ratioy;
fxanhkhoa 0:ffd0caf3db9f 510 drum5_min[0] = 85 - ratioy;
fxanhkhoa 0:ffd0caf3db9f 511 drum5_max[0] = 85 + ratioy;
fxanhkhoa 0:ffd0caf3db9f 512 drum6_min[0] = 0 - ratioy;
fxanhkhoa 0:ffd0caf3db9f 513 drum6_max[0] = 0 + ratioy;
fxanhkhoa 0:ffd0caf3db9f 514 drum7_min[0] = 0 - ratioy;
fxanhkhoa 0:ffd0caf3db9f 515 drum7_max[0] = 0 + ratioy;
fxanhkhoa 0:ffd0caf3db9f 516 drum8_min[0] = 0 - ratioy;
fxanhkhoa 0:ffd0caf3db9f 517 drum8_max[0] = 0 + ratioy;
fxanhkhoa 0:ffd0caf3db9f 518 drum9_min[0] = 0 - ratioy;
fxanhkhoa 0:ffd0caf3db9f 519 drum9_max[0] = 0 + ratioy;
fxanhkhoa 0:ffd0caf3db9f 520 drum10_min[0] = 0 - ratioy;
fxanhkhoa 0:ffd0caf3db9f 521 drum10_max[0] = 0 + ratioy;
fxanhkhoa 0:ffd0caf3db9f 522
fxanhkhoa 0:ffd0caf3db9f 523 drum1_max[1] = 105;
fxanhkhoa 0:ffd0caf3db9f 524 drum2_max[1] = 100;
fxanhkhoa 0:ffd0caf3db9f 525 drum3_max[1] = 105;
fxanhkhoa 0:ffd0caf3db9f 526 drum4_max[1] = 20;
fxanhkhoa 0:ffd0caf3db9f 527 drum5_max[1] = 20;
fxanhkhoa 0:ffd0caf3db9f 528 drum6_max[1] = 0;
fxanhkhoa 0:ffd0caf3db9f 529 drum7_max[1] = 0;
fxanhkhoa 0:ffd0caf3db9f 530 drum8_max[1] = 0;
fxanhkhoa 0:ffd0caf3db9f 531 drum9_max[1] = 0;
fxanhkhoa 0:ffd0caf3db9f 532 drum10_max[1] = 0;
fxanhkhoa 0:ffd0caf3db9f 533
fxanhkhoa 0:ffd0caf3db9f 534 Ticker ticker;
fxanhkhoa 0:ffd0caf3db9f 535 ticker.attach(periodicCallback, 0.0001); // blink LED every second
fxanhkhoa 0:ffd0caf3db9f 536 mybutton.fall(get);
fxanhkhoa 0:ffd0caf3db9f 537
fxanhkhoa 0:ffd0caf3db9f 538
fxanhkhoa 0:ffd0caf3db9f 539 BLE::Instance().init(bleInitComplete);
fxanhkhoa 0:ffd0caf3db9f 540 }
fxanhkhoa 0:ffd0caf3db9f 541
fxanhkhoa 0:ffd0caf3db9f 542 void get()
fxanhkhoa 0:ffd0caf3db9f 543 {
fxanhkhoa 0:ffd0caf3db9f 544 j++;
fxanhkhoa 0:ffd0caf3db9f 545 led1 = 0;
fxanhkhoa 0:ffd0caf3db9f 546 //gettime.start();
fxanhkhoa 0:ffd0caf3db9f 547 //while (gettime.read() < 2) {};
fxanhkhoa 0:ffd0caf3db9f 548 if (j == 1){
fxanhkhoa 0:ffd0caf3db9f 549 drum1_min[0] = yaw - ratioy;
fxanhkhoa 0:ffd0caf3db9f 550 drum1_min[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 551 drum1_min[2] = angle[0] * Rad2Dree - ratio;
fxanhkhoa 0:ffd0caf3db9f 552
fxanhkhoa 0:ffd0caf3db9f 553 drum1_max[0] = yaw + ratioy;
fxanhkhoa 0:ffd0caf3db9f 554 drum1_max[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 555 drum1_max[2] = angle[0] * Rad2Dree + ratio;
fxanhkhoa 0:ffd0caf3db9f 556 }
fxanhkhoa 0:ffd0caf3db9f 557 else if (j == 2){
fxanhkhoa 0:ffd0caf3db9f 558 drum2_min[0] = yaw - ratioy;
fxanhkhoa 0:ffd0caf3db9f 559 drum2_min[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 560 drum2_min[2] = angle[0] * Rad2Dree - ratio;
fxanhkhoa 0:ffd0caf3db9f 561
fxanhkhoa 0:ffd0caf3db9f 562 drum2_max[0] = yaw + ratioy;
fxanhkhoa 0:ffd0caf3db9f 563 drum2_max[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 564 drum2_max[2] = angle[0] * Rad2Dree + ratio;
fxanhkhoa 0:ffd0caf3db9f 565 }
fxanhkhoa 0:ffd0caf3db9f 566 else if (j == 3){
fxanhkhoa 0:ffd0caf3db9f 567 drum3_min[0] = yaw - ratioy;
fxanhkhoa 0:ffd0caf3db9f 568 drum3_min[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 569 drum3_min[2] = angle[0] * Rad2Dree - ratio;
fxanhkhoa 0:ffd0caf3db9f 570
fxanhkhoa 0:ffd0caf3db9f 571 drum3_max[0] = yaw + ratioy;
fxanhkhoa 0:ffd0caf3db9f 572 drum3_max[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 573 drum3_max[2] = angle[0] * Rad2Dree + ratio;
fxanhkhoa 0:ffd0caf3db9f 574 }
fxanhkhoa 0:ffd0caf3db9f 575 else if (j == 4){
fxanhkhoa 0:ffd0caf3db9f 576 drum4_min[0] = yaw - ratioy;
fxanhkhoa 0:ffd0caf3db9f 577 drum4_min[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 578 drum4_min[2] = angle[0] * Rad2Dree - ratio;
fxanhkhoa 0:ffd0caf3db9f 579
fxanhkhoa 0:ffd0caf3db9f 580 drum4_max[0] = yaw + ratioy;
fxanhkhoa 0:ffd0caf3db9f 581 drum4_max[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 582 drum4_max[2] = angle[0] * Rad2Dree + ratio;
fxanhkhoa 0:ffd0caf3db9f 583 }
fxanhkhoa 0:ffd0caf3db9f 584 else if (j == 5){
fxanhkhoa 0:ffd0caf3db9f 585 drum5_min[0] = yaw - ratioy;
fxanhkhoa 0:ffd0caf3db9f 586 drum5_min[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 587 drum5_min[2] = angle[0] * Rad2Dree - ratio;
fxanhkhoa 0:ffd0caf3db9f 588
fxanhkhoa 0:ffd0caf3db9f 589 drum5_max[0] = yaw + ratioy;
fxanhkhoa 0:ffd0caf3db9f 590 drum5_max[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 591 drum5_max[2] = angle[0] * Rad2Dree + ratio;
fxanhkhoa 0:ffd0caf3db9f 592 }
fxanhkhoa 0:ffd0caf3db9f 593 else if (j == 6){
fxanhkhoa 0:ffd0caf3db9f 594 drum6_min[0] = yaw - ratioy;
fxanhkhoa 0:ffd0caf3db9f 595 drum6_min[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 596 drum6_min[2] = angle[0] * Rad2Dree - ratio;
fxanhkhoa 0:ffd0caf3db9f 597
fxanhkhoa 0:ffd0caf3db9f 598 drum6_max[0] = yaw + ratioy;
fxanhkhoa 0:ffd0caf3db9f 599 drum6_max[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 600 drum6_max[2] = angle[0] * Rad2Dree + ratio;
fxanhkhoa 0:ffd0caf3db9f 601 }
fxanhkhoa 0:ffd0caf3db9f 602 else if (j == 7){
fxanhkhoa 0:ffd0caf3db9f 603 drum7_min[0] = yaw - ratioy;
fxanhkhoa 0:ffd0caf3db9f 604 drum7_min[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 605 drum7_min[2] = angle[0] * Rad2Dree - ratio;
fxanhkhoa 0:ffd0caf3db9f 606
fxanhkhoa 0:ffd0caf3db9f 607 drum7_max[0] = yaw + ratioy;
fxanhkhoa 0:ffd0caf3db9f 608 drum7_max[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 609 drum7_max[2] = angle[0] * Rad2Dree + ratio;
fxanhkhoa 0:ffd0caf3db9f 610 }
fxanhkhoa 0:ffd0caf3db9f 611 else if (j == 8){
fxanhkhoa 0:ffd0caf3db9f 612 drum8_min[0] = yaw - ratioy;
fxanhkhoa 0:ffd0caf3db9f 613 drum8_min[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 614 drum8_min[2] = angle[0] * Rad2Dree - ratio;
fxanhkhoa 0:ffd0caf3db9f 615
fxanhkhoa 0:ffd0caf3db9f 616 drum8_max[0] = yaw + ratioy;
fxanhkhoa 0:ffd0caf3db9f 617 drum8_max[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 618 drum8_max[2] = angle[0] * Rad2Dree + ratio;
fxanhkhoa 0:ffd0caf3db9f 619 }
fxanhkhoa 0:ffd0caf3db9f 620 else if (j == 9){
fxanhkhoa 0:ffd0caf3db9f 621 drum9_min[0] = yaw - ratioy;
fxanhkhoa 0:ffd0caf3db9f 622 drum9_min[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 623 drum9_min[2] = angle[0] * Rad2Dree - ratio;
fxanhkhoa 0:ffd0caf3db9f 624
fxanhkhoa 0:ffd0caf3db9f 625 drum9_max[0] = yaw + ratioy;
fxanhkhoa 0:ffd0caf3db9f 626 drum9_max[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 627 drum9_max[2] = angle[0] * Rad2Dree + ratio;
fxanhkhoa 0:ffd0caf3db9f 628 }
fxanhkhoa 0:ffd0caf3db9f 629 else if (j == 10){
fxanhkhoa 0:ffd0caf3db9f 630 drum10_min[0] = yaw - ratioy;
fxanhkhoa 0:ffd0caf3db9f 631 drum10_min[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 632 drum10_min[2] = angle[0] * Rad2Dree - ratio;
fxanhkhoa 0:ffd0caf3db9f 633
fxanhkhoa 0:ffd0caf3db9f 634 drum10_max[0] = yaw + ratioy;
fxanhkhoa 0:ffd0caf3db9f 635 drum10_max[1] = angle[1] * Rad2Dree;
fxanhkhoa 0:ffd0caf3db9f 636 drum10_max[2] = angle[0] * Rad2Dree + ratio;
fxanhkhoa 0:ffd0caf3db9f 637 }
fxanhkhoa 0:ffd0caf3db9f 638 if (j == 10) j = 0;
fxanhkhoa 0:ffd0caf3db9f 639 pc.printf("x,y,z: %f %f %f \r\n",yaw,pitch,roll);
fxanhkhoa 0:ffd0caf3db9f 640 led1 = 1;
fxanhkhoa 0:ffd0caf3db9f 641 }
fxanhkhoa 0:ffd0caf3db9f 642