kor bork wa cop koy ma

Dependencies:   mbed

Fork of testIMU2_copy2 by OX

Files at this revision

API Documentation at this revision

Comitter:
csggreen
Date:
Thu Dec 07 08:04:06 2017 +0000
Parent:
2:6bc2c5d68446
Commit message:
copy koy ma

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 6bc2c5d68446 -r ee0df78b0dd3 main.cpp
--- a/main.cpp	Sun Dec 04 09:19:53 2016 +0000
+++ b/main.cpp	Thu Dec 07 08:04:06 2017 +0000
@@ -2,295 +2,19 @@
 #include "zmu9250.h"
 #include "math.h"
 
-ZMU9250 a;
+ZMU9250 axis_Gen;
 Serial pc(D1, D0);
 
-float yaaw_z, piitch_y, rooll_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 rooll ,float piitch, float yaaw);
-int round(int rt_x, int rt_y, int rt_z);
-
-
-DigitalIn op_rooll(USER_BUTTON);
-
-
+float axis_z, axis_y, axis_x;
 int main(){
 
     while(1){
         wait(0.1);
-        a.Update();
-        yaaw_z = a.Yaw();
-        piitch_y = a.Pitch();
-        rooll_x = a.Roll();
-        
-       // pc.printf("yaaw_z %f\t piitch_y %f\t rooll_x %f\n\n\n\r", yaaw_z, piitch_y, rooll_x);
-
-//l = cube1(3,3,3);
-//         
-//        pc.printf("%d\t yaaw_z %f\t piitch_y %f\t rooll_x %f\n\n\n\r",l , yaaw_z, piitch_y, rooll_x);  //add
-//         
-        pc.printf("%d\t yaaw_z %f\t piitch_y %f\t rooll_x %f\t %d\t %c\t %d\t xlr %d\t yfb %d\t ztd %d\t \n\n\n\r ",round(5,5,5), yaaw_z, piitch_y, rooll_x, sata, rt, l,xlr ,yfb, ztd);                  //round
-        
-      //  pc.printf( "%d\n" ,seccon(rooll_x, piitch_y, yaaw_z));
-      //  pc.printf("========\n");  
-                                     //it
-        if(op_rooll == 0){
-            xlr = 0;
-            yfb = 0;
-            ztd = 0;
-         l = cube1(3,3,3);
-            }
-        
-        
+        axis_Gen.Update();
+        axis_z = axis_Gen.Yaw();
+        axis_y = axis_Gen.Pitch();
+        axis_x = axis_Gen.Roll();   
+        pc.printf(" axis_x %f\t axis_y %f\t axis_z %f\n\n\n\r", axis_x, axis_y, axis_z);         
      }
- }
- 
-      int cube1(int x, int y, int z){
-        // vCC LEFT
-           
-             if( ((yaaw_z > 36) && (yaaw_z < 113)) && ( ((rooll_x > -40) && (rooll_x < 40)) )  ){
-                x++; //R
-                xlr = 1;
-            }
-             if( ((yaaw_z > -74) && (yaaw_z < 35)) && ( ((rooll_x > -40) && (rooll_x < 40)) )  ){
-                y--;  //B
-                yfb = -1;
-            }
-             if( ((yaaw_z > 110) && (yaaw_z < 179)) &&( ((rooll_x > -40) && (rooll_x < 40)) )  ){
-                y++; //F
-                yfb = 1;
-            }
-            if( ((yaaw_z > -180) && (yaaw_z < -75)) &&( ((rooll_x > -40) && (rooll_x < 40)) )  ){
-                x--; //L
-                xlr = -1;
-            }
-            if((rooll_x > 41) && (rooll_x < 130) ){
-                z++; //T
-                ztd = 1;
-            }
-            if( (rooll_x > -130) && (rooll_x < -41)){
-                z--; //D
-                ztd = -1;
-            }
-       
-       return (x*100+y*10+z); 
-    }
-    
-    
-    
-    int seccon(float rooll ,float piitch, float yaaw){
-        if(   ( ((yaaw > -50) && (yaaw < -30)) || ((yaaw > -145) && (yaaw < -125)) || ((yaaw > 140) && (yaaw < 160))||((yaaw > 0) && (yaaw < -20)) )
-                 && ((rooll_x > -10 )&& (rooll_x < 10))
-                && (( piitch > -10 )&&(piitch < 10))  //L T R D
-            ){
-            ir = rooll;
-            ip = piitch;
-            iy = yaaw;
-            return 1;
-            }
-        else{
-             return 0;   
-                }
-        }
-        
-        
-    int round(int rt_x, int rt_y, int rt_z){
-              
-           if( (piitch_y > -30) && (piitch_y < 30) ){    // vCC L leg D
-                 
-                 if( ((yaaw_z > 36) && (yaaw_z < 113)) && ( ((rooll_x > -40) && (rooll_x < 40)) )  ){
-                   //R x+
-                    if(xlr == 1){
-                        sata =  0;
-                        rt = 'z';
-                        }
-                    else if(yfb == -1){
-                        sata = 90; 
-                        rt = 'z';
-                        }
-                    else if(yfb == 1){
-                        sata = 270;
-                        rt = 'z';
-                        }
-                    else if(xlr == -1){
-                        sata = 180;
-                        rt = 'z';
-                        }
-                    else if(ztd == 1){
-                        sata = 270;       //x
-                        rt = 'x';
-                        }
-                    else if(ztd == -1){
-                        sata = 90;        //x
-                        rt ='x';
-                        }
-                 }
-                 if( ((yaaw_z > -74) && (yaaw_z < 35)) && ( ((rooll_x > -40) && (rooll_x < 40)) )  ){
-                   //B y+
-                    if(xlr == 1){
-                        sata =  270;
-                        rt = 'z';
-                        }
-                    else if(yfb == -1){
-                        sata = 0;
-                        rt = 'z'; 
-                        }
-                    else if(yfb == 1){
-                        sata = 180;
-                        rt = 'z';
-                        }
-                    else if(xlr == -1){
-                        sata = 90;
-                        rt = 'z';
-                        }
-                    else if(ztd == 1){
-                        sata = 270; //x
-                        rt = 'x';
-                        }
-                    else if(ztd == -1){
-                        sata = 90; //x
-                        rt = 'x';
-                        }
-                }
-                 if( ((yaaw_z > 110) && (yaaw_z < 179)) &&( ((rooll_x > -40) && (rooll_x < 40)) )  ){
-                    //F y-
-                    if(xlr == 1){
-                        sata =  90;
-                        rt = 'z';
-                        }
-                    else if(yfb == -1){
-                        sata = 180;
-                        rt = 'z';
-                        }
-                    else if(yfb == 1){
-                        sata = 0;
-                        rt = 'z';
-                        }
-                    else if(xlr == -1){
-                        sata = 270;
-                        rt = 'z';
-                        }
-                    else if(ztd == 1){
-                        sata = 90;
-                        rt = 'x';
-                        }
-                    else if(ztd == -1){
-                        sata = 270;
-                        rt = 'x';
-                        }
-                }
-                if( ((yaaw_z > -180) && (yaaw_z < -75)) &&( ((rooll_x > -40) && (rooll_x < 40)) )  ){
-                    //L x-
-                    if(xlr == 1){
-                        sata =  180;
-                        rt = 'z';
-                        }
-                    else if(yfb == -1){
-                        sata = 270;
-                        rt = 'z';
-                        }
-                    else if(yfb == 1){
-                        sata = 90;
-                        rt = 'z';
-                        }
-                    else if(xlr == -1){
-                        sata = 0;
-                        rt = 'z';
-                        }
-                    else if(ztd == 1){
-                        sata = 90;
-                        rt = 'x';
-                        }
-                    else if(ztd == -1){
-                        sata = 270;
-                        rt = 'x';
-                        }
-                }
-                if((rooll_x > 41) && (rooll_x < 130) ){
-                    //T z+
-                    if(xlr == 1){
-                        sata =  90; //x
-                        rt = 'x';
-                        }
-                    else if(yfb == -1){
-                        sata = 90; //y
-                        rt = 'y';
-                        }
-                    else if(yfb == 1){
-                        sata = 270;
-                        rt = 'y';
-                        }
-                    else if(xlr == -1){
-                        sata = 270;
-                        rt = 'x';
-                        }
-                    else if(ztd == 1){
-                        sata = 0;
-                        rt = 'x';
-                        }
-                    else if(ztd == -1){
-                        sata = 180;     //x
-                        rt = 'x';
-                        }
-                }
-                if( (rooll_x > -130) && (rooll_x < -41)){
-                    //D z-
-                    if(xlr == 1){
-                        sata =  270;
-                        rt = 'x';
-                        }
-                    else if(yfb == -1){
-                        sata = 270;
-                        rt = 'y'; 
-                        }
-                    else if(yfb == 1){
-                        sata = 90;
-                        rt = 'y';
-                        }
-                    else if(xlr == -1){
-                        sata = 90;
-                        rt = 'x';
-                        }
-                    else if(ztd == 1){
-                        sata = 180;
-                        rt = 'x';
-                        }
-                    else if(ztd == -1){
-                        sata = 0 ; //x
-                        rt = 'x';
-                        }
-           
-                }
-            }
-        int k1,k2,rtt_x,rtt_y,rtt_z;
-        k1 = sin(sata*PI/180);
-        k2 = cos(sata*PI/180);
-       rt_x = rt_x - 3;
-       rt_y = rt_y - 3;
-       rt_z = rt_z - 3;
-       if(rt == 'x'){
-       rtt_x = rt_x ;   //x
-       rtt_y = ((rt_y * k2)- (rt_z * k1 ));
-       rtt_z = ((rt_y * k1)+ (rt_z * k2 ));
-       }
-       if(rt == 'y'){
-       rtt_x = ((rt_x * k2) + (rt_z * k1 ));    // y
-       rtt_y = rt_y ;
-       rtt_z = ((-rt_x * k1) + (rt_z * k2 ));
-       }
-       if(rt == 'z'){
-       rtt_x = ((rt_x * k2)- (rt_y * k1 ));    //z
-       rtt_y = ((rt_x * k1)+ (rt_y * k2 ));
-       rtt_z = rt_z;
-        }
-    
-    
-       return (rtt_x*100 + rtt_y*10 + rtt_z + 333); 
-       }
+}
     
\ No newline at end of file