Diff: main.cpp

Revision:

0:dd400e4fe461

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 05 14:47:17 2016 +0000
@@ -0,0 +1,117 @@
+#include "mbed.h"
+#include "zmu9250.h"
+#include "math.h"
+
+class bmuimu
+
+    {
+    public:
+            int yaw_z,roll_x,pitch_y,stard_x,stard_y,stard_z;
+        int bmuimu:: cube(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 0;
+                    }
+            return x*100+y*10+z ;
+         }   
+    
+      void bmuimu::update() {
+                ZMU9250 a;
+                a.Update();
+                this->yaw_z = a.Yaw();
+                this->pitch_y = a.Pitch();
+                this->roll_x = a.Roll();
+       
+       }
+    
+    int bmuimu::rotate(int ok_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
\ No newline at end of file