File content as of revision 0:ffd0caf3db9f:

/* 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;
}