File content as of revision 2:6bc2c5d68446:

#include "mbed.h"
#include "zmu9250.h"
#include "math.h"

ZMU9250 a;
Serial pc(D1, D0);

float yaaw_z, piitch_y, rooll_x;
int ir,ip,iy;
int l;
int xlr,yfb,ztd;
int sata;
char rt;

int cube1(int x, int y, int z);
int seccon(float rooll ,float piitch, float yaaw);
int round(int rt_x, int rt_y, int rt_z);


DigitalIn op_rooll(USER_BUTTON);


int main(){

    while(1){
        wait(0.1);
        a.Update();
        yaaw_z = a.Yaw();
        piitch_y = a.Pitch();
        rooll_x = a.Roll();
        
       // pc.printf("yaaw_z %f\t piitch_y %f\t rooll_x %f\n\n\n\r", yaaw_z, piitch_y, rooll_x);

//l = cube1(3,3,3);
//         
//        pc.printf("%d\t yaaw_z %f\t piitch_y %f\t rooll_x %f\n\n\n\r",l , yaaw_z, piitch_y, rooll_x);  //add
//         
        pc.printf("%d\t yaaw_z %f\t piitch_y %f\t rooll_x %f\t %d\t %c\t %d\t xlr %d\t yfb %d\t ztd %d\t \n\n\n\r ",round(5,5,5), yaaw_z, piitch_y, rooll_x, sata, rt, l,xlr ,yfb, ztd);                  //round
        
      //  pc.printf( "%d\n" ,seccon(rooll_x, piitch_y, yaaw_z));
      //  pc.printf("========\n");  
                                     //it
        if(op_rooll == 0){
            xlr = 0;
            yfb = 0;
            ztd = 0;
         l = cube1(3,3,3);
            }
        
        
     }
 }
 
      int cube1(int x, int y, int z){
        // vCC LEFT
           
             if( ((yaaw_z > 36) && (yaaw_z < 113)) && ( ((rooll_x > -40) && (rooll_x < 40)) )  ){
                x++; //R
                xlr = 1;
            }
             if( ((yaaw_z > -74) && (yaaw_z < 35)) && ( ((rooll_x > -40) && (rooll_x < 40)) )  ){
                y--;  //B
                yfb = -1;
            }
             if( ((yaaw_z > 110) && (yaaw_z < 179)) &&( ((rooll_x > -40) && (rooll_x < 40)) )  ){
                y++; //F
                yfb = 1;
            }
            if( ((yaaw_z > -180) && (yaaw_z < -75)) &&( ((rooll_x > -40) && (rooll_x < 40)) )  ){
                x--; //L
                xlr = -1;
            }
            if((rooll_x > 41) && (rooll_x < 130) ){
                z++; //T
                ztd = 1;
            }
            if( (rooll_x > -130) && (rooll_x < -41)){
                z--; //D
                ztd = -1;
            }
       
       return (x*100+y*10+z); 
    }
    
    
    
    int seccon(float rooll ,float piitch, float yaaw){
        if(   ( ((yaaw > -50) && (yaaw < -30)) || ((yaaw > -145) && (yaaw < -125)) || ((yaaw > 140) && (yaaw < 160))||((yaaw > 0) && (yaaw < -20)) )
                 && ((rooll_x > -10 )&& (rooll_x < 10))
                && (( piitch > -10 )&&(piitch < 10))  //L T R D
            ){
            ir = rooll;
            ip = piitch;
            iy = yaaw;
            return 1;
            }
        else{
             return 0;   
                }
        }
        
        
    int round(int rt_x, int rt_y, int rt_z){
              
           if( (piitch_y > -30) && (piitch_y < 30) ){    // vCC L leg D
                 
                 if( ((yaaw_z > 36) && (yaaw_z < 113)) && ( ((rooll_x > -40) && (rooll_x < 40)) )  ){
                   //R x+
                    if(xlr == 1){
                        sata =  0;
                        rt = 'z';
                        }
                    else if(yfb == -1){
                        sata = 90; 
                        rt = 'z';
                        }
                    else if(yfb == 1){
                        sata = 270;
                        rt = 'z';
                        }
                    else if(xlr == -1){
                        sata = 180;
                        rt = 'z';
                        }
                    else if(ztd == 1){
                        sata = 270;       //x
                        rt = 'x';
                        }
                    else if(ztd == -1){
                        sata = 90;        //x
                        rt ='x';
                        }
                 }
                 if( ((yaaw_z > -74) && (yaaw_z < 35)) && ( ((rooll_x > -40) && (rooll_x < 40)) )  ){
                   //B y+
                    if(xlr == 1){
                        sata =  270;
                        rt = 'z';
                        }
                    else if(yfb == -1){
                        sata = 0;
                        rt = 'z'; 
                        }
                    else if(yfb == 1){
                        sata = 180;
                        rt = 'z';
                        }
                    else if(xlr == -1){
                        sata = 90;
                        rt = 'z';
                        }
                    else if(ztd == 1){
                        sata = 270; //x
                        rt = 'x';
                        }
                    else if(ztd == -1){
                        sata = 90; //x
                        rt = 'x';
                        }
                }
                 if( ((yaaw_z > 110) && (yaaw_z < 179)) &&( ((rooll_x > -40) && (rooll_x < 40)) )  ){
                    //F y-
                    if(xlr == 1){
                        sata =  90;
                        rt = 'z';
                        }
                    else if(yfb == -1){
                        sata = 180;
                        rt = 'z';
                        }
                    else if(yfb == 1){
                        sata = 0;
                        rt = 'z';
                        }
                    else if(xlr == -1){
                        sata = 270;
                        rt = 'z';
                        }
                    else if(ztd == 1){
                        sata = 90;
                        rt = 'x';
                        }
                    else if(ztd == -1){
                        sata = 270;
                        rt = 'x';
                        }
                }
                if( ((yaaw_z > -180) && (yaaw_z < -75)) &&( ((rooll_x > -40) && (rooll_x < 40)) )  ){
                    //L x-
                    if(xlr == 1){
                        sata =  180;
                        rt = 'z';
                        }
                    else if(yfb == -1){
                        sata = 270;
                        rt = 'z';
                        }
                    else if(yfb == 1){
                        sata = 90;
                        rt = 'z';
                        }
                    else if(xlr == -1){
                        sata = 0;
                        rt = 'z';
                        }
                    else if(ztd == 1){
                        sata = 90;
                        rt = 'x';
                        }
                    else if(ztd == -1){
                        sata = 270;
                        rt = 'x';
                        }
                }
                if((rooll_x > 41) && (rooll_x < 130) ){
                    //T z+
                    if(xlr == 1){
                        sata =  90; //x
                        rt = 'x';
                        }
                    else if(yfb == -1){
                        sata = 90; //y
                        rt = 'y';
                        }
                    else if(yfb == 1){
                        sata = 270;
                        rt = 'y';
                        }
                    else if(xlr == -1){
                        sata = 270;
                        rt = 'x';
                        }
                    else if(ztd == 1){
                        sata = 0;
                        rt = 'x';
                        }
                    else if(ztd == -1){
                        sata = 180;     //x
                        rt = 'x';
                        }
                }
                if( (rooll_x > -130) && (rooll_x < -41)){
                    //D z-
                    if(xlr == 1){
                        sata =  270;
                        rt = 'x';
                        }
                    else if(yfb == -1){
                        sata = 270;
                        rt = 'y'; 
                        }
                    else if(yfb == 1){
                        sata = 90;
                        rt = 'y';
                        }
                    else if(xlr == -1){
                        sata = 90;
                        rt = 'x';
                        }
                    else if(ztd == 1){
                        sata = 180;
                        rt = 'x';
                        }
                    else if(ztd == -1){
                        sata = 0 ; //x
                        rt = 'x';
                        }
           
                }
            }
        int k1,k2,rtt_x,rtt_y,rtt_z;
        k1 = sin(sata*PI/180);
        k2 = cos(sata*PI/180);
       rt_x = rt_x - 3;
       rt_y = rt_y - 3;
       rt_z = rt_z - 3;
       if(rt == 'x'){
       rtt_x = rt_x ;   //x
       rtt_y = ((rt_y * k2)- (rt_z * k1 ));
       rtt_z = ((rt_y * k1)+ (rt_z * k2 ));
       }
       if(rt == 'y'){
       rtt_x = ((rt_x * k2) + (rt_z * k1 ));    // y
       rtt_y = rt_y ;
       rtt_z = ((-rt_x * k1) + (rt_z * k2 ));
       }
       if(rt == 'z'){
       rtt_x = ((rt_x * k2)- (rt_y * k1 ));    //z
       rtt_y = ((rt_x * k1)+ (rt_y * k2 ));
       rtt_z = rt_z;
        }
    
    
       return (rtt_x*100 + rtt_y*10 + rtt_z + 333); 
       }