File content as of revision 0:77a7d1a1c6db:

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

ZMU9250 a;
Serial pc(D1, D0);

float yaw_z, pitch_y, roll_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 roll ,float pitch, float yaw);
int round(int rt_x, int rt_y, int rt_z);


DigitalIn op_roll(USER_BUTTON);


int main(){

    while(1){
        wait(0.1);
        a.Update();
        yaw_z = a.Yaw();
        pitch_y = a.Pitch();
        roll_x = a.Roll();
        
//        pc.printf("Yaw_z %f\t Pitch_y %f\t Roll_x %f\n", yaw_z, pitch_y, roll_x);

//l = cube1(3,3,3);
         
        //pc.printf("%d\t Yaw_z %f\t Pitch_y %f\t Roll_x %f\n",l , yaw_z, pitch_y, roll_x);  //add
        
        pc.printf("%d\t Yaw_z %f\t Pitch_y %f\t Roll_x %f\t %d\t %c\t %d\t \r\n ",round(5,5,5), yaw_z, pitch_y, roll_x, sata, rt, l);                  //round
        
      //  pc.printf( "%d\n" ,seccon(roll_x, pitch_y, yaw_z));
//        pc.printf("========\n");                                      //it
        if(op_roll == 0){
            
         l = cube1(3,3,3);
            }
        
        
     }
 }
 
    int cube1(int x, int y, int z){
        
           if( ((pitch_y > -30) && (pitch_y < 30)) ){    // vCC L leg D
                 
                 if( ((yaw_z > 36) && (yaw_z < 113)) &&  ((roll_x > -40) && (roll_x < 40))   ){
                    x++; //R x+
                    xlr = 1;
                    
                
                } 
                 if( ((yaw_z > -101) && (yaw_z < 35)) &&  ((roll_x > -40) && (roll_x < 40))   ){
                    y--;  //B y+
                    yfb = -1;
                }
                 if( ((yaw_z > 110) && (yaw_z < 179)) && ((roll_x > -40) && (roll_x < 40))   ){
                    y++; //F y-
                    yfb = 1;
                
                }
                if( ((yaw_z > -180) && (yaw_z < -100)) && ((roll_x > -40) && (roll_x < 40))   ){
                    x--; //L x-
                    xlr = -1;
                 
                }
                if((roll_x > 41) && (roll_x < 130) ){
                    z++; //T z+
                    ztd = 1;
                    
                }
                if( (roll_x > -130) && (roll_x < -41)){
                    z--; //D z-
                    ztd = -1;
                   
                }
           
            }
          
       
       
       return (x*100+y*10+z); 
    }
    
    
    
    
    int seccon(float roll ,float pitch, float yaw){
        if(   ( ((yaw > -50) && (yaw < -30)) || ((yaw > -145) && (yaw < -125)) || ((yaw > 140) && (yaw < 160))||((yaw > 0) && (yaw < -20)) )
                 && ((roll_x > -10 )&& (roll_x < 10))
                && (( pitch > -10 )&&(pitch < 10))  //L T R D
            ){
            ir = roll;
            ip = pitch;
            iy = yaw;
            return 1;
            }
        else{
             return 0;   
                }
        }
        
        
    int round(int rt_x, int rt_y, int rt_z){
              
           if( (pitch_y > -30) && (pitch_y < 30) ){    // vCC L leg D
                 
                 if( ((yaw_z > 36) && (yaw_z < 113)) && ( ((roll_x > -40) && (roll_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( ((yaw_z > -74) && (yaw_z < 35)) && ( ((roll_x > -40) && (roll_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( ((yaw_z > 110) && (yaw_z < 179)) &&( ((roll_x > -40) && (roll_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( ((yaw_z > -180) && (yaw_z < -75)) &&( ((roll_x > -40) && (roll_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((roll_x > 41) && (roll_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( (roll_x > -130) && (roll_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';
                        }
           
                }
            }
          
       rt_x = rt_x - 3;
       rt_y = rt_y - 3;
       rt_z = rt_z - 3;
       if(rt == 'x'){
       rt_x = rt_x + 3;   //x
       rt_y = (rt_y * cos(sata*(PI/180))+ rt_z * sin(sata*(PI/180)) )+ 3;
       rt_z = (-rt_y * sin(sata*(PI/180))+ rt_z * cos(sata*(PI/180)) ) + 3;
       }
       if(rt == 'y'){
       rt_x = (rt_x * cos(sata*(PI/180)) - rt_z * sin(sata*(PI/180)) )+ 3;    // y
       rt_y = rt_y + 3;
       rt_z = (rt_x * sin(sata*(PI/180)) + rt_z * cos(sata*(PI/180)) )+ 3;
       }
       if(rt == 'z'){
       rt_x = (rt_x * cos(sata*(PI/180))+ rt_y * sin(sata*(PI/180)) )+ 3;    //z
       rt_y = (rt_y * cos(sata*(PI/180))- rt_x * sin(sata*(PI/180)) )+ 3;
       rt_z = rt_z + 3;
        }
    pc.printf("rt_x %d\t", rt_x);
    pc.printf("rt_y %d\t", rt_y);
    pc.printf("rt_z %d \n\n\n\n\r", rt_z);
    
       return (rt_x*100 + rt_y*10 + rt_z); 
       }