File content as of revision 0:a291977ec0b1:

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


ZMU9250 a;
class bmuimu

    {
        
              
        
    public:
              
            int yaw_z,roll_x,pitch_y,stard_x,stard_y,stard_z;
            int X_pos , Y_pos , Z_pos ;
        bool bmuimu:: Getposition(int x, int y, int z){
                if( ((this->yaw_z > -40) && (this->yaw_z < 0)) &&  ((this->roll_x > -30) && (this->roll_x < 30)) && ((this->pitch_y > -20) && (this->pitch_y < 30))  ){
                    x++; //R x+
                    
                } 
                 else if( ((this->yaw_z > -105) && (this->yaw_z < -75))&& ((this->roll_x > -20) && (this->roll_x < 35)) && ((this->pitch_y > -20) && (this->pitch_y < 30)) ){
                    y--;  //B y+
                 
                }
                 else if( ((this->yaw_z > 140) && (this->yaw_z < 175)) && ((this->roll_x > -40) && (this->roll_x < 25)) && ((this->pitch_y > -35) && (this->pitch_y < 25))  ){
                    y++; //F y-
                
                
                }
                else if( ((this->yaw_z > -155) && (this->yaw_z < -130)) && ((this->roll_x > -25) && (this->roll_x < 10)) && ((this->pitch_y > -20) && (this->pitch_y < 15))  ){
                    x--; //L x-
               
                 
                }
                else if( ((this->yaw_z > -150) && (this->yaw_z < -88)) && ((this->roll_x > 70) && (this->roll_x < 110)) && ((this->pitch_y > -20) && (this->pitch_y < 35))){
                    z++; //T z+
                 
                
                    
                }
                else if( ((this->yaw_z > -125) && (this->yaw_z < -100)) && ((this->roll_x > -100) && (this->roll_x < -75)) && ((this->pitch_y > -25) && (this->pitch_y < 35))){
                    z--; //D z-
         
                   
                }
                
                else if( ((this->yaw_z > -80) && (this->yaw_z < -40)) && ((this->roll_x > 160) || (this->roll_x < -160)) && ((this->pitch_y > -15) && (this->pitch_y < 15))){
                    y++; 
      
                   
                }
                else if( ((this->yaw_z > 160) || (this->yaw_z < -165)) &&  ((this->roll_x > 165) || (this->roll_x < -165)) && ((this->pitch_y > -10) && (this->pitch_y < 10))  ){
                    x++; 
       
                    }
                else if( ((this->yaw_z > 100) && (this->yaw_z < 130)) && ((this->roll_x > 165) || (this->roll_x < -165)) && ((this->pitch_y > -20) && (this->pitch_y < 15))){
                    y--; 
              
                    }
                else if( ((this->yaw_z > 45) && (this->yaw_z < 65)) && ((this->roll_x > 165) || (this->roll_x < -165)) && ((this->pitch_y > -15) && (this->pitch_y < 15))  ){
                    x--; //L x-
       
                 
                }
                else
                {
                    return false;
                }
                this->X_pos = x;
                this->Y_pos = y;
                this->Z_pos = z;
                return true ;
         }   
    
      void bmuimu::update() {
                
                a.Update();
                this->yaw_z = a.Yaw();
                this->pitch_y = a.Pitch();
                this->roll_x = a.Roll();
       
       }
       
       
    int bmuimu::GetX()
    {
        return X_pos;   
    }
    
    int bmuimu::GetY()
    {
        return Y_pos;   
    }
    
    int bmuimu::GetZ()
    {
        return Z_pos;   
    }
    
    int bmuimu::Rotate(){
  
        if(this->yaw_z >= 0){
            if((this->yaw_z <= 45) && (this->yaw_z >= 0)){
                return 0;
                }
            if((this->yaw_z <= 90) && (this->yaw_z >= 46)){
                return 90;
                }
            if((this->yaw_z <= 135) && (this->yaw_z >= 91)){
                return -90;
                }
            if((this->yaw_z <= 180) && (this->yaw_z >= 136)){
                return 180;
                }
            }
        else{
            if((this->yaw_z <= 0) && (this->yaw_z >= -45)){
                return 180;
                }
            if((this->yaw_z <= -46) && (this->yaw_z >= -90)){
                return -90;
                }
            if((this->yaw_z <= -91) && (this->yaw_z >= -135)){
                return 90;
                }
            if((this->yaw_z <= -136) && (this->yaw_z >= -180)){
                return 0;
                }
        }
    }
    
    void bmuimu:: stard(){
        this->stard_x = this->roll_x;
        this->stard_y = this->pitch_y;
        this->stard_z = this->yaw_z;
        }
    
    
    
    
    
    
    };//class