File content as of revision 1:e31dea65b070:

#include "mbed.h"
#include "MPU6050.h"

DigitalOut led(LED1);
DigitalOut led1(LED2);
Serial pc(USBTX, USBRX);
MPU6050 mpu;
Timer timer;
Timer timer1;
Timer timer2;
Timer timer3;

int16_t ax, ay, az;
int16_t gx, gy, gz, buff_x[1000],buff_y[1000],buff_z[1000];
int16_t i=1,ym=5;
int32_t toplam_x=0,toplam_y=0,toplam_z=0;
float x,y,z,k,l,m,k1,l1,x1,y1;
float begin, end,end1,end2, ve, s,t,u,s1,t1,u1;

void accelerometer(void)
{
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    ax=(ax-toplam_x);
    ay=ay-toplam_y;
    az=az+toplam_z;

    ax=ax-toplam_x;
    ay=ay-toplam_y;
    az=az-toplam_z;

    x=ax/16383.00;
    y=ay/16383.00;
    z=az/16383.00;
    x1=x;
    y1=y;

}

void accelerometer1(void)
{
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    ax=(ax-toplam_x);
    ay=ay-toplam_y;
    az=az+toplam_z;

    ax=ax-toplam_x;
    ay=ay-toplam_y;
    az=az-toplam_z;

    s=ax/16383.00;
    t=ay/16383.00;
    u=az/16383.00;

}

void accelerometer2(void)
{
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    ax=(ax-toplam_x);
    ay=ay-toplam_y;
    az=az+toplam_z;

    ax=ax-toplam_x;
    ay=ay-toplam_y;
    az=az-toplam_z;

    s1=ax/16383.00;
    t1=ay/16383.00;
    u1=az/16383.00;

}

int main()
{
    pc.printf("MPU6050 testConnection\n");
    
    bool mpu6050TestResult = mpu.testConnection();

    if(mpu6050TestResult) {
        pc.printf("MPU6050 test passed \n");
    } else {
        pc.printf("MPU6050 test failed \n");
    }

    mpu.initialize();
    mpu.setDLPFMode(1);
    pc.printf("DLPF=%d\n\r",mpu.getDLPFMode());


    for(i=0; i<500; i++) {
        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        buff_x[i]=ax;
        toplam_x=toplam_x+buff_x[i];
        buff_y[i]=ay;
        toplam_y=toplam_y+buff_y[i];
        buff_z[i]=az;
        toplam_z=toplam_z+buff_z[i];
    }

    toplam_x=toplam_x/500;
    toplam_y=toplam_y/500;
    toplam_z=toplam_z/500;
    toplam_z=16383-toplam_z;

    ax=(ax-toplam_x);
    ay=ay-toplam_y;
    az=az+toplam_z;

    ax=ax-toplam_x;
    ay=ay-toplam_y;
    az=az-toplam_z;

    k=ax/16383.00;
    l=ay/16383.00;
    m=az/16383.00;

    i=0;
    led=1;
    
    k1=k;
    l1=l;
    
    while(1) {
        k=k1;
        l=l1;
        timer.start();
        begin = timer.read();

        accelerometer();
        wait_ms(10);
        i++;
        pc.printf("Beklemede\n\r");
        
        /* HEAD FRONT MOVEMENT */
        timer1.reset();
        if ((y+2) < (l+1.7)) {
                                
                                timer1.start();
                                accelerometer1();
                                wait_ms(10);
            
                                while((t+2)<(l+1.85)) {
                                                        
                                                        end=timer1.read();
                                                        accelerometer1();
                                                        wait_ms(10);
                                                        
                                                        if (end>1.5) {
                                                                        while (t<-0.15) { accelerometer1();  }
                                                                        y=5.0;
                                                                        t=7.0;
                                                                        end=0;
                                                                        timer1.reset();
                                                                        break;
                                                                     }
                                                                     
                                                        }
                                                              
                                if (  (y+2)< (l+1.7) ) {                 
                                                   
                                accelerometer2();                      
                                timer2.reset();
                                timer2.start();
                                while((t1+2)>(l+1.85)) {  
                                
                                                        accelerometer2();
                                                        wait_ms(10);
                                                        end1=timer2.read();
                                                        ym=1;
                                                        if (end1>1.5) {
                                                                        y=5;
                                                                        ym=5;
                                                                        l=5;
                                                                        break;
                                                                      }
                                                      }  
                                                      
                                                      }
                        }
                        
                        
        /* HEAD back MOVEMENT */
       if ((y1+1.7) > (l+2)) {
                                timer1.reset();
                                timer1.start();
                                accelerometer1();
                                wait_ms(10);
            
                                while((t+1.85)>(l+2)) {
                                                        end=timer1.read();
                                                        accelerometer1();
                                                        wait_ms(10);

                                                        if (end>1.5) {
                                                                        while (t>0.3) { accelerometer1(); }
                                                                        y1=-5.0;
                                                                        t=-7.0;
                                                                        end=0;
                                                                        timer1.reset();
                                                                        break;
                                                                     }
                                                                     
                                                        }
                                
                                
                                if (  (y1+1.7)>(l+2.0) ) {                 
                   
                                accelerometer2();                      
                                timer2.reset();
                                timer2.start();

                                while((t1+1.85)<(l+2)) {  
                                                        accelerometer2();
                                                        wait_ms(10);
                                                        end1=timer2.read();
                                                        ym=2;
                                                        if (end1>1.5) {
                                                                        y1=-5;
                                                                        ym=5;
                                                                        t1=5;
                                                                        break;
                                                                      }
                                                      }  
                                                      
                                                      }
                        }
                               
        /* HEAD left MOVEMENT */
       if ((x1+1.7) > (k+2)) {
                                timer1.reset();
                                timer1.start();
                                accelerometer1();
                                wait_ms(10);
            
                                while((s+1.85)>(k+2)) {
                                                        end=timer1.read();
                                                        accelerometer1();
                                                        wait_ms(10);
                                                        
                                                        if (end>1.5) {
                                                                        while (s>0.3) { accelerometer1();  }
                                                                        x1=-5.0;
                                                                        s=-7.0;
                                                                        end=0;
                                                                        timer1.reset();
                                                                        break;
                                                                     }
                                                                    
                                                        }
                                
                                  
                                if (  (x1+1.7)>(k+2.0) ) {                 
                                                   
                                accelerometer2();                      
                                timer2.reset();
                                timer2.start();
                                while((s1+1.85)<(k+2)) {  
                                                        accelerometer2();
                                                        wait_ms(10);
                                                        end1=timer2.read();
                                                        ym=4;
                                                        if (end1>1.5) {
                                                                        x1=-5;
                                                                        ym=5;
                                                                        s1=5;
                                                                        break;
                                                                      }
                                                      }  
                                                      
                                                      }
                        }
                        
        /* HEAD right MOVEMENT */
        timer1.reset();
        if ((x+2) < (k+1.7)) {
                                
                                timer1.start();
                                accelerometer1();
                                wait_ms(10);
            
                                while((s+2)<(k+1.85)) {
                                                        end=timer1.read();
                                                        accelerometer1();
                                                        wait_ms(10);
                                                        if (end>1.5) {
                                                                        while (s<-0.15) { accelerometer1();  }
                                                                        x=5.0;
                                                                        s=7.0;
                                                                        end=0;
                                                                        timer1.reset();
                                                                        break;
                                                                     }
                                                                     
                                                        }
                                
                                if (  (x+2)< (k+1.7) ) {                 
                                                   
                                accelerometer2();                      
                                timer2.reset();
                                timer2.start();
                                
                                while((s1+2)>(k+1.85)) {  
                                                        accelerometer2();
                                                        wait_ms(10);
                                                        end1=timer2.read();
                                                        ym=3;
                                                        if (end1>1.5) {
                                                                        x=5;
                                                                        ym=5;
                                                                        k=5;
                                                                        break;
                                                                      }
                                                      }  
                                                      
                                                      }
                        }
          
        if (ym==1){ pc.printf("head infront\n\r"); ym=5; wait(2.0); }
        if (ym==2){ pc.printf("head back\n\r");    ym=5; wait(2.0); }
        if (ym==3){ pc.printf("head right\n\r");   ym=5; wait(2.0); }
        if (ym==4){ pc.printf("head left\n\r");    ym=5; wait(2.0); }
        
                
        if (i==2000) {
            led=0;
            break;
                     }

    }

}