OX
/
testIMUmain
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 Ticker flipper; 00006 ZMU9250 a; 00007 Serial pc(D1, D0); 00008 00009 float yaw_z, pitch_y, roll_x; 00010 int ir,ip,iy; 00011 int l; 00012 int xlr,yfb,ztd; 00013 int sata; 00014 char rt; 00015 00016 int cube1(int x, int y, int z); 00017 int seccon(float roll ,float pitch, float yaw); 00018 int round(int rt_x, int rt_y, int rt_z); 00019 int sata0(int neww); 00020 int roo(int a1); 00021 00022 int Rotate_Roll, Rotate_Pitch, Rotate_Yaw, A, B, C ; 00023 int rmap[5][5][5]; 00024 int map[5][5][5]; 00025 char dete[50]; 00026 int indx=0; 00027 int ii; 00028 bool anach = false; 00029 00030 char data[50]; 00031 char elem; 00032 00033 Serial mas(D8,D2); 00034 00035 00036 00037 00038 // state 0 = initial / start. 00039 // 1 = waiting for limit switch. 00040 00041 unsigned int turn = 0; // turn. 00042 00043 unsigned int x = 0, y = 0, z = 0; 00044 00045 int helperRoll = 0, helperPitch=0,helperYaw=0; 00046 00047 unsigned int state = 0; 00048 DigitalIn op_roll(USER_BUTTON); 00049 int Cross(int degree) 00050 { 00051 if (degree == 0) 00052 { 00053 return 1; 00054 }else if (degree == 180) 00055 { 00056 return -1; 00057 }else 00058 { 00059 return 0; 00060 } 00061 } 00062 00063 int Zine(int degree) 00064 { 00065 if (degree == 90) 00066 { 00067 return 1; 00068 }else if (degree == -90) 00069 { 00070 return -1; 00071 }else 00072 { 00073 return 0; 00074 } 00075 } 00076 void Sent(char arrayi[],float it,int siz) 00077 { 00078 for (int i = 0;i<siz;i++) 00079 { 00080 mas.putc(arrayi[i]); 00081 wait(it); 00082 } 00083 } 00084 00085 00086 void Rotate(int role,int pit,int yaw) 00087 {pc.printf("role %d\t pit %d\t yaw %d\n\n\n\r",role , pit ,yaw); 00088 int i=2,j=3,k=4; 00089 if ((role == 90) || (role == -90)) 00090 { 00091 /*for (int i=0;i<5;i++) 00092 { 00093 for (int j=0;j<5;j++) 00094 { 00095 for(int k=0;k<5;k++) 00096 {*/ 00097 A = ((i-2)*Cross(pit))-((k-2)*Zine(pit) )+2; 00098 B = ((j-2)*Cross(yaw))-((i-2)*Zine(pit)*Zine(yaw))-((k-2)*Zine(yaw)*Cross(pit) )+2; 00099 C = ((j-2)*Zine(yaw))+((i-2)*Cross(yaw)*Zine(pit))+((k-2)*Cross(yaw)*Cross(pit) )+2; 00100 pc.printf("from %d %d %d to %d %d %d\t=1 \n\n\n\r",i,j,k,A,B,C); 00101 00102 // } 00103 //} 00104 //} 00105 }else if ((pitch == 90) || (pitch == -90)) 00106 { 00107 /*for (int i=0;i<5;i++) 00108 { 00109 for (int j=0;j<5;j++) 00110 { 00111 for(int k=0;k<5;k++) 00112 {*/ 00113 A = ((i-2)*Cross(yaw))+((j-2)*Zine(role)*Zine(yaw))+((k-2)*Zine(yaw)*Cross(role))+2; 00114 B = ((j-2)*Cross(role))-((k-2)*Zine(role))+2; 00115 C = -((i-2)*Zine(yaw))+((j-2)*Cross(yaw)*Zine(role))+((k-2)*Cross(yaw)*Cross(role))+2; 00116 pc.printf("from %d %d %d to %d %d %d\t=2 \n\n\n\r",i,j,k,A,B,C); 00117 00118 // } 00119 //} 00120 // } 00121 }else 00122 { 00123 /*for (int i=0;i<5;i++) 00124 { 00125 for (int j=0;j<5;j++) 00126 { 00127 for(int k=0;k<5;k++) 00128 {*/ 00129 A = ((i-2)*Cross(yaw))-((j-2)*Zine(yaw))+2; 00130 B = ((i-2)*Zine(yaw))+((j-2)*Cross(yaw))+2; 00131 C = k; 00132 pc.printf("from %d %d %d to %d %d %d\t=3 \n\n\n\r",i,j,k,A,B,C); 00133 00134 //} 00135 //} 00136 // } 00137 } 00138 for (int i=0;i<5;i++) 00139 { 00140 for (int j=0;j<5;j++) 00141 { 00142 for(int k=0;k<5;k++) 00143 { 00144 00145 map[i][j][k]= rmap[i][j][k]; 00146 00147 } 00148 } 00149 } 00150 } 00151 void getStr() 00152 { 00153 pc.printf("x\n"); 00154 while (mas.readable()) 00155 { 00156 00157 elem = mas.getc(); 00158 pc.printf("char is %d\n\r",elem); 00159 if ((elem == '\n')) 00160 { 00161 pc.printf("string is %s\n\r",data); 00162 for(int i=0;i<indx;i++) 00163 { 00164 if (indx > 2) 00165 { 00166 dete[i] = data[i]; 00167 } 00168 } 00169 for(int i=0;i<30;i++) 00170 { 00171 data[i] = '\0'; 00172 } 00173 indx = 0; 00174 break; 00175 }else 00176 { 00177 data[indx] = elem; 00178 indx += 1; 00179 } 00180 00181 } 00182 } 00183 00184 00185 int main(){ 00186 pc.printf("KKKKKKKKKKKKKKKKKKKKKKKKKKKK"); 00187 flipper.attach(&getStr,1.0); 00188 while(1){ 00189 00190 wait(0.1); 00191 a.Update(); 00192 yaw_z = a.Yaw(); 00193 pitch_y = a.Pitch(); 00194 roll_x = a.Roll(); 00195 00196 pc.printf("Yaw_z %f\t Pitch_y %f\t Roll_x %f\n\n\n\r", yaw_z, pitch_y, roll_x); 00197 00198 //l = cube1(3,3,3); 00199 00200 // Rotate(Rotate_Roll, Rotate_Pitch, Rotate_Yaw); 00201 //pc.printf("%d\t Yaw_z %f\t Pitch_y %f\t Roll_x %f\n\n\n\r",l , yaw_z, pitch_y, roll_x); //add 00202 // pc.printf("%d\t sata(Yaw_z) %d\t sata0(pitch_y) %d\t sata0(Roll_x) %d\n\n\n\r",l , sata0(yaw_z), sata0(pitch_y), sata0(roll_x)); 00203 00204 //pc.printf("%d\t Yaw_z %f\t Pitch_y %f\t Roll_x %f\t %d\t %c\t %d\t \r\n ",round(5,5,5), yaw_z, pitch_y, roll_x, sata, rt, l); //round 00205 00206 // pc.printf( "%d\n" ,seccon(roll_x, pitch_y, yaw_z)); 00207 // pc.printf("========\n"); //it 00208 // if(op_roll == 0){ 00209 // 00210 // l = cube1(3,3,3); 00211 // } 00212 00213 00214 00215 Sent("hely\n",0.1,5); // tell the other box, that we want helper roll. 00216 while((dete[0]!='h')||(dete[1]!='e')||(dete[2]!='l')||(dete[3]!='y')) 00217 { 00218 pc.printf("srt %s\n\r",dete); 00219 } 00220 helperYaw = int( ((int(dete[4+ii])-48)*100)+((int(dete[5+ii])-48)*10)+(int(dete[6+ii])-48) ); //get roll from another box 00221 pc.printf("help %d\n\n\n\n\n\n\n\t",helperYaw); 00222 00223 } 00224 } 00225 // 00226 // int cube1(int x, int y, int z){ 00227 // 00228 // // if( ((pitch_y > -30) && (pitch_y < 30)) ){ // vCC L leg D 00229 // 00230 // if( ((yaw_z > -40) && (yaw_z < 0)) && ((roll_x > -30) && (roll_x < 30)) && ((pitch_y > -20) && (pitch_y < 30)) ){ 00231 // x++; //R x+ 00232 // xlr = 1; 00233 // } 00234 // else if( ((yaw_z > -105) && (yaw_z < -75))&& ((roll_x > -20) && (roll_x < 35)) && ((pitch_y > -20) && (pitch_y < 30)) ){ 00235 // y--; //B y+ 00236 // yfb = -1; 00237 // } 00238 // else if( ((yaw_z > 140) && (yaw_z < 175)) && ((roll_x > -40) && (roll_x < 25)) && ((pitch_y > -35) && (pitch_y < 25)) ){ 00239 // y++; //F y- 00240 // yfb = 1; 00241 // 00242 // } 00243 // else if( ((yaw_z > -155) && (yaw_z < -130)) && ((roll_x > -25) && (roll_x < 10)) && ((pitch_y > -20) && (pitch_y < 15)) ){ 00244 // x--; //L x- 00245 // xlr = -1; 00246 // 00247 // } 00248 // else if( ((yaw_z > -150) && (yaw_z < -88)) && ((roll_x > 70) && (roll_x < 110)) && ((pitch_y > -20) && (pitch_y < 35))){ 00249 // z++; //T z+ 00250 // ztd = 1; 00251 // 00252 // 00253 // } 00254 // else if( ((yaw_z > -125) && (yaw_z < -100)) && ((roll_x > -100) && (roll_x < -75)) && ((pitch_y > -25) && (pitch_y < 35))){ 00255 // z--; //D z- 00256 // ztd = -1; 00257 // 00258 // } 00259 // 00260 // else if( ((yaw_z > -80) && (yaw_z < -40)) && ((roll_x > 160) || (roll_x < -160)) && ((pitch_y > -15) && (pitch_y < 15))){ 00261 // y++; 00262 // yfb = 1; 00263 // 00264 // } 00265 // else if( ((yaw_z > 160) || (yaw_z < -165)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -10) && (pitch_y < 10)) ){ 00266 // x++; 00267 // xlr = 1; 00268 // } 00269 // else if( ((yaw_z > 100) && (yaw_z < 130)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -20) && (pitch_y < 15))){ 00270 // y--; 00271 // yfb = -1; 00272 // } 00273 // else if( ((yaw_z > 45) && (yaw_z < 65)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -15) && (pitch_y < 15)) ){ 00274 // x--; //L x- 00275 // xlr = -1; 00276 // 00277 // } 00278 // int rt_x , rt_y , rt_z; 00279 // int a = rt_x - 3; 00280 // int b = rt_y - 3; 00281 // int c = rt_z - 3; 00282 // if(rt == 'x'){ 00283 // rt_x = a + 3; //x 00284 // rt_y = (b * cos(sata*(PI/180))+ c * sin(sata*(PI/180)) )+ 3; 00285 // rt_z = (-b * sin(sata*(PI/180))+ c * cos(sata*(PI/180)) ) + 3; 00286 // } 00287 // if(rt == 'y'){ 00288 // rt_x = (a * cos(sata*(PI/180)) - c * sin(sata*(PI/180)) )+ 3; // y 00289 // rt_y = b + 3; 00290 // rt_z = (a * sin(sata*(PI/180)) + c * cos(sata*(PI/180)) )+ 3; 00291 // } 00292 // if(rt == 'z'){ 00293 // rt_x = (a * cos(sata*(PI/180))+ b * sin(sata*(PI/180)) )+ 3; //z 00294 // rt_y = (b * cos(sata*(PI/180))- a * sin(sata*(PI/180)) )+ 3; 00295 // rt_z = c + 3; 00296 // } 00297 // 00298 // 00299 // 00300 // 00301 // return (x*100+y*10+z); 00302 // } 00303 // int sata0(int neww){ 00304 // if(neww>=0){ 00305 // if((neww <= 45) && (neww >= 0)){ 00306 // return 0; 00307 // } 00308 // if((neww <= 90) && (neww >= 46)){ 00309 // return 90; 00310 // } 00311 // if((neww <= 135) && (neww >= 91)){ 00312 // return -90; 00313 // } 00314 // if((neww <= 180) && (neww >= 136)){ 00315 // return 180; 00316 // } 00317 // } 00318 // else{ 00319 // if((neww <= 0) && (neww >= -45)){ 00320 // return 180; 00321 // } 00322 // if((neww <= -46) && (neww >= -90)){ 00323 // return -90; 00324 // } 00325 // if((neww <= -91) && (neww >= -135)){ 00326 // return 90; 00327 // } 00328 // if((neww <= -136) && (neww >= -180)){ 00329 // return 0; 00330 // } 00331 // } 00332 // } 00333 // 00334 // 00335 // int roo(int a1){ 00336 // 00337 // 00338 // //if( ((yaw_z > -40) && (yaw_z < 0)) && ((roll_x > -30) && (roll_x < 30)) && ((pitch_y > -20) && (pitch_y < 30)) ){ 00339 //// //R x+ 00340 //// Rotate_Roll = roll_x; 00341 //// Rotate_Pitch = pitch_y; 00342 //// Rotate_Yaw = yaw_z; 00343 //// 00344 //// 00345 //// } 00346 // if ( ((yaw_z > -150) && (yaw_z < -110))&& ((roll_x > 60) && (roll_x < 100)) && ((pitch_y > -10) && (pitch_y < 10)) ){ 00347 // 00348 // Rotate_Roll = 0; // 0,90,2 00349 // Rotate_Pitch = sata0(pitch_y); 00350 // Rotate_Yaw = 180; 00351 // pc.printf("0,90,2 \n\n\n\r"); 00352 // } 00353 // else if ( ((yaw_z > -140) && (yaw_z < -100))&& ((roll_x > -100) && (roll_x < -60)) && ((pitch_y > -10) && (pitch_y < 10)) ){ 00354 // Rotate_Roll = 0; // 0,-90,2 00355 // Rotate_Pitch = sata0(pitch_y); 00356 // Rotate_Yaw = 180; 00357 // pc.printf("0,-90,2 \n\n\n\r"); 00358 // } 00359 // else if( ((yaw_z > 60) && (yaw_z < 100))&& ((roll_x > 60) && (roll_x < 100)) && ((pitch_y > -10) && (pitch_y < 10)) ){ 00360 // Rotate_Roll = 180; // 180,90,2 00361 // Rotate_Pitch = sata0(pitch_y); 00362 // Rotate_Yaw = 0; 00363 // pc.printf("180,90,2 \n\n\n\r"); 00364 // } 00365 // else if ( ((yaw_z > 50) && (yaw_z < 90))&& ((roll_x > -110) && (roll_x < -70)) && ((pitch_y > -15) && (pitch_y < 15)) ){ 00366 // Rotate_Roll = 180; // 180,-90,2 00367 // Rotate_Pitch = sata0(pitch_y); 00368 // Rotate_Yaw = 0; 00369 // pc.printf("180,-90,2 \n\n\n\r"); 00370 // } 00371 // // else if( ((al > 0) && (a1 < 9999)) && ((pitch_y > -85) && (pitch_y < -45)) ){ 00372 //// 00373 //// 00374 //// Rotate_Roll = sata0(roll_x); // 90,0,3 00375 //// Rotate_Pitch = sata0(pitch_y); 00376 //// Rotate_Yaw = sata0(yaw_z); 00377 //// pc.printf("90,0,3 \n\n\n\r"); 00378 //// } 00379 // else if( ((a1 > 0) && (a1 < 9999)) && ((pitch_y > 60) && (pitch_y < 85)) ){ 00380 // Rotate_Roll = sata0(roll_x); // -90,0,3 00381 // Rotate_Pitch = sata0(pitch_y); 00382 // Rotate_Yaw = sata0(yaw_z); 00383 // pc.printf("-90,0,3 \n\n\n\r"); 00384 // } 00385 // else if( ((a1> 0) && (a1 < 9999)) && ((pitch_y > -85) && (pitch_y < -45)) ){ 00386 // Rotate_Roll = sata0(roll_x); // 90,180,3 00387 // Rotate_Pitch = sata0(pitch_y); 00388 // Rotate_Yaw = sata0(yaw_z); 00389 // pc.printf("90,180,3 \n\n\n\r"); 00390 // } 00391 // else if( ((a1 > 0) && (a1 < 9999)) && ((pitch_y > 60) && (pitch_y < 85)) ){ 00392 // Rotate_Roll = sata0(roll_x); // 90,180,3 00393 // Rotate_Pitch = sata0(pitch_y); 00394 // Rotate_Yaw = sata0(yaw_z); 00395 // pc.printf("90,180,3 \n\n\n\r"); 00396 // } 00397 // else if( ((yaw_z > -105) && (yaw_z < -75))&& ((roll_x > -20) && (roll_x < 35)) && ((pitch_y > -20) && (pitch_y < 30)) ){ 00398 // //B y+ 180,180,1 00399 // Rotate_Roll = sata0(roll_x); 00400 // Rotate_Pitch = sata0(pitch_y); 00401 // Rotate_Yaw = sata0(yaw_z); 00402 // pc.printf("180,180,1 \n\n\n\r"); 00403 // } 00404 // else if( ((yaw_z > 140) && (yaw_z < 175)) && ((roll_x > -40) && (roll_x < 25)) && ((pitch_y > -35) && (pitch_y < 25)) ){ 00405 // //F y- 0,0,1 00406 // 00407 // Rotate_Roll = sata0(roll_x); 00408 // Rotate_Pitch = sata0(pitch_y); 00409 // Rotate_Yaw = sata0(yaw_z); 00410 // pc.printf("0,0,1 \n\n\n\r"); 00411 // } 00412 // // else if( ((yaw_z > -155) && (yaw_z < -130)) && ((roll_x > -25) && (roll_x < 10)) && ((pitch_y > -20) && (pitch_y < 15)) ){ 00413 //// //L x- 180,180,1 00414 //// 00415 //// Rotate_Roll = roll_x; 00416 //// Rotate_Pitch = pitch_y; 00417 //// Rotate_Yaw = yaw_z; 00418 //// pc.printf("180,180,1 \n\n\n\r"); 00419 //// 00420 //// } 00421 // //else if( ((yaw_z > -150) && (yaw_z < -88)) && ((roll_x > 70) && (roll_x < 110)) && ((pitch_y > -20) && (pitch_y < 35))){ 00422 //// z++; //T z+ 00423 //// ztd = 1; 00424 //// Rotate_Roll = roll_x 00425 //// Rotate_Pitch = pitch_y 00426 //// Rotate_Yaw = yaw_z 00427 //// 00428 //// } 00429 //// else if( ((yaw_z > -125) && (yaw_z < -100)) && ((roll_x > -100) && (roll_x < -75)) && ((pitch_y > -25) && (pitch_y < 35))){ 00430 //// z--; //D z- 00431 //// ztd = -1; 00432 //// Rotate_Roll = roll_x 00433 //// Rotate_Pitch = pitch_y 00434 //// Rotate_Yaw = yaw_z 00435 //// } 00436 //// 00437 // else if( ((yaw_z > -80) && (yaw_z < -40)) && ((roll_x > 160) || (roll_x < -160)) && ((pitch_y > -15) && (pitch_y < 15))){ 00438 // //180,0,4 00439 // Rotate_Roll = sata0(roll_x); 00440 // Rotate_Pitch = sata0(pitch_y); 00441 // Rotate_Yaw = sata0(yaw_z); 00442 // pc.printf("180,0,4"); 00443 // 00444 // } 00445 //// else if( ((yaw_z > 160) || (yaw_z < -165)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -10) && (pitch_y < 10)) ){ 00446 //// x++; 00447 //// xlr = 1; 00448 //// Rotate_Roll = roll_x 00449 //// Rotate_Pitch = pitch_y 00450 //// Rotate_Yaw = yaw_z 00451 //// } 00452 // else if( ((yaw_z > 100) && (yaw_z < 130)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -20) && (pitch_y < 15))){ 00453 // //0,180,4 00454 // Rotate_Roll = sata0(roll_x); 00455 // Rotate_Pitch = sata0(pitch_y); 00456 // Rotate_Yaw = sata0(yaw_z); 00457 // pc.printf("0,180,4"); 00458 // } 00459 //// else if( ((yaw_z > 45) && (yaw_z < 65)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -15) && (pitch_y < 15)) ){ 00460 //// x--; //L x- 00461 //// xlr = -1; 00462 //// Rotate_Roll = roll_x 00463 //// Rotate_Pitch = pitch_y 00464 //// Rotate_Yaw = yaw_z 00465 //// 00466 //// } 00467 // 00468 // return (1); 00469 // 00470 // }
Generated on Wed Jul 20 2022 13:43:14 by
1.7.2