File content as of revision 0:d75dd19f5e59:

#include "mbed.h"
#include "kalman.c"
#include "MPU6050.h"
#include <math.h>

#define PI             3.1415926535897932384626433832795
#define Rad2Dree       57.295779513082320876798154814105

#define dir 20
#define ratio 2

Serial pc(USBTX, USBRX);
 
void get(); 
void rafius(int, int, int, int);
 
Timer GlobalTime;
Timer ProgramTimer;
InterruptIn mybutton(USER_BUTTON);
 
float R;
double angle[3];
unsigned long timer;
long loopStartTime;

MPU6050 ark(PB_9,PB_8);
kalman filter_pitch; 
kalman filter_roll;
kalman filter_yaw;

volatile float central[3] = {0,0,0},drum[3] = {0,0,0}, delta[3] = {0,0,0};
volatile float delta_1_x[2] ;
volatile float delta_1_y[2] = {23,24}, delta_2_y[2] = {17,18}, delta_3_y[2] = {16,17}, delta_4_y[2] = {16,17}, delta_5_y[2] = {23,24};
volatile float delta_1_z[2];
volatile int drum_stt = 0, drum_1_stt = 0, drum_2_stt = 0, drum_3_stt = 0, drum_4_stt = 0, drum_5_stt = 0;

int main()
{
    mybutton.fall(get);
    //int count = 0;
    GlobalTime.start();
    
    float Acc[3]= {0, 0, 0};
    ark.getAccelero(Acc);
    
    float Gyro[3]= {0,0,0};
    ark.getGyro(Gyro);
    
    
    // Parameters ( R_angle, Q_gyro, Q_angle ) 
    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;
    int i;
    for (i = 0; i< 1000; i++)
    {
        //Aquire new values for the Gyro and Acc
        ark.getAccelero(Acc);
        ark.getGyro(Gyro);
        
        //Calcuate the resulting vector R from the 3 acc axes
        R = sqrt(std::pow(Acc[0] , 2) + std::pow(Acc[1] , 2) + std::pow(Acc[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, Gyro[2],    (ProgramTimer.read_us() - timer));
        kalman_update(&filter_yaw, acos(Acc[2]/R));
        
        angle[0] = kalman_get_angle(&filter_pitch);
        angle[1] = kalman_get_angle(&filter_roll);
        angle[2] = kalman_get_angle(&filter_yaw);
        
        timer = ProgramTimer.read_us();
        
        //count++;
        //if(count == 10000){pc.printf("Program time: %i\n\r", ProgramTimer.read_us() - loopStartTime);loopStartTime = ProgramTimer.read_us(); count = 0;}
        //pc.printf("Pitch Angle X: %.6f   Pitch Angle Y: %.6f \n\r", Rad2Dree * angle[1], Rad2Dree * angle[0]);
        central[0] = Rad2Dree * angle[1];
        central[1] = Rad2Dree * angle[0];
        }
        pc.printf("central %f %f\r\n",central[0],central[1]);
    while(1) {
        //Aquire new values for the Gyro and Acc
        ark.getAccelero(Acc);
        ark.getGyro(Gyro);
        
        //Calcuate the resulting vector R from the 3 acc axes
        R = sqrt(std::pow(Acc[0] , 2) + std::pow(Acc[1] , 2) + std::pow(Acc[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, Gyro[2],    (ProgramTimer.read_us() - timer));
        kalman_update(&filter_yaw, acos(Acc[2]/R));
        
        angle[0] = kalman_get_angle(&filter_pitch);
        angle[1] = kalman_get_angle(&filter_roll);
        angle[2] = kalman_get_angle(&filter_yaw);
        
        timer = ProgramTimer.read_us();
        angle[2] = (sqrt(Acc[0]*Acc[0] + (Acc[1] * Acc[1])) / Acc[2]);
        //count++;
        //if(count == 10000){pc.printf("Program time: %i\n\r", ProgramTimer.read_us() - loopStartTime);loopStartTime = ProgramTimer.read_us(); count = 0;}
        pc.printf("Pitch Angle X: %.6f   Pitch Angle Y: %.6f\r\n Z: %.6f \n\r",Rad2Dree * angle[1],Rad2Dree * angle[0],Rad2Dree * angle[2]);
        delta[0] = Rad2Dree * angle[1];
        delta[1] = Rad2Dree * angle[0];
        delta[2] = Rad2Dree * angle[2];
        //float x,y,z;
        //y = cos(Rad2Dree * angle[1]) * dir;
        //x = cos(90-(Rad2Dree * angle[1])) * dir;
        //z = cos(Rad2Dree * angle[0]) * dir;
        //pc.printf("x: %f\n , y: %f\n , z: %f\n",x,y,z);
        //wait_ms(100);
        switch (drum_stt)
        {
            case 0:
                 if ((delta[0] > delta_1_x[0]) && (delta[0] < delta_1_x[1]) && (delta[1] > delta_1_y[0]) && (delta[1] < delta_1_y[1])&& (delta[2] > delta_1_z[0]) && (delta[2] < delta_1_z[1])) drum_stt = 1;
                 if ((delta[0] >= 20)) drum_stt = 4; 
                break;
            case 1:
                if ((delta[0] > delta_1_x[0]) && (delta[0] < delta_1_x[1]) && (delta[1] > delta_1_y[0]) && (delta[1] < delta_1_y[1])&& (delta[2] > delta_1_z[0]) && (delta[2] < delta_1_z[1]) && (drum_1_stt == 0)) 
                    {
                        pc.printf("drum 1_1");
                        drum_1_stt = 1;
                    }
                else if ((delta[0] > delta_1_x[1]))
                    {
                        drum_stt = 0 ;
                        drum_1_stt = 0;
                    }
                break;
            case 2:
                if ((delta[0] > -30) && (delta[0] < -25) && (delta[1] < 18) && (delta[1] > 17) && (drum_2_stt == 0))
                    {
                        pc.printf("drum 2_2");
                        drum_2_stt = 1;
                    }
                else if ((delta[0] > -25)) 
                    {
                        drum_stt = 0;
                        drum_2_stt = 0;
                    }
                else if ((delta[0] <= -30)) drum_stt = 1;
                break; 
            case 3:
                if ((delta[0] > -25) && (delta[0] < -20) && (delta[1] < 17) && (delta[1] > 16) && (drum_3_stt == 0)) 
                    {
                        pc.printf("drum 3_3");
                        drum_3_stt = 1;
                    }
                else if ((delta[0] > -20)) 
                    {
                        drum_stt = 0;
                        drum_3_stt = 0;
                    }
                else if ((delta[0] <= -25)) drum_stt = 2;
                break;
            case 4:
                if ((delta[0] > 20) && (delta[0] < 25) && (delta[1] > 16) && (delta[1] < 17) && (drum_4_stt == 0))
                    {
                        pc.printf("drum 4_4");
                        drum_4_stt = 1;
                    } 
                else if ((delta[0] < 20)) 
                    {
                        drum_stt = 0;
                        drum_4_stt = 0;
                    }
                else if ((delta[0] >=25)) drum_stt = 5;
                break;
            case 5:
                if ((delta[0] >= 25) && (delta[1] > 23) && (delta[1] < 24) && (drum_5_stt == 0))
                    {
                        pc.printf("drum 5_5");
                        drum_5_stt = 1;
                    }
                else if (delta[0] < 25) 
                    {
                        drum_stt = 0;
                        drum_5_stt = 0;
                    }
                break;
        }
        
    }
}

void get()
{
    drum[0] = Rad2Dree * angle[1];
    drum[1] = Rad2Dree * angle[0];
    drum[2] = Rad2Dree * angle[2];
    pc.printf("drum %f %f\r\n",drum[0],drum[1]);
    pc.printf("deltaX: %f , deltaY: %f\r\n",delta[0],delta[1]);
    radius(drum[0],drum[1],drum[2],1);
}

void radius(float x,float y,float z, int ind)
{
    delta_1_x[0] = x - ratio;
    delta_1_x[1] = x + ratio;
    delta_1_y[0] = y - ratio;
    delta_1_y[1] = y + ratio;
    delta_1_z[0] = z - ratio;
    delta_1_z[1] = z + ratio;
}