bmu.h

00001 #include "mbed.h"
00002 #include "zmu9250.h"
00003 #include "math.h"
00004 
00005 Serial aaa (USBTX,USBRX);
00006 
00007 
00008 class bmuimu
00009 
00010     {
00011         
00012     private:
00013             ZMU9250 a;
00014               
00015         
00016     public:
00017               
00018             int yaw_z,roll_x,pitch_y,stard_x,stard_y,stard_z;
00019             int X_pos , Y_pos , Z_pos ;
00020         bool bmuimu:: Getposition(int x, int y, int z){
00021                 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))  ){
00022                     x++; //R x+
00023                     
00024                 } 
00025                  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)) ){
00026                     y--;  //B y+
00027                  
00028                 }
00029                  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))  ){
00030                     y++; //F y-
00031                 
00032                 
00033                 }
00034                 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))  ){
00035                     x--; //L x-
00036                
00037                  
00038                 }
00039                 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))){
00040                     z++; //T z+
00041                  
00042                 
00043                     
00044                 }
00045                 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))){
00046                     z--; //D z-
00047          
00048                    
00049                 }
00050                 
00051                 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))){
00052                     y++; 
00053       
00054                    
00055                 }
00056                 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))  ){
00057                     x++; 
00058        
00059                     }
00060                 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))){
00061                     y--; 
00062               
00063                     }
00064                 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))  ){
00065                     x--; //L x-
00066        
00067                  
00068                 }
00069                 else
00070                 {
00071                     return false;
00072                 }
00073                 this->X_pos = x;
00074                 this->Y_pos = y;
00075                 this->Z_pos = z;
00076                 return true ;
00077          }   
00078     
00079       void bmuimu::bmuupdate() {
00080                 this->a.Update();
00081                 this->yaw_z = this->a.Yaw();
00082                 this->pitch_y = this->a.Pitch();
00083                 this->roll_x = this->a.Roll();
00084                 aaa.printf("%d , %d and %d\n\r",this->roll_x,this->pitch_y,this->yaw_z);
00085                 
00086        
00087        }
00088        
00089        
00090     int bmuimu::GetX()
00091     {
00092         return X_pos;   
00093     }
00094     
00095     int bmuimu::GetY()
00096     {
00097         return Y_pos;   
00098     }
00099     
00100     int bmuimu::GetZ()
00101     {
00102         return Z_pos;   
00103     }
00104     
00105     int bmuimu::Rotate(){
00106   
00107         if(this->yaw_z >= 0){
00108             if((this->yaw_z <= 45) && (this->yaw_z >= 0)){
00109                 return 0;
00110                 }
00111             if((this->yaw_z <= 90) && (this->yaw_z >= 46)){
00112                 return 90;
00113                 }
00114             if((this->yaw_z <= 135) && (this->yaw_z >= 91)){
00115                 return -90;
00116                 }
00117             if((this->yaw_z <= 180) && (this->yaw_z >= 136)){
00118                 return 180;
00119                 }
00120             }
00121         else{
00122             if((this->yaw_z <= 0) && (this->yaw_z >= -45)){
00123                 return 180;
00124                 }
00125             if((this->yaw_z <= -46) && (this->yaw_z >= -90)){
00126                 return -90;
00127                 }
00128             if((this->yaw_z <= -91) && (this->yaw_z >= -135)){
00129                 return 90;
00130                 }
00131             if((this->yaw_z <= -136) && (this->yaw_z >= -180)){
00132                 return 0;
00133                 }
00134         }
00135     }
00136     
00137     void bmuimu:: stard(){
00138         this->stard_x = this->roll_x;
00139         this->stard_y = this->pitch_y;
00140         this->stard_z = this->yaw_z;
00141         }
00142     
00143     
00144     
00145     
00146     
00147     
00148     };//class