Khoa Bui Anh
Wireless auto note device
Dependencies: BLE_API invisdrum X_NUCLEO_IDB0XA1 kalman mbed
Diff: main.cpp
- Revision:
- 0:ffd0caf3db9f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Nov 22 02:57:33 2016 +0000
@@ -0,0 +1,642 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2015 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "mbed.h"
+#include "MPU6050.h"
+#include "MPU60501.h"
+#include "HMC5883L.h"
+#include "ble/BLE.h"
+#include "ble/services/HeartRateService.h"
+#include "kalman.c"
+#include <math.h>
+
+#define ratio 5
+#define ratioy 9
+#define pitch_ratio 6
+#define PI 3.1415926535897932384626433832795
+#define Rad2Dree 57.295779513082320876798154814105
+
+#define wait_time 0.02
+
+int i= 0,j = 0;
+float sum = 0;
+uint32_t sumCount = 0;
+volatile uint8_t hrmCounter;
+
+float central1[3], central2[3];
+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];
+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];
+int flag = 0;
+int stt1 = 0, stt2 = 0;
+int drum1_stt1 = 0,drum2_stt1 = 0,drum3_stt1 = 0,drum4_stt1 = 0,drum5_stt1 = 0;
+int drum1_stt2 = 0,drum2_stt2 = 0,drum3_stt2 = 0,drum4_stt2 = 0,drum5_stt2 = 0;
+
+ float Acc[3];
+ float Gyro[3];
+ float angle[3];
+ int Mag[3];
+ float R,R2;
+ unsigned long timer;
+ long loopStartTime;
+
+ Timer GlobalTime;
+ Timer ProgramTimer;
+ kalman filter_pitch;
+ kalman filter_roll;
+ kalman filter_yaw;
+
+
+ InterruptIn mybutton(USER_BUTTON);
+
+ MPU60501 mpu6050;
+
+ MPU60501 mpu6050_2;
+
+ MPU6050 ark(PB_9,PB_8);
+
+ HMC5883L compass(PB_9, PB_8);
+
+ Timer tmpu;
+
+ Timer gettime;
+
+ Serial pc(USBTX, USBRX); // tx, rx
+
+ // VCC, SCE, RST, D/C, MOSI,S CLK, LED
+ //N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
+
+void get();
+
+DigitalOut led1(LED1, 1);
+
+const static char DEVICE_NAME[] = "Drum";
+static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
+
+static volatile bool triggerSensorPolling = false;
+
+void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
+{
+ (void)params;
+ BLE::Instance().gap().startAdvertising(); // restart advertising
+}
+
+void periodicCallback(void)
+{
+ //led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
+ /* Note that the periodicCallback() executes in interrupt context, so it is safer to do
+ * heavy-weight sensor polling from the main thread. */
+ triggerSensorPolling = true;
+}
+
+void onBleInitError(BLE &ble, ble_error_t error)
+{
+ (void)ble;
+ (void)error;
+ /* Initialization error handling should go here */
+}
+
+void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
+{
+ BLE& ble = params->ble;
+ ble_error_t error = params->error;
+
+ if (error != BLE_ERROR_NONE) {
+ onBleInitError(ble, error);
+ return;
+ }
+
+ if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
+ return;
+ }
+
+ ble.gap().onDisconnection(disconnectionCallback);
+
+ /* Setup primary service. */
+ uint8_t hrmCounter = 'A'; // init HRM to 60bps
+ HeartRateService hrService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
+
+ /* Setup advertising. */
+ ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
+ ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
+ ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
+ ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
+ ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+ ble.gap().setAdvertisingInterval(1000); /* 1000ms */
+ ble.gap().startAdvertising();
+
+ pc.baud(9600);
+
+ //Set up I2C
+ i2c.frequency(400000); // use fast (400 kHz) I2C
+ i2c2.frequency(400000);
+
+ compass.setDefault();
+ wait(0.1);
+
+
+ //lcd.init();
+ //lcd.setBrightness(0.05);
+
+
+ // Read the WHO_AM_I register, this is a good test of communication
+ uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
+ //uint8_t whoami2 = mpu6050_2.readByte2(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
+ pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
+
+ if ((whoami == 0x68) /*|| (whoami2 == 0x68)*/) // WHO_AM_I should always be 0x68
+ {
+ pc.printf("MPU6050 is online...");
+ wait(1);
+ //lcd.clear();
+ //lcd.printString("MPU6050 OK", 0, 0);
+
+
+ mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
+ mpu6050.MPU6050SelfTest2(SelfTest2); // Start by performing self test and reporting values
+ pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
+ pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
+ pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
+ pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
+ pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
+ pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
+ wait(1);
+
+ 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)
+ {
+ mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
+ mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+ mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+
+ /*lcd.clear();
+ lcd.printString("MPU6050", 0, 0);
+ lcd.printString("pass self test", 0, 1);
+ lcd.printString("initializing", 0, 2); */
+ wait(2);
+ }
+ else
+ {
+ pc.printf("Device did not the pass self-test!\n\r");
+
+ /*lcd.clear();
+ lcd.printString("MPU6050", 0, 0);
+ lcd.printString("no pass", 0, 1);
+ lcd.printString("self test", 0, 2);*/
+ }
+ }
+ else
+ {
+ pc.printf("Could not connect to MPU6050: \n\r");
+ pc.printf("%#x \n", whoami);
+
+ /*lcd.clear();
+ lcd.printString("MPU6050", 0, 0);
+ lcd.printString("no connection", 0, 1);
+ lcd.printString("0x", 0, 2); lcd.setXYAddress(20, 2); lcd.printChar(whoami);*/
+
+ while(1) ; // Loop forever if communication doesn't happen
+ }
+ i++;
+ tmpu.start();
+
+ kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+ kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+ kalman_init(&filter_yaw, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+
+ ProgramTimer.start();
+ loopStartTime = ProgramTimer.read_us();
+ timer = loopStartTime;
+ // infinite loop
+ while (1) {
+ ark.getAccelero(Acc);
+ ark.getGyro(Gyro);
+ Mag[0] = compass.getMx();
+ Mag[1] = compass.getMy();
+ Mag[2] = compass.getMz();
+ R = sqrt(std::pow(Acc[0] , 2) + std::pow(Acc[1] , 2) + std::pow(Acc[2] , 2));
+ //R2 = sqrt(std::pow(Acc2[0] , 2) + std::pow(Acc2[1] , 2) + std::pow(Acc2[2] , 2));
+
+ kalman_predict(&filter_pitch, Gyro[0], (ProgramTimer.read_us() - timer));
+ kalman_update(&filter_pitch, acos(Acc[0]/R));
+ kalman_predict(&filter_roll, Gyro[1], (ProgramTimer.read_us() - timer));
+ kalman_update(&filter_roll, acos(Acc[1]/R));
+ kalman_predict(&filter_yaw, (float)Mag[1]/Mag[0], (ProgramTimer.read_us() - timer));
+ kalman_update(&filter_yaw, atan((float)Mag[1]/Mag[0]));
+
+ angle[0] = kalman_get_angle(&filter_pitch);
+ angle[1] = kalman_get_angle(&filter_roll);
+ angle[2] = kalman_get_angle(&filter_yaw);
+
+ //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)) );
+ timer = ProgramTimer.read_us();
+
+ // If data ready bit set, all data registers have new data
+ if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
+ mpu6050.readAccelData(accelCount);
+ }
+
+ yaw = angle[2] * Rad2Dree;
+ if ((yaw < 46) && (yaw > 44) && (flag == 0))
+ {
+
+ central1[0] = yaw;
+ central1[1] = pitch;
+ central1[2] = roll;
+ central2[0] = yaw2;
+ central2[1] = pitch2;
+ central2[2] = roll2;
+
+
+ pc.printf("central x y z : %f %f %f \r\n", central1[0],central1[1],central1[2]);
+ flag = 1;
+ }
+ if (i == 1000) i = 0;
+ if (flag == 1)
+ {
+ pitch = angle[1] * Rad2Dree;
+ roll = angle[0] * Rad2Dree;
+ yaw = angle[2] * Rad2Dree;
+ //yaw = atan((float) Mag[1]/Mag[0])*Rad2Dree;
+ //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)) );
+ //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
+ //pc.printf("%d %d %d \r\n",Mag[0],Mag[1],Mag[2]);
+ //pc.printf("Magx Magy Magz : %d %d %d \r\n",Mag[0],Mag[1],Mag[2]);
+ //wait(0.1);
+ //hrService.updateHeartRate((uint8_t)yaw);
+ //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", drum1_min[0], drum1_max[0], drum1_min[2]);
+ int step = 0, step2 = 0;
+ //while (triggerSensorPolling && ble.getGapState().connected == 1) {};
+ if (triggerSensorPolling && ble.getGapState().connected) {
+ triggerSensorPolling = false;
+ switch (stt1)
+ {
+ case 0:
+ //pc.printf("%d\n",stt1);
+ step = 0;
+ if ((yaw < drum2_max[0]) && (yaw > drum2_min[0]) && (pitch > drum2_max[1])) stt1 = 2;
+ else if ((yaw < drum1_max[0]) && (yaw > drum1_min[0]) && (pitch > drum1_max[1])) stt1 = 1;
+ else if ((yaw < drum3_max[0]) && (yaw > drum3_min[0]) && (pitch > drum3_max[1])) stt1 = 3;
+ else if ((yaw < drum4_max[0]) && (yaw > drum4_min[0]) && (pitch < drum4_max[1])) stt1 = 4;
+ else if ((yaw < drum5_max[0]) && (yaw > drum5_min[0]) && (pitch < drum5_max[1])) stt1 = 5;
+ //hrService.updateHeartRate((uint8_t)96);
+ break;
+ case 1:
+ //pc.printf("%d\n",stt1);
+ if ((drum1_stt1 == 0) && (yaw < drum1_max[0]) && (yaw > drum1_min[0]) && (pitch > drum1_max[1]))
+ {
+ //pc.printf("drum 1_1\r\n");
+ drum1_stt1 = 1;
+ hrService.updateHeartRate((uint8_t)1);
+ wait(wait_time);
+ }
+ //else if ((yaw > drum1_max[0] + 2)|| (yaw < drum1_min[0] - 2) || (pitch < (drum1_max[1] - pitch_ratio)) /*&& (roll > drum1_max[2])*/) {
+ else if (pitch < (drum1_max[1] - pitch_ratio)){
+ stt1 = 0 ;
+ drum1_stt1 = 0;
+ //pc.printf("up\r\n");
+ }
+ break;
+ case 2:
+ //pc.printf("%d\n",stt1);
+ if ((drum2_stt1 == 0) && (yaw < drum2_max[0]) && (yaw > drum2_min[0]) && (pitch > drum2_max[1]))
+ {
+ //pc.printf("drum 2_2\r\n");
+ drum2_stt1 = 1;
+ hrService.updateHeartRate((uint8_t)2);
+ wait(wait_time);
+ }
+ //else if ((yaw > drum2_max[0])|| (yaw < drum2_min[0]) || (pitch < (drum2_max[1] - pitch_ratio)) /*&& (roll > drum2_max[2])*/) {
+ else if (pitch < (drum2_max[1] - pitch_ratio)){
+ stt1 = 0 ;
+ drum2_stt1 = 0;
+ //pc.printf("up\r\n");
+ }
+ break;
+ case 3:
+ //pc.printf("%d\n",stt1);
+ if ((drum3_stt1 == 0) && (yaw < drum3_max[0]) && (yaw > drum3_min[0]) && (pitch > drum3_max[1]))
+ {
+ //pc.printf("drum 3_3\r\n");
+ drum3_stt1 = 1;
+ hrService.updateHeartRate((uint8_t)3);
+ wait(wait_time);
+ }
+ //else if ((yaw > drum3_max[0])|| (yaw < drum3_min[0]) || (pitch < (drum3_max[1] - pitch_ratio)) /*&& (roll > drum2_max[2])*/) {
+ else if (pitch < (drum3_max[1] - pitch_ratio)){
+ stt1 = 0 ;
+ drum3_stt1 = 0;
+ //pc.printf("up\r\n");
+ }
+ break;
+ case 4:
+ //pc.printf("%d\n",stt1);
+ if (drum4_stt1 == 0)
+ {
+ if ((pitch > drum4_max[1]) && (step == 0))
+ {
+ //pc.printf("drum 4_4\r\n");
+ drum4_stt1 = 1;
+ hrService.updateHeartRate((uint8_t)4);
+ step = 1;
+ wait(wait_time);
+ }
+ }
+ //else if ((yaw > drum4_max[0])|| (yaw < drum4_min[0]) || (pitch > (drum4_max[1] + pitch_ratio)) /*&& (roll > drum2_max[2])*/) {
+ else if (pitch > (drum4_max[1] + pitch_ratio)){
+ stt1 = 0 ;
+ drum4_stt1 = 0;
+ //pc.printf("up\r\n");
+ }
+ break;
+ case 5:
+ //pc.printf("%d\n",stt1);
+ if (drum5_stt1 == 0)
+ {
+ if ((pitch > drum5_max[1]) && (step == 0))
+ {
+ pc.printf("drum 5_5\r\n");
+ drum5_stt1 = 1;
+ hrService.updateHeartRate((uint8_t)5);
+ step = 1;
+ wait(wait_time);
+ }
+ }
+ //else if ((yaw > drum5_max[0])|| (yaw < drum5_min[0]) || (pitch > (drum5_max[1] + pitch_ratio)) /*&& (roll > drum2_max[2])*/) {
+ else if (pitch > (drum5_max[1] + pitch_ratio)){
+ stt1 = 0 ;
+ drum5_stt1 = 0;
+ //pc.printf("up\r\n");
+ }
+ break;
+ default:
+ break;
+ };
+ triggerSensorPolling = false;
+ switch (stt2){
+ case 0:
+ //hrService.updateHeartRate((uint8_t)69);
+ //pc.printf("%d\r\n",stt2);
+ step2 = 0;
+ /*
+ if ((yaw < drum6_max[0]) && (yaw > drum6_min[0]) && (pitch > drum6_max[1])) stt1 = 1;
+ else if ((yaw < drum7_max[0]) && (yaw > drum7_min[0]) && (pitch > drum7_max[1])) stt1 = 2;
+ else if ((yaw < drum8_max[0]) && (yaw > drum8_min[0]) && (pitch > drum8_max[1])) stt1 = 3;
+ else if ((yaw < drum9_max[0]) && (yaw > drum9_min[0]) && (pitch < drum9_max[1])) stt1 = 4;
+ else if ((yaw < drum10_max[0]) && (yaw > drum10_min[0]) && (pitch < drum10_max[1])) stt1 = 5;
+ */
+ //hrService.updateHeartRate((uint8_t)35);
+ break;
+ case 1:
+ //pc.printf("stt 2: %d ",stt2);
+ if ((drum1_stt2 == 0) && (yaw < drum6_max[0]) && (yaw > drum6_min[0]) && (pitch > drum6_max[1]))
+ {
+ pc.printf("drum 1_1\r\n");
+ drum1_stt2 = 1;
+ hrService.updateHeartRate((uint8_t)1);
+ wait(wait_time);
+ }
+ //else if ((yaw > drum1_max[0]) || (yaw < drum6_min[0]) || (pitch < drum6_max[1] - pitch_ratio) /*&& (roll > drum1_max[2])*/) {
+ else if (pitch < (drum6_max[1] - pitch_ratio)){
+ stt2 = 0 ;
+ drum1_stt2 = 0;
+ //pc.printf("up\r\n");
+ }
+ break;
+ case 2:
+ //pc.printf("stt 2:%d ",stt2);
+ if (drum2_stt2 == 0)
+ {
+ pc.printf("drum 2_2\r\n");
+ drum2_stt2 = 1;
+ hrService.updateHeartRate((uint8_t)2);
+ wait(wait_time);
+ }
+ //else if ((yaw > drum2_max[0])|| (yaw < drum7_min[0]) || (pitch < drum7_max[1] - pitch_ratio) /*&& (roll > drum2_max[2])*/) {
+ else if (pitch < (drum7_max[1] - pitch_ratio)){
+ stt2 = 0 ;
+ drum2_stt2 = 0;
+ //pc.printf("up\r\n");
+ }
+ break;
+ case 3:
+ //pc.printf("stt 2: %d ",stt2);
+ if (drum3_stt2 == 0)
+ {
+ pc.printf("drum 3_3\r\n");
+ drum3_stt2 = 1;
+ hrService.updateHeartRate((uint8_t)3);
+ wait(wait_time);
+ }
+ //else if ((yaw > drum8_max[0])|| (yaw < drum8_min[0]) || (pitch < drum8_max[1] - pitch_ratio) /*&& (roll > drum2_max[2])*/) {
+ else if (pitch > (drum8_max[1] + pitch_ratio)){
+ stt1 = 0 ;
+ drum3_stt2 = 0;
+ //pc.printf("up\r\n");
+ }
+ break;
+ case 4:
+ pc.printf("stt 2: %d ",stt2);
+ if (drum4_stt2 == 0)
+ {
+ if ((pitch > drum9_max[1]) && (step2 == 0) && (yaw < drum9_max[0]) && (yaw > drum9_min[0]) && (pitch < drum9_max[1]))
+ {
+ pc.printf("drum 4_4\r\n");
+ drum4_stt2 = 1;
+ hrService.updateHeartRate((uint8_t)4);
+ step2 = 1;
+ wait(wait_time);
+ }
+ }
+ //else if ((yaw > drum9_max[0])|| (yaw < drum9_min[0]) || (pitch > drum9_max[1] + pitch_ratio) /*&& (roll > drum2_max[2])*/) {
+ else if (pitch > (drum9_max[1] + pitch_ratio)){
+ stt2 = 0 ;
+ drum4_stt2 = 0;
+ pc.printf("up\r\n");
+ }
+ break;
+ case 5:
+ //pc.printf("stt 2: %d ",stt2);
+ if (drum5_stt2 == 0)
+ {
+ if ((pitch > drum10_max[1]) && (step2 == 0) && (yaw < drum10_max[0]) && (yaw > drum10_min[0]))
+ {
+ pc.printf("drum 5_5\r\n");
+ drum5_stt2 = 1;
+ hrService.updateHeartRate((uint8_t)5);
+ step2 = 1;
+ wait(wait_time);
+ }
+ }
+ //else if ((yaw > drum10_max[0])|| (yaw < drum10_min[0]) || (pitch > drum10_max[1] + pitch_ratio) /*&& (roll > drum2_max[2])*/) {
+ else if (pitch < (drum10_max[1] - pitch_ratio)){
+ stt2 = 0 ;
+ drum5_stt2 = 0;
+ pc.printf("up\r\n");
+ }
+ break;
+ default:
+ break;
+ };
+ }
+ else {ble.waitForEvent();} // low power wait for event
+}
+//}
+ }
+}
+
+int main(void)
+{
+
+drum1_min[0] = 15 - ratioy;
+drum1_max[0] = 15 + ratioy;
+drum2_min[0] = 45 - ratioy;
+drum2_max[0] = 45 + ratioy;
+drum3_min[0] = 75 - ratioy;
+drum3_max[0] = 75 + ratioy;
+drum4_min[0] = 16 - ratioy;
+drum4_max[0] = 16 + ratioy;
+drum5_min[0] = 85 - ratioy;
+drum5_max[0] = 85 + ratioy;
+drum6_min[0] = 0 - ratioy;
+drum6_max[0] = 0 + ratioy;
+drum7_min[0] = 0 - ratioy;
+drum7_max[0] = 0 + ratioy;
+drum8_min[0] = 0 - ratioy;
+drum8_max[0] = 0 + ratioy;
+drum9_min[0] = 0 - ratioy;
+drum9_max[0] = 0 + ratioy;
+drum10_min[0] = 0 - ratioy;
+drum10_max[0] = 0 + ratioy;
+
+drum1_max[1] = 105;
+drum2_max[1] = 100;
+drum3_max[1] = 105;
+drum4_max[1] = 20;
+drum5_max[1] = 20;
+drum6_max[1] = 0;
+drum7_max[1] = 0;
+drum8_max[1] = 0;
+drum9_max[1] = 0;
+drum10_max[1] = 0;
+
+ Ticker ticker;
+ ticker.attach(periodicCallback, 0.0001); // blink LED every second
+ mybutton.fall(get);
+
+
+ BLE::Instance().init(bleInitComplete);
+}
+
+void get()
+{
+ j++;
+ led1 = 0;
+ //gettime.start();
+ //while (gettime.read() < 2) {};
+ if (j == 1){
+ drum1_min[0] = yaw - ratioy;
+ drum1_min[1] = angle[1] * Rad2Dree;
+ drum1_min[2] = angle[0] * Rad2Dree - ratio;
+
+ drum1_max[0] = yaw + ratioy;
+ drum1_max[1] = angle[1] * Rad2Dree;
+ drum1_max[2] = angle[0] * Rad2Dree + ratio;
+ }
+ else if (j == 2){
+ drum2_min[0] = yaw - ratioy;
+ drum2_min[1] = angle[1] * Rad2Dree;
+ drum2_min[2] = angle[0] * Rad2Dree - ratio;
+
+ drum2_max[0] = yaw + ratioy;
+ drum2_max[1] = angle[1] * Rad2Dree;
+ drum2_max[2] = angle[0] * Rad2Dree + ratio;
+ }
+ else if (j == 3){
+ drum3_min[0] = yaw - ratioy;
+ drum3_min[1] = angle[1] * Rad2Dree;
+ drum3_min[2] = angle[0] * Rad2Dree - ratio;
+
+ drum3_max[0] = yaw + ratioy;
+ drum3_max[1] = angle[1] * Rad2Dree;
+ drum3_max[2] = angle[0] * Rad2Dree + ratio;
+ }
+ else if (j == 4){
+ drum4_min[0] = yaw - ratioy;
+ drum4_min[1] = angle[1] * Rad2Dree;
+ drum4_min[2] = angle[0] * Rad2Dree - ratio;
+
+ drum4_max[0] = yaw + ratioy;
+ drum4_max[1] = angle[1] * Rad2Dree;
+ drum4_max[2] = angle[0] * Rad2Dree + ratio;
+ }
+ else if (j == 5){
+ drum5_min[0] = yaw - ratioy;
+ drum5_min[1] = angle[1] * Rad2Dree;
+ drum5_min[2] = angle[0] * Rad2Dree - ratio;
+
+ drum5_max[0] = yaw + ratioy;
+ drum5_max[1] = angle[1] * Rad2Dree;
+ drum5_max[2] = angle[0] * Rad2Dree + ratio;
+ }
+ else if (j == 6){
+ drum6_min[0] = yaw - ratioy;
+ drum6_min[1] = angle[1] * Rad2Dree;
+ drum6_min[2] = angle[0] * Rad2Dree - ratio;
+
+ drum6_max[0] = yaw + ratioy;
+ drum6_max[1] = angle[1] * Rad2Dree;
+ drum6_max[2] = angle[0] * Rad2Dree + ratio;
+ }
+ else if (j == 7){
+ drum7_min[0] = yaw - ratioy;
+ drum7_min[1] = angle[1] * Rad2Dree;
+ drum7_min[2] = angle[0] * Rad2Dree - ratio;
+
+ drum7_max[0] = yaw + ratioy;
+ drum7_max[1] = angle[1] * Rad2Dree;
+ drum7_max[2] = angle[0] * Rad2Dree + ratio;
+ }
+ else if (j == 8){
+ drum8_min[0] = yaw - ratioy;
+ drum8_min[1] = angle[1] * Rad2Dree;
+ drum8_min[2] = angle[0] * Rad2Dree - ratio;
+
+ drum8_max[0] = yaw + ratioy;
+ drum8_max[1] = angle[1] * Rad2Dree;
+ drum8_max[2] = angle[0] * Rad2Dree + ratio;
+ }
+ else if (j == 9){
+ drum9_min[0] = yaw - ratioy;
+ drum9_min[1] = angle[1] * Rad2Dree;
+ drum9_min[2] = angle[0] * Rad2Dree - ratio;
+
+ drum9_max[0] = yaw + ratioy;
+ drum9_max[1] = angle[1] * Rad2Dree;
+ drum9_max[2] = angle[0] * Rad2Dree + ratio;
+ }
+ else if (j == 10){
+ drum10_min[0] = yaw - ratioy;
+ drum10_min[1] = angle[1] * Rad2Dree;
+ drum10_min[2] = angle[0] * Rad2Dree - ratio;
+
+ drum10_max[0] = yaw + ratioy;
+ drum10_max[1] = angle[1] * Rad2Dree;
+ drum10_max[2] = angle[0] * Rad2Dree + ratio;
+ }
+ if (j == 10) j = 0;
+ pc.printf("x,y,z: %f %f %f \r\n",yaw,pitch,roll);
+ led1 = 1;
+}
+