OX
/
testIMU2_copy2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "zmu9250.h" 00003 #include "math.h" 00004 00005 ZMU9250 a; 00006 Serial pc(D1, D0); 00007 00008 float yaaw_z, piitch_y, rooll_x; 00009 int ir,ip,iy; 00010 int l; 00011 int xlr,yfb,ztd; 00012 int sata; 00013 char rt; 00014 00015 int cube1(int x, int y, int z); 00016 int seccon(float rooll ,float piitch, float yaaw); 00017 int round(int rt_x, int rt_y, int rt_z); 00018 00019 00020 DigitalIn op_rooll(USER_BUTTON); 00021 00022 00023 int main(){ 00024 00025 while(1){ 00026 wait(0.1); 00027 a.Update(); 00028 yaaw_z = a.Yaw(); 00029 piitch_y = a.Pitch(); 00030 rooll_x = a.Roll(); 00031 00032 // pc.printf("yaaw_z %f\t piitch_y %f\t rooll_x %f\n\n\n\r", yaaw_z, piitch_y, rooll_x); 00033 00034 //l = cube1(3,3,3); 00035 // 00036 // 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 00037 // 00038 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 00039 00040 // pc.printf( "%d\n" ,seccon(rooll_x, piitch_y, yaaw_z)); 00041 // pc.printf("========\n"); 00042 //it 00043 if(op_rooll == 0){ 00044 xlr = 0; 00045 yfb = 0; 00046 ztd = 0; 00047 l = cube1(3,3,3); 00048 } 00049 00050 00051 } 00052 } 00053 00054 int cube1(int x, int y, int z){ 00055 // vCC LEFT 00056 00057 if( ((yaaw_z > 36) && (yaaw_z < 113)) && ( ((rooll_x > -40) && (rooll_x < 40)) ) ){ 00058 x++; //R 00059 xlr = 1; 00060 } 00061 if( ((yaaw_z > -74) && (yaaw_z < 35)) && ( ((rooll_x > -40) && (rooll_x < 40)) ) ){ 00062 y--; //B 00063 yfb = -1; 00064 } 00065 if( ((yaaw_z > 110) && (yaaw_z < 179)) &&( ((rooll_x > -40) && (rooll_x < 40)) ) ){ 00066 y++; //F 00067 yfb = 1; 00068 } 00069 if( ((yaaw_z > -180) && (yaaw_z < -75)) &&( ((rooll_x > -40) && (rooll_x < 40)) ) ){ 00070 x--; //L 00071 xlr = -1; 00072 } 00073 if((rooll_x > 41) && (rooll_x < 130) ){ 00074 z++; //T 00075 ztd = 1; 00076 } 00077 if( (rooll_x > -130) && (rooll_x < -41)){ 00078 z--; //D 00079 ztd = -1; 00080 } 00081 00082 return (x*100+y*10+z); 00083 } 00084 00085 00086 00087 int seccon(float rooll ,float piitch, float yaaw){ 00088 if( ( ((yaaw > -50) && (yaaw < -30)) || ((yaaw > -145) && (yaaw < -125)) || ((yaaw > 140) && (yaaw < 160))||((yaaw > 0) && (yaaw < -20)) ) 00089 && ((rooll_x > -10 )&& (rooll_x < 10)) 00090 && (( piitch > -10 )&&(piitch < 10)) //L T R D 00091 ){ 00092 ir = rooll; 00093 ip = piitch; 00094 iy = yaaw; 00095 return 1; 00096 } 00097 else{ 00098 return 0; 00099 } 00100 } 00101 00102 00103 int round(int rt_x, int rt_y, int rt_z){ 00104 00105 if( (piitch_y > -30) && (piitch_y < 30) ){ // vCC L leg D 00106 00107 if( ((yaaw_z > 36) && (yaaw_z < 113)) && ( ((rooll_x > -40) && (rooll_x < 40)) ) ){ 00108 //R x+ 00109 if(xlr == 1){ 00110 sata = 0; 00111 rt = 'z'; 00112 } 00113 else if(yfb == -1){ 00114 sata = 90; 00115 rt = 'z'; 00116 } 00117 else if(yfb == 1){ 00118 sata = 270; 00119 rt = 'z'; 00120 } 00121 else if(xlr == -1){ 00122 sata = 180; 00123 rt = 'z'; 00124 } 00125 else if(ztd == 1){ 00126 sata = 270; //x 00127 rt = 'x'; 00128 } 00129 else if(ztd == -1){ 00130 sata = 90; //x 00131 rt ='x'; 00132 } 00133 } 00134 if( ((yaaw_z > -74) && (yaaw_z < 35)) && ( ((rooll_x > -40) && (rooll_x < 40)) ) ){ 00135 //B y+ 00136 if(xlr == 1){ 00137 sata = 270; 00138 rt = 'z'; 00139 } 00140 else if(yfb == -1){ 00141 sata = 0; 00142 rt = 'z'; 00143 } 00144 else if(yfb == 1){ 00145 sata = 180; 00146 rt = 'z'; 00147 } 00148 else if(xlr == -1){ 00149 sata = 90; 00150 rt = 'z'; 00151 } 00152 else if(ztd == 1){ 00153 sata = 270; //x 00154 rt = 'x'; 00155 } 00156 else if(ztd == -1){ 00157 sata = 90; //x 00158 rt = 'x'; 00159 } 00160 } 00161 if( ((yaaw_z > 110) && (yaaw_z < 179)) &&( ((rooll_x > -40) && (rooll_x < 40)) ) ){ 00162 //F y- 00163 if(xlr == 1){ 00164 sata = 90; 00165 rt = 'z'; 00166 } 00167 else if(yfb == -1){ 00168 sata = 180; 00169 rt = 'z'; 00170 } 00171 else if(yfb == 1){ 00172 sata = 0; 00173 rt = 'z'; 00174 } 00175 else if(xlr == -1){ 00176 sata = 270; 00177 rt = 'z'; 00178 } 00179 else if(ztd == 1){ 00180 sata = 90; 00181 rt = 'x'; 00182 } 00183 else if(ztd == -1){ 00184 sata = 270; 00185 rt = 'x'; 00186 } 00187 } 00188 if( ((yaaw_z > -180) && (yaaw_z < -75)) &&( ((rooll_x > -40) && (rooll_x < 40)) ) ){ 00189 //L x- 00190 if(xlr == 1){ 00191 sata = 180; 00192 rt = 'z'; 00193 } 00194 else if(yfb == -1){ 00195 sata = 270; 00196 rt = 'z'; 00197 } 00198 else if(yfb == 1){ 00199 sata = 90; 00200 rt = 'z'; 00201 } 00202 else if(xlr == -1){ 00203 sata = 0; 00204 rt = 'z'; 00205 } 00206 else if(ztd == 1){ 00207 sata = 90; 00208 rt = 'x'; 00209 } 00210 else if(ztd == -1){ 00211 sata = 270; 00212 rt = 'x'; 00213 } 00214 } 00215 if((rooll_x > 41) && (rooll_x < 130) ){ 00216 //T z+ 00217 if(xlr == 1){ 00218 sata = 90; //x 00219 rt = 'x'; 00220 } 00221 else if(yfb == -1){ 00222 sata = 90; //y 00223 rt = 'y'; 00224 } 00225 else if(yfb == 1){ 00226 sata = 270; 00227 rt = 'y'; 00228 } 00229 else if(xlr == -1){ 00230 sata = 270; 00231 rt = 'x'; 00232 } 00233 else if(ztd == 1){ 00234 sata = 0; 00235 rt = 'x'; 00236 } 00237 else if(ztd == -1){ 00238 sata = 180; //x 00239 rt = 'x'; 00240 } 00241 } 00242 if( (rooll_x > -130) && (rooll_x < -41)){ 00243 //D z- 00244 if(xlr == 1){ 00245 sata = 270; 00246 rt = 'x'; 00247 } 00248 else if(yfb == -1){ 00249 sata = 270; 00250 rt = 'y'; 00251 } 00252 else if(yfb == 1){ 00253 sata = 90; 00254 rt = 'y'; 00255 } 00256 else if(xlr == -1){ 00257 sata = 90; 00258 rt = 'x'; 00259 } 00260 else if(ztd == 1){ 00261 sata = 180; 00262 rt = 'x'; 00263 } 00264 else if(ztd == -1){ 00265 sata = 0 ; //x 00266 rt = 'x'; 00267 } 00268 00269 } 00270 } 00271 int k1,k2,rtt_x,rtt_y,rtt_z; 00272 k1 = sin(sata*PI/180); 00273 k2 = cos(sata*PI/180); 00274 rt_x = rt_x - 3; 00275 rt_y = rt_y - 3; 00276 rt_z = rt_z - 3; 00277 if(rt == 'x'){ 00278 rtt_x = rt_x ; //x 00279 rtt_y = ((rt_y * k2)- (rt_z * k1 )); 00280 rtt_z = ((rt_y * k1)+ (rt_z * k2 )); 00281 } 00282 if(rt == 'y'){ 00283 rtt_x = ((rt_x * k2) + (rt_z * k1 )); // y 00284 rtt_y = rt_y ; 00285 rtt_z = ((-rt_x * k1) + (rt_z * k2 )); 00286 } 00287 if(rt == 'z'){ 00288 rtt_x = ((rt_x * k2)- (rt_y * k1 )); //z 00289 rtt_y = ((rt_x * k1)+ (rt_y * k2 )); 00290 rtt_z = rt_z; 00291 } 00292 00293 00294 return (rtt_x*100 + rtt_y*10 + rtt_z + 333); 00295 } 00296
Generated on Wed Jul 20 2022 20:38:11 by
1.7.2