main.cpp

00001 #include "mbed.h"
00002 #include "zmu9250.h"
00003 #include "math.h"
00004 
00005 class bmuimu
00006 
00007     {
00008     public:
00009             int yaw_z,roll_x,pitch_y,stard_x,stard_y,stard_z;
00010         int bmuimu:: cube(int x, int y, int z){
00011                 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))  ){
00012                     x++; //R x+
00013                     
00014                 } 
00015                  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)) ){
00016                     y--;  //B y+
00017                  
00018                 }
00019                  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))  ){
00020                     y++; //F y-
00021                 
00022                 
00023                 }
00024                 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))  ){
00025                     x--; //L x-
00026                
00027                  
00028                 }
00029                 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))){
00030                     z++; //T z+
00031                  
00032                 
00033                     
00034                 }
00035                 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))){
00036                     z--; //D z-
00037          
00038                    
00039                 }
00040                 
00041                 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))){
00042                     y++; 
00043       
00044                    
00045                 }
00046                 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))  ){
00047                     x++; 
00048        
00049                     }
00050                 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))){
00051                     y--; 
00052               
00053                     }
00054                 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))  ){
00055                     x--; //L x-
00056        
00057                  
00058                 }
00059                 else{
00060                     return 0;
00061                     }
00062             return x*100+y*10+z ;
00063          }   
00064     
00065       void bmuimu::update() {
00066                 ZMU9250 a;
00067                 a.Update();
00068                 this->yaw_z = a.Yaw();
00069                 this->pitch_y = a.Pitch();
00070                 this->roll_x = a.Roll();
00071        
00072        }
00073     
00074     int bmuimu::rotate(int ok_rotate){
00075   
00076         if(this->yaw_z >= 0){
00077             if((this->yaw_z <= 45) && (this->yaw_z >= 0)){
00078                 return 0;
00079                 }
00080             if((this->yaw_z <= 90) && (this->yaw_z >= 46)){
00081                 return 90;
00082                 }
00083             if((this->yaw_z <= 135) && (this->yaw_z >= 91)){
00084                 return -90;
00085                 }
00086             if((this->yaw_z <= 180) && (this->yaw_z >= 136)){
00087                 return 180;
00088                 }
00089             }
00090         else{
00091             if((this->yaw_z <= 0) && (this->yaw_z >= -45)){
00092                 return 180;
00093                 }
00094             if((this->yaw_z <= -46) && (this->yaw_z >= -90)){
00095                 return -90;
00096                 }
00097             if((this->yaw_z <= -91) && (this->yaw_z >= -135)){
00098                 return 90;
00099                 }
00100             if((this->yaw_z <= -136) && (this->yaw_z >= -180)){
00101                 return 0;
00102                 }
00103         }
00104     }
00105     
00106     void bmuimu:: stard(){
00107         this->stard_x = this->roll_x;
00108         this->stard_y = this->pitch_y;
00109         this->stard_z = this->yaw_z;
00110         }
00111     
00112     
00113     
00114     
00115     
00116     
00117     };//class