File content as of revision 1:cc3c774c218a:

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

ZMU9250 a;
Serial pc(D1, D0);
Serial mas(D8,D2);
Ticker flipper;

char dete[50];
 int indx=0;
 int ii;
bool anach = false;

char data[50];
char elem;

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);
int sata0(int neww);
int roo(int a1);

int Rotate_Roll, Rotate_Pitch, Rotate_Yaw, A, B, C ;
int rmap[5][5][5];
int map[5][5][5];
char yaw_buf1 = 0;
char yaw_buf2 = 0;
char yaw_buf3 = 0;
char inttost[50];

DigitalIn op_roll(USER_BUTTON);
 int Cross(int degree)
        {
            if (degree == 0)
            {
                return 1;
            }else if (degree == 180)
            {
                return -1;
            }else
            {
                return 0;   
            }
        }
    
        int Zine(int degree)
        {
            if (degree == 90)
            {
                return 1;
            }else if (degree == -90)
            {
                return -1;
            }else
            {
                return 0;   
            }
        }
void Sent(char arrayi[],float it,int siz)

{
    for (int i = 0;i<siz;i++)
        {
            pc.printf("%c",arrayi[i]);
            mas.putc(arrayi[i]);
            wait(it);
        }   
}


void getStr()
{
    pc.printf("x\n");
    while (mas.readable())
    {
        
        elem = mas.getc();
        pc.printf("char is %d\n\r",elem);
        if ((elem == '\n'))
        {
            pc.printf("string is %s\n\r",data);   
            for(int i=0;i<indx;i++)
            {
                if (indx > 2)
                {
                    dete[i] = data[i];      
                }
            }
            for(int i=0;i<30;i++)
            {
                data[i] = '\0';
            }
            indx = 0;
            break;
        }else
        {
            data[indx] = elem;
            indx += 1;
        }
            
    }  
}

 void Rotate(int role,int pit,int yaw)
        {pc.printf("role %d\t pit %d\t yaw %d\n\n\n\r",role , pit ,yaw);
            int i=2,j=3,k=4;
            if ((role == 90) || (role == -90))
            {
                /*for (int i=0;i<5;i++)
                {
                    for (int j=0;j<5;j++)
                    {
                        for(int k=0;k<5;k++)
                        {*/
                          A =  ((i-2)*Cross(pit))-((k-2)*Zine(pit) )+2;
                          B =  ((j-2)*Cross(yaw))-((i-2)*Zine(pit)*Zine(yaw))-((k-2)*Zine(yaw)*Cross(pit) )+2;
                          C =  ((j-2)*Zine(yaw))+((i-2)*Cross(yaw)*Zine(pit))+((k-2)*Cross(yaw)*Cross(pit) )+2;
                          pc.printf("from %d %d %d to %d %d %d\t=1  \n\n\n\r",i,j,k,A,B,C);
                            
                       // }   
                    //}   
                //}
            }else if ((pitch == 90) || (pitch == -90))
            {
                /*for (int i=0;i<5;i++)
                {
                    for (int j=0;j<5;j++)
                    {
                        for(int k=0;k<5;k++)
                        {*/
                           A =  ((i-2)*Cross(yaw))+((j-2)*Zine(role)*Zine(yaw))+((k-2)*Zine(yaw)*Cross(role))+2;
                           B =  ((j-2)*Cross(role))-((k-2)*Zine(role))+2;
                           C =  -((i-2)*Zine(yaw))+((j-2)*Cross(yaw)*Zine(role))+((k-2)*Cross(yaw)*Cross(role))+2;
                           pc.printf("from %d %d %d to %d %d %d\t=2  \n\n\n\r",i,j,k,A,B,C);
                            
                       // }   
                    //}   
               // } 
            }else
            {
                /*for (int i=0;i<5;i++)
                {
                    for (int j=0;j<5;j++)
                    {
                        for(int k=0;k<5;k++)
                        {*/
                            A =  ((i-2)*Cross(yaw))-((j-2)*Zine(yaw))+2;
                            B =  ((i-2)*Zine(yaw))+((j-2)*Cross(yaw))+2;
                            C =  k;
                            pc.printf("from %d %d %d to %d %d %d\t=3  \n\n\n\r",i,j,k,A,B,C);
                            
                        //}   
                    //}   
               // } 
            }
            for (int i=0;i<5;i++)
                {
                    for (int j=0;j<5;j++)
                    {
                        for(int k=0;k<5;k++)
                        {
                           
                            map[i][j][k]= rmap[i][j][k];
                            
                        }   
                    }   
                }
        }



int main(){

flipper.attach(&getStr,1.0);
    while(1){
        pc.printf("ddddd");
        
        wait(0.1);
        a.Update();
        yaw_z = a.Yaw();
        pitch_y = a.Pitch();
        roll_x = a.Roll();
  //      pc.printf("data %d\n",dete);
//        pc.printf("data %c\n",yaw_buf);
        
        
        
         if ((dete[0]=='h')||(dete[1]=='e')||(dete[2]=='l')||(dete[3]=='y')){
              
              //Sent("999\n",0.1,5);
              
              sprintf(inttost,"%d",int(yaw_z));
              //yaw_buf1=int((int(yaw_z)%1000)/100)+48;
//              yaw_buf2=int((int(yaw_z)%100)/10)+48;
//              yaw_buf3=int((int(yaw_z)%10))+48;
//              Sent("hely",0.1,4);
//              Sent(yaw_buf1,0.1,1);
//              Sent(yaw_buf2,0.1,1);
              Sent(inttost,0.1,5);
//              pc.printf("data %c\n",yaw_buf);
              Sent("\n",0.1,1);
            
            }
        
        
        pc.printf("Yaw_z %f\t Pitch_y %f\t Roll_x %f\n\n\n\r", yaw_z, pitch_y, roll_x);
        
//l = cube1(3,3,3);
        //l = roo(10);    
         //Rotate(Rotate_Roll, Rotate_Pitch, Rotate_Yaw);
        //pc.printf("%d\t Yaw_z %f\t Pitch_y %f\t Roll_x %f\n\n\n\r",l , yaw_z, pitch_y, roll_x);  //add
        //pc.printf("%d\t sata(Yaw_z) %d\t sata0(pitch_y) %d\t sata0(Roll_x) %d\n\n\n\r",l , sata0(yaw_z), sata0(pitch_y), sata0(roll_x));
        
        //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 > -40) && (yaw_z < 0)) &&  ((roll_x > -30) && (roll_x < 30)) && ((pitch_y > -20) && (pitch_y < 30))  ){
                    x++; //R x+
                    xlr = 1;
                } 
                 else if( ((yaw_z > -105) && (yaw_z < -75))&& ((roll_x > -20) && (roll_x < 35)) && ((pitch_y > -20) && (pitch_y < 30)) ){
                    y--;  //B y+
                    yfb = -1;
                }
                 else if( ((yaw_z > 140) && (yaw_z < 175)) && ((roll_x > -40) && (roll_x < 25)) && ((pitch_y > -35) && (pitch_y < 25))  ){
                    y++; //F y-
                    yfb = 1;
                
                }
                else if( ((yaw_z > -155) && (yaw_z < -130)) && ((roll_x > -25) && (roll_x < 10)) && ((pitch_y > -20) && (pitch_y < 15))  ){
                    x--; //L x-
                    xlr = -1;
                 
                }
                else if( ((yaw_z > -150) && (yaw_z < -88)) && ((roll_x > 70) && (roll_x < 110)) && ((pitch_y > -20) && (pitch_y < 35))){
                    z++; //T z+
                    ztd = 1;
                    
                }
                else if( ((yaw_z > -125) && (yaw_z < -100)) && ((roll_x > -100) && (roll_x < -75)) && ((pitch_y > -25) && (pitch_y < 35))){
                    z--; //D z-
                    ztd = -1;
                   
                }
                
                else if( ((yaw_z > -80) && (yaw_z < -40)) && ((roll_x > 160) || (roll_x < -160)) && ((pitch_y > -15) && (pitch_y < 15))){
                    y++; 
                    yfb = 1;
                   
                }
                else if( ((yaw_z > 160) || (yaw_z < -165)) &&  ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -10) && (pitch_y < 10))  ){
                    x++; 
                    xlr = 1;
                    }
                else if( ((yaw_z > 100) && (yaw_z < 130)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -20) && (pitch_y < 15))){
                    y--; 
                    yfb = -1;
                    }
                else if( ((yaw_z > 45) && (yaw_z < 65)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -15) && (pitch_y < 15))  ){
                    x--; //L x-
                    xlr = -1;
                 
                }
        int rt_x , rt_y , rt_z;
        int a = rt_x - 3;
        int b = rt_y - 3;
        int c = rt_z - 3;
        if(rt == 'x'){
            rt_x = a + 3;   //x
            rt_y = (b * cos(sata*(PI/180))+ c * sin(sata*(PI/180)) )+ 3;
            rt_z = (-b * sin(sata*(PI/180))+ c * cos(sata*(PI/180)) ) + 3;
        }
        if(rt == 'y'){
            rt_x = (a * cos(sata*(PI/180)) - c * sin(sata*(PI/180)) )+ 3;    // y
            rt_y = b + 3;
            rt_z = (a * sin(sata*(PI/180)) + c * cos(sata*(PI/180)) )+ 3;
        }
        if(rt == 'z'){
            rt_x = (a * cos(sata*(PI/180))+ b * sin(sata*(PI/180)) )+ 3;    //z
            rt_y = (b * cos(sata*(PI/180))- a * sin(sata*(PI/180)) )+ 3;
            rt_z = c + 3;
        }
            
          
       
       
       return (x*100+y*10+z); 
    }
    int sata0(int neww){
        if(neww>=0){
            if((neww <= 45) && (neww >= 0)){
                return 0;
                }
            if((neww <= 90) && (neww >= 46)){
                return 90;
                }
            if((neww <= 135) && (neww >= 91)){
                return -90;
                }
            if((neww <= 180) && (neww >= 136)){
                return 180;
                }
            }
        else{
            if((neww <= 0) && (neww >= -45)){
                return 180;
                }
            if((neww <= -46) && (neww >= -90)){
                return -90;
                }
            if((neww <= -91) && (neww >= -135)){
                return 90;
                }
            if((neww <= -136) && (neww >= -180)){
                return 0;
                }
            }
        }
    
    
     int roo(int a1){
         
      
     //if( ((yaw_z > -40) && (yaw_z < 0)) &&  ((roll_x > -30) && (roll_x < 30)) && ((pitch_y > -20) && (pitch_y < 30))  ){
//                    //R x+     
//                     Rotate_Roll = roll_x;    
//                    Rotate_Pitch = pitch_y;
//                    Rotate_Yaw = yaw_z;
//                    
//                   
//                } 
                if ( ((yaw_z > -150) && (yaw_z < -110))&& ((roll_x > 60) && (roll_x < 100)) && ((pitch_y > -10) && (pitch_y < 10)) ){
                  
                  Rotate_Roll = 0;         // 0,90,2
                    Rotate_Pitch = sata0(pitch_y);
                    Rotate_Yaw = 180;
                     pc.printf("0,90,2 \n\n\n\r");
                 }
                 else if ( ((yaw_z > -140) && (yaw_z < -100))&& ((roll_x > -100) && (roll_x < -60)) && ((pitch_y > -10) && (pitch_y < 10)) ){
                      Rotate_Roll = 0;    // 0,-90,2
                    Rotate_Pitch = sata0(pitch_y);
                    Rotate_Yaw = 180;
                     pc.printf("0,-90,2 \n\n\n\r");
                     }
                else if( ((yaw_z > 60) && (yaw_z < 100))&& ((roll_x > 60) && (roll_x < 100)) && ((pitch_y > -10) && (pitch_y < 10)) ){
                      Rotate_Roll = 180;    // 180,90,2
                    Rotate_Pitch = sata0(pitch_y);
                    Rotate_Yaw = 0;
                     pc.printf("180,90,2 \n\n\n\r");
                     }
                else if ( ((yaw_z > 50) && (yaw_z < 90))&& ((roll_x > -110) && (roll_x < -70)) && ((pitch_y > -15) && (pitch_y < 15)) ){
                      Rotate_Roll = 180;    // 180,-90,2
                    Rotate_Pitch = sata0(pitch_y);
                    Rotate_Yaw = 0;
                     pc.printf("180,-90,2 \n\n\n\r");
                     }
                else if( ((a1 > 0) && (a1 < 9999)) && ((pitch_y > -85) && (pitch_y < -45)) ){
                      Rotate_Roll = sata0(roll_x);    // 90,0,3
                    Rotate_Pitch = sata0(pitch_y);
                    Rotate_Yaw = sata0(yaw_z);
                     pc.printf("90,0,3 \n\n\n\r");
                     }
                else if( ((a1 > 0) && (a1 < 9999)) && ((pitch_y > 60) && (pitch_y < 85)) ){
                      Rotate_Roll = sata0(roll_x);    // -90,0,3
                    Rotate_Pitch = sata0(pitch_y);
                    Rotate_Yaw = sata0(yaw_z);
                     pc.printf("-90,0,3 \n\n\n\r");
                     }
                else if( ((a1> 0) && (a1 < 9999)) && ((pitch_y > -85) && (pitch_y < -45)) ){
                      Rotate_Roll = sata0(roll_x);    // 90,180,3
                    Rotate_Pitch = sata0(pitch_y);
                    Rotate_Yaw = sata0(yaw_z);
                     pc.printf("90,180,3 \n\n\n\r");
                     }
                 else if( ((a1 > 0) && (a1 < 9999)) && ((pitch_y > 60) && (pitch_y < 85)) ){
                      Rotate_Roll = sata0(roll_x);    // 90,180,3
                    Rotate_Pitch = sata0(pitch_y);
                    Rotate_Yaw = sata0(yaw_z);
                     pc.printf("90,180,3 \n\n\n\r");
                     }
                 else if( ((yaw_z > -105) && (yaw_z < -75))&& ((roll_x > -20) && (roll_x < 35)) && ((pitch_y > -20) && (pitch_y < 30)) ){
                     //B y+ 180,180,1
                      Rotate_Roll = sata0(roll_x);
                    Rotate_Pitch = sata0(pitch_y);
                    Rotate_Yaw = sata0(yaw_z);
                    pc.printf("180,180,1 \n\n\n\r");
                }
                 else if( ((yaw_z > 140) && (yaw_z < 175)) && ((roll_x > -40) && (roll_x < 25)) && ((pitch_y > -35) && (pitch_y < 25))  ){
                     //F y- 0,0,1
                    
                    Rotate_Roll = sata0(roll_x);
                    Rotate_Pitch = sata0(pitch_y);
                    Rotate_Yaw = sata0(yaw_z);
                    pc.printf("0,0,1 \n\n\n\r");
                }
               // else if( ((yaw_z > -155) && (yaw_z < -130)) && ((roll_x > -25) && (roll_x < 10)) && ((pitch_y > -20) && (pitch_y < 15))  ){
//                    //L x-  180,180,1
//                   
//                     Rotate_Roll = roll_x;
//                    Rotate_Pitch = pitch_y;
//                    Rotate_Yaw = yaw_z;
//                     pc.printf("180,180,1 \n\n\n\r");
//                 
//                }
                //else if( ((yaw_z > -150) && (yaw_z < -88)) && ((roll_x > 70) && (roll_x < 110)) && ((pitch_y > -20) && (pitch_y < 35))){
//                    z++; //T z+
//                    ztd = 1;
//                     Rotate_Roll = roll_x
//                    Rotate_Pitch = pitch_y
//                    Rotate_Yaw = yaw_z
//                    
//                }
//                else if( ((yaw_z > -125) && (yaw_z < -100)) && ((roll_x > -100) && (roll_x < -75)) && ((pitch_y > -25) && (pitch_y < 35))){
//                    z--; //D z-
//                    ztd = -1;
//                    Rotate_Roll = roll_x
//                    Rotate_Pitch = pitch_y
//                    Rotate_Yaw = yaw_z
//                }
//                
                else if( ((yaw_z > -80) && (yaw_z < -40)) && ((roll_x > 160) || (roll_x < -160)) && ((pitch_y > -15) && (pitch_y < 15))){
                    //180,0,4
                     Rotate_Roll = sata0(roll_x);
                    Rotate_Pitch = sata0(pitch_y);
                    Rotate_Yaw = sata0(yaw_z);
                    pc.printf("180,0,4");
                   
                }
//                else if( ((yaw_z > 160) || (yaw_z < -165)) &&  ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -10) && (pitch_y < 10))  ){
//                    x++; 
//                    xlr = 1;
//                     Rotate_Roll = roll_x
//                    Rotate_Pitch = pitch_y
//                    Rotate_Yaw = yaw_z
//                    }
                else if( ((yaw_z > 100) && (yaw_z < 130)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -20) && (pitch_y < 15))){
                    //0,180,4
                     Rotate_Roll = sata0(roll_x);
                    Rotate_Pitch = sata0(pitch_y);
                    Rotate_Yaw = sata0(yaw_z);
                    pc.printf("0,180,4");
                    }
//                else if( ((yaw_z > 45) && (yaw_z < 65)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -15) && (pitch_y < 15))  ){
//                    x--; //L x-
//                    xlr = -1;
//                     Rotate_Roll = roll_x
//                    Rotate_Pitch = pitch_y
//                    Rotate_Yaw = yaw_z
//                 
//                }
                
            return (1); 
            
        }