File content as of revision 1:4a15cf53ca28:

#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 10

Serial pc(USBTX, USBRX);

void get();
void radius(float, float, float, 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], 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];
        central[2] = Rad2Dree * angle[2];
    }
    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 %f\r\n",drum[0],drum[1],drum[2]);
    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;
}