Chawisorn samrit / Mbed 2 deprecated testIMU2_copy2

kor bork wa cop koy ma

Dependencies:   mbed

Fork of testIMU2_copy2 by OX

main.cpp@1:cc3c774c218a, 2016-12-05 (annotated)

Committer:
silvermist
Date:
Mon Dec 05 08:48:17 2016 +0000
Revision:
1:cc3c774c218a
Parent:
0:77a7d1a1c6db 
22

Who changed what in which revision?

UserRevisionLine numberNew contents of line
siwakon 0:77a7d1a1c6db 1 #include "mbed.h"
siwakon 0:77a7d1a1c6db 2 #include "zmu9250.h"
siwakon 0:77a7d1a1c6db 3 #include "math.h"
siwakon 0:77a7d1a1c6db 4
siwakon 0:77a7d1a1c6db 5 ZMU9250 a;
siwakon 0:77a7d1a1c6db 6 Serial pc(D1, D0);
silvermist 1:cc3c774c218a 7 Serial mas(D8,D2);
silvermist 1:cc3c774c218a 8 Ticker flipper;
silvermist 1:cc3c774c218a 9
silvermist 1:cc3c774c218a 10 char dete[50];
silvermist 1:cc3c774c218a 11 int indx=0;
silvermist 1:cc3c774c218a 12 int ii;
silvermist 1:cc3c774c218a 13 bool anach = false;
silvermist 1:cc3c774c218a 14
silvermist 1:cc3c774c218a 15 char data[50];
silvermist 1:cc3c774c218a 16 char elem;
siwakon 0:77a7d1a1c6db 17
siwakon 0:77a7d1a1c6db 18 float yaw_z, pitch_y, roll_x;
siwakon 0:77a7d1a1c6db 19 int ir,ip,iy;
siwakon 0:77a7d1a1c6db 20 int l;
siwakon 0:77a7d1a1c6db 21 int xlr,yfb,ztd;
siwakon 0:77a7d1a1c6db 22 int sata;
siwakon 0:77a7d1a1c6db 23 char rt;
siwakon 0:77a7d1a1c6db 24
siwakon 0:77a7d1a1c6db 25 int cube1(int x, int y, int z);
siwakon 0:77a7d1a1c6db 26 int seccon(float roll ,float pitch, float yaw);
siwakon 0:77a7d1a1c6db 27 int round(int rt_x, int rt_y, int rt_z);
silvermist 1:cc3c774c218a 28 int sata0(int neww);
silvermist 1:cc3c774c218a 29 int roo(int a1);
silvermist 1:cc3c774c218a 30
silvermist 1:cc3c774c218a 31 int Rotate_Roll, Rotate_Pitch, Rotate_Yaw, A, B, C ;
silvermist 1:cc3c774c218a 32 int rmap[5][5][5];
silvermist 1:cc3c774c218a 33 int map[5][5][5];
silvermist 1:cc3c774c218a 34 char yaw_buf1 = 0;
silvermist 1:cc3c774c218a 35 char yaw_buf2 = 0;
silvermist 1:cc3c774c218a 36 char yaw_buf3 = 0;
silvermist 1:cc3c774c218a 37 char inttost[50];
silvermist 1:cc3c774c218a 38
silvermist 1:cc3c774c218a 39 DigitalIn op_roll(USER_BUTTON);
silvermist 1:cc3c774c218a 40 int Cross(int degree)
silvermist 1:cc3c774c218a 41 {
silvermist 1:cc3c774c218a 42 if (degree == 0)
silvermist 1:cc3c774c218a 43 {
silvermist 1:cc3c774c218a 44 return 1;
silvermist 1:cc3c774c218a 45 }else if (degree == 180)
silvermist 1:cc3c774c218a 46 {
silvermist 1:cc3c774c218a 47 return -1;
silvermist 1:cc3c774c218a 48 }else
silvermist 1:cc3c774c218a 49 {
silvermist 1:cc3c774c218a 50 return 0;
silvermist 1:cc3c774c218a 51 }
silvermist 1:cc3c774c218a 52 }
silvermist 1:cc3c774c218a 53
silvermist 1:cc3c774c218a 54 int Zine(int degree)
silvermist 1:cc3c774c218a 55 {
silvermist 1:cc3c774c218a 56 if (degree == 90)
silvermist 1:cc3c774c218a 57 {
silvermist 1:cc3c774c218a 58 return 1;
silvermist 1:cc3c774c218a 59 }else if (degree == -90)
silvermist 1:cc3c774c218a 60 {
silvermist 1:cc3c774c218a 61 return -1;
silvermist 1:cc3c774c218a 62 }else
silvermist 1:cc3c774c218a 63 {
silvermist 1:cc3c774c218a 64 return 0;
silvermist 1:cc3c774c218a 65 }
silvermist 1:cc3c774c218a 66 }
silvermist 1:cc3c774c218a 67 void Sent(char arrayi[],float it,int siz)
silvermist 1:cc3c774c218a 68
silvermist 1:cc3c774c218a 69 {
silvermist 1:cc3c774c218a 70 for (int i = 0;i<siz;i++)
silvermist 1:cc3c774c218a 71 {
silvermist 1:cc3c774c218a 72 pc.printf("%c",arrayi[i]);
silvermist 1:cc3c774c218a 73 mas.putc(arrayi[i]);
silvermist 1:cc3c774c218a 74 wait(it);
silvermist 1:cc3c774c218a 75 }
silvermist 1:cc3c774c218a 76 }
siwakon 0:77a7d1a1c6db 77
siwakon 0:77a7d1a1c6db 78
silvermist 1:cc3c774c218a 79 void getStr()
silvermist 1:cc3c774c218a 80 {
silvermist 1:cc3c774c218a 81 pc.printf("x\n");
silvermist 1:cc3c774c218a 82 while (mas.readable())
silvermist 1:cc3c774c218a 83 {
silvermist 1:cc3c774c218a 84
silvermist 1:cc3c774c218a 85 elem = mas.getc();
silvermist 1:cc3c774c218a 86 pc.printf("char is %d\n\r",elem);
silvermist 1:cc3c774c218a 87 if ((elem == '\n'))
silvermist 1:cc3c774c218a 88 {
silvermist 1:cc3c774c218a 89 pc.printf("string is %s\n\r",data);
silvermist 1:cc3c774c218a 90 for(int i=0;i<indx;i++)
silvermist 1:cc3c774c218a 91 {
silvermist 1:cc3c774c218a 92 if (indx > 2)
silvermist 1:cc3c774c218a 93 {
silvermist 1:cc3c774c218a 94 dete[i] = data[i];
silvermist 1:cc3c774c218a 95 }
silvermist 1:cc3c774c218a 96 }
silvermist 1:cc3c774c218a 97 for(int i=0;i<30;i++)
silvermist 1:cc3c774c218a 98 {
silvermist 1:cc3c774c218a 99 data[i] = '\0';
silvermist 1:cc3c774c218a 100 }
silvermist 1:cc3c774c218a 101 indx = 0;
silvermist 1:cc3c774c218a 102 break;
silvermist 1:cc3c774c218a 103 }else
silvermist 1:cc3c774c218a 104 {
silvermist 1:cc3c774c218a 105 data[indx] = elem;
silvermist 1:cc3c774c218a 106 indx += 1;
silvermist 1:cc3c774c218a 107 }
silvermist 1:cc3c774c218a 108
silvermist 1:cc3c774c218a 109 }
silvermist 1:cc3c774c218a 110 }
silvermist 1:cc3c774c218a 111
silvermist 1:cc3c774c218a 112 void Rotate(int role,int pit,int yaw)
silvermist 1:cc3c774c218a 113 {pc.printf("role %d\t pit %d\t yaw %d\n\n\n\r",role , pit ,yaw);
silvermist 1:cc3c774c218a 114 int i=2,j=3,k=4;
silvermist 1:cc3c774c218a 115 if ((role == 90) || (role == -90))
silvermist 1:cc3c774c218a 116 {
silvermist 1:cc3c774c218a 117 /*for (int i=0;i<5;i++)
silvermist 1:cc3c774c218a 118 {
silvermist 1:cc3c774c218a 119 for (int j=0;j<5;j++)
silvermist 1:cc3c774c218a 120 {
silvermist 1:cc3c774c218a 121 for(int k=0;k<5;k++)
silvermist 1:cc3c774c218a 122 {*/
silvermist 1:cc3c774c218a 123 A = ((i-2)*Cross(pit))-((k-2)*Zine(pit) )+2;
silvermist 1:cc3c774c218a 124 B = ((j-2)*Cross(yaw))-((i-2)*Zine(pit)*Zine(yaw))-((k-2)*Zine(yaw)*Cross(pit) )+2;
silvermist 1:cc3c774c218a 125 C = ((j-2)*Zine(yaw))+((i-2)*Cross(yaw)*Zine(pit))+((k-2)*Cross(yaw)*Cross(pit) )+2;
silvermist 1:cc3c774c218a 126 pc.printf("from %d %d %d to %d %d %d\t=1 \n\n\n\r",i,j,k,A,B,C);
silvermist 1:cc3c774c218a 127
silvermist 1:cc3c774c218a 128 // }
silvermist 1:cc3c774c218a 129 //}
silvermist 1:cc3c774c218a 130 //}
silvermist 1:cc3c774c218a 131 }else if ((pitch == 90) || (pitch == -90))
silvermist 1:cc3c774c218a 132 {
silvermist 1:cc3c774c218a 133 /*for (int i=0;i<5;i++)
silvermist 1:cc3c774c218a 134 {
silvermist 1:cc3c774c218a 135 for (int j=0;j<5;j++)
silvermist 1:cc3c774c218a 136 {
silvermist 1:cc3c774c218a 137 for(int k=0;k<5;k++)
silvermist 1:cc3c774c218a 138 {*/
silvermist 1:cc3c774c218a 139 A = ((i-2)*Cross(yaw))+((j-2)*Zine(role)*Zine(yaw))+((k-2)*Zine(yaw)*Cross(role))+2;
silvermist 1:cc3c774c218a 140 B = ((j-2)*Cross(role))-((k-2)*Zine(role))+2;
silvermist 1:cc3c774c218a 141 C = -((i-2)*Zine(yaw))+((j-2)*Cross(yaw)*Zine(role))+((k-2)*Cross(yaw)*Cross(role))+2;
silvermist 1:cc3c774c218a 142 pc.printf("from %d %d %d to %d %d %d\t=2 \n\n\n\r",i,j,k,A,B,C);
silvermist 1:cc3c774c218a 143
silvermist 1:cc3c774c218a 144 // }
silvermist 1:cc3c774c218a 145 //}
silvermist 1:cc3c774c218a 146 // }
silvermist 1:cc3c774c218a 147 }else
silvermist 1:cc3c774c218a 148 {
silvermist 1:cc3c774c218a 149 /*for (int i=0;i<5;i++)
silvermist 1:cc3c774c218a 150 {
silvermist 1:cc3c774c218a 151 for (int j=0;j<5;j++)
silvermist 1:cc3c774c218a 152 {
silvermist 1:cc3c774c218a 153 for(int k=0;k<5;k++)
silvermist 1:cc3c774c218a 154 {*/
silvermist 1:cc3c774c218a 155 A = ((i-2)*Cross(yaw))-((j-2)*Zine(yaw))+2;
silvermist 1:cc3c774c218a 156 B = ((i-2)*Zine(yaw))+((j-2)*Cross(yaw))+2;
silvermist 1:cc3c774c218a 157 C = k;
silvermist 1:cc3c774c218a 158 pc.printf("from %d %d %d to %d %d %d\t=3 \n\n\n\r",i,j,k,A,B,C);
silvermist 1:cc3c774c218a 159
silvermist 1:cc3c774c218a 160 //}
silvermist 1:cc3c774c218a 161 //}
silvermist 1:cc3c774c218a 162 // }
silvermist 1:cc3c774c218a 163 }
silvermist 1:cc3c774c218a 164 for (int i=0;i<5;i++)
silvermist 1:cc3c774c218a 165 {
silvermist 1:cc3c774c218a 166 for (int j=0;j<5;j++)
silvermist 1:cc3c774c218a 167 {
silvermist 1:cc3c774c218a 168 for(int k=0;k<5;k++)
silvermist 1:cc3c774c218a 169 {
silvermist 1:cc3c774c218a 170
silvermist 1:cc3c774c218a 171 map[i][j][k]= rmap[i][j][k];
silvermist 1:cc3c774c218a 172
silvermist 1:cc3c774c218a 173 }
silvermist 1:cc3c774c218a 174 }
silvermist 1:cc3c774c218a 175 }
silvermist 1:cc3c774c218a 176 }
silvermist 1:cc3c774c218a 177
siwakon 0:77a7d1a1c6db 178
siwakon 0:77a7d1a1c6db 179
siwakon 0:77a7d1a1c6db 180 int main(){
siwakon 0:77a7d1a1c6db 181
silvermist 1:cc3c774c218a 182 flipper.attach(&getStr,1.0);
siwakon 0:77a7d1a1c6db 183 while(1){
silvermist 1:cc3c774c218a 184 pc.printf("ddddd");
silvermist 1:cc3c774c218a 185
siwakon 0:77a7d1a1c6db 186 wait(0.1);
siwakon 0:77a7d1a1c6db 187 a.Update();
siwakon 0:77a7d1a1c6db 188 yaw_z = a.Yaw();
siwakon 0:77a7d1a1c6db 189 pitch_y = a.Pitch();
siwakon 0:77a7d1a1c6db 190 roll_x = a.Roll();
silvermist 1:cc3c774c218a 191 // pc.printf("data %d\n",dete);
silvermist 1:cc3c774c218a 192 // pc.printf("data %c\n",yaw_buf);
siwakon 0:77a7d1a1c6db 193
silvermist 1:cc3c774c218a 194
silvermist 1:cc3c774c218a 195
silvermist 1:cc3c774c218a 196 if ((dete[0]=='h')||(dete[1]=='e')||(dete[2]=='l')||(dete[3]=='y')){
silvermist 1:cc3c774c218a 197
silvermist 1:cc3c774c218a 198 //Sent("999\n",0.1,5);
silvermist 1:cc3c774c218a 199
silvermist 1:cc3c774c218a 200 sprintf(inttost,"%d",int(yaw_z));
silvermist 1:cc3c774c218a 201 //yaw_buf1=int((int(yaw_z)%1000)/100)+48;
silvermist 1:cc3c774c218a 202 // yaw_buf2=int((int(yaw_z)%100)/10)+48;
silvermist 1:cc3c774c218a 203 // yaw_buf3=int((int(yaw_z)%10))+48;
silvermist 1:cc3c774c218a 204 // Sent("hely",0.1,4);
silvermist 1:cc3c774c218a 205 // Sent(yaw_buf1,0.1,1);
silvermist 1:cc3c774c218a 206 // Sent(yaw_buf2,0.1,1);
silvermist 1:cc3c774c218a 207 Sent(inttost,0.1,5);
silvermist 1:cc3c774c218a 208 // pc.printf("data %c\n",yaw_buf);
silvermist 1:cc3c774c218a 209 Sent("\n",0.1,1);
silvermist 1:cc3c774c218a 210
silvermist 1:cc3c774c218a 211 }
silvermist 1:cc3c774c218a 212
silvermist 1:cc3c774c218a 213
silvermist 1:cc3c774c218a 214 pc.printf("Yaw_z %f\t Pitch_y %f\t Roll_x %f\n\n\n\r", yaw_z, pitch_y, roll_x);
silvermist 1:cc3c774c218a 215
siwakon 0:77a7d1a1c6db 216 //l = cube1(3,3,3);
silvermist 1:cc3c774c218a 217 //l = roo(10);
silvermist 1:cc3c774c218a 218 //Rotate(Rotate_Roll, Rotate_Pitch, Rotate_Yaw);
silvermist 1:cc3c774c218a 219 //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
silvermist 1:cc3c774c218a 220 //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));
siwakon 0:77a7d1a1c6db 221
silvermist 1:cc3c774c218a 222 //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
siwakon 0:77a7d1a1c6db 223
siwakon 0:77a7d1a1c6db 224 // pc.printf( "%d\n" ,seccon(roll_x, pitch_y, yaw_z));
siwakon 0:77a7d1a1c6db 225 // pc.printf("========\n"); //it
silvermist 1:cc3c774c218a 226 // if(op_roll == 0){
silvermist 1:cc3c774c218a 227 //
silvermist 1:cc3c774c218a 228 // l = cube1(3,3,3);
silvermist 1:cc3c774c218a 229 // }
siwakon 0:77a7d1a1c6db 230
siwakon 0:77a7d1a1c6db 231
siwakon 0:77a7d1a1c6db 232 }
siwakon 0:77a7d1a1c6db 233 }
siwakon 0:77a7d1a1c6db 234
siwakon 0:77a7d1a1c6db 235 int cube1(int x, int y, int z){
siwakon 0:77a7d1a1c6db 236
silvermist 1:cc3c774c218a 237 // if( ((pitch_y > -30) && (pitch_y < 30)) ){ // vCC L leg D
siwakon 0:77a7d1a1c6db 238
silvermist 1:cc3c774c218a 239 if( ((yaw_z > -40) && (yaw_z < 0)) && ((roll_x > -30) && (roll_x < 30)) && ((pitch_y > -20) && (pitch_y < 30)) ){
siwakon 0:77a7d1a1c6db 240 x++; //R x+
siwakon 0:77a7d1a1c6db 241 xlr = 1;
siwakon 0:77a7d1a1c6db 242 }
silvermist 1:cc3c774c218a 243 else if( ((yaw_z > -105) && (yaw_z < -75))&& ((roll_x > -20) && (roll_x < 35)) && ((pitch_y > -20) && (pitch_y < 30)) ){
siwakon 0:77a7d1a1c6db 244 y--; //B y+
siwakon 0:77a7d1a1c6db 245 yfb = -1;
siwakon 0:77a7d1a1c6db 246 }
silvermist 1:cc3c774c218a 247 else if( ((yaw_z > 140) && (yaw_z < 175)) && ((roll_x > -40) && (roll_x < 25)) && ((pitch_y > -35) && (pitch_y < 25)) ){
siwakon 0:77a7d1a1c6db 248 y++; //F y-
siwakon 0:77a7d1a1c6db 249 yfb = 1;
siwakon 0:77a7d1a1c6db 250
siwakon 0:77a7d1a1c6db 251 }
silvermist 1:cc3c774c218a 252 else if( ((yaw_z > -155) && (yaw_z < -130)) && ((roll_x > -25) && (roll_x < 10)) && ((pitch_y > -20) && (pitch_y < 15)) ){
siwakon 0:77a7d1a1c6db 253 x--; //L x-
siwakon 0:77a7d1a1c6db 254 xlr = -1;
siwakon 0:77a7d1a1c6db 255
siwakon 0:77a7d1a1c6db 256 }
silvermist 1:cc3c774c218a 257 else if( ((yaw_z > -150) && (yaw_z < -88)) && ((roll_x > 70) && (roll_x < 110)) && ((pitch_y > -20) && (pitch_y < 35))){
siwakon 0:77a7d1a1c6db 258 z++; //T z+
siwakon 0:77a7d1a1c6db 259 ztd = 1;
siwakon 0:77a7d1a1c6db 260
siwakon 0:77a7d1a1c6db 261 }
silvermist 1:cc3c774c218a 262 else if( ((yaw_z > -125) && (yaw_z < -100)) && ((roll_x > -100) && (roll_x < -75)) && ((pitch_y > -25) && (pitch_y < 35))){
siwakon 0:77a7d1a1c6db 263 z--; //D z-
siwakon 0:77a7d1a1c6db 264 ztd = -1;
siwakon 0:77a7d1a1c6db 265
siwakon 0:77a7d1a1c6db 266 }
silvermist 1:cc3c774c218a 267
silvermist 1:cc3c774c218a 268 else if( ((yaw_z > -80) && (yaw_z < -40)) && ((roll_x > 160) || (roll_x < -160)) && ((pitch_y > -15) && (pitch_y < 15))){
silvermist 1:cc3c774c218a 269 y++;
silvermist 1:cc3c774c218a 270 yfb = 1;
silvermist 1:cc3c774c218a 271
silvermist 1:cc3c774c218a 272 }
silvermist 1:cc3c774c218a 273 else if( ((yaw_z > 160) || (yaw_z < -165)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -10) && (pitch_y < 10)) ){
silvermist 1:cc3c774c218a 274 x++;
silvermist 1:cc3c774c218a 275 xlr = 1;
silvermist 1:cc3c774c218a 276 }
silvermist 1:cc3c774c218a 277 else if( ((yaw_z > 100) && (yaw_z < 130)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -20) && (pitch_y < 15))){
silvermist 1:cc3c774c218a 278 y--;
silvermist 1:cc3c774c218a 279 yfb = -1;
silvermist 1:cc3c774c218a 280 }
silvermist 1:cc3c774c218a 281 else if( ((yaw_z > 45) && (yaw_z < 65)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -15) && (pitch_y < 15)) ){
silvermist 1:cc3c774c218a 282 x--; //L x-
silvermist 1:cc3c774c218a 283 xlr = -1;
silvermist 1:cc3c774c218a 284
silvermist 1:cc3c774c218a 285 }
silvermist 1:cc3c774c218a 286 int rt_x , rt_y , rt_z;
silvermist 1:cc3c774c218a 287 int a = rt_x - 3;
silvermist 1:cc3c774c218a 288 int b = rt_y - 3;
silvermist 1:cc3c774c218a 289 int c = rt_z - 3;
silvermist 1:cc3c774c218a 290 if(rt == 'x'){
silvermist 1:cc3c774c218a 291 rt_x = a + 3; //x
silvermist 1:cc3c774c218a 292 rt_y = (b * cos(sata*(PI/180))+ c * sin(sata*(PI/180)) )+ 3;
silvermist 1:cc3c774c218a 293 rt_z = (-b * sin(sata*(PI/180))+ c * cos(sata*(PI/180)) ) + 3;
silvermist 1:cc3c774c218a 294 }
silvermist 1:cc3c774c218a 295 if(rt == 'y'){
silvermist 1:cc3c774c218a 296 rt_x = (a * cos(sata*(PI/180)) - c * sin(sata*(PI/180)) )+ 3; // y
silvermist 1:cc3c774c218a 297 rt_y = b + 3;
silvermist 1:cc3c774c218a 298 rt_z = (a * sin(sata*(PI/180)) + c * cos(sata*(PI/180)) )+ 3;
silvermist 1:cc3c774c218a 299 }
silvermist 1:cc3c774c218a 300 if(rt == 'z'){
silvermist 1:cc3c774c218a 301 rt_x = (a * cos(sata*(PI/180))+ b * sin(sata*(PI/180)) )+ 3; //z
silvermist 1:cc3c774c218a 302 rt_y = (b * cos(sata*(PI/180))- a * sin(sata*(PI/180)) )+ 3;
silvermist 1:cc3c774c218a 303 rt_z = c + 3;
silvermist 1:cc3c774c218a 304 }
silvermist 1:cc3c774c218a 305
siwakon 0:77a7d1a1c6db 306
siwakon 0:77a7d1a1c6db 307
siwakon 0:77a7d1a1c6db 308
siwakon 0:77a7d1a1c6db 309 return (x*100+y*10+z);
siwakon 0:77a7d1a1c6db 310 }
silvermist 1:cc3c774c218a 311 int sata0(int neww){
silvermist 1:cc3c774c218a 312 if(neww>=0){
silvermist 1:cc3c774c218a 313 if((neww <= 45) && (neww >= 0)){
silvermist 1:cc3c774c218a 314 return 0;
silvermist 1:cc3c774c218a 315 }
silvermist 1:cc3c774c218a 316 if((neww <= 90) && (neww >= 46)){
silvermist 1:cc3c774c218a 317 return 90;
silvermist 1:cc3c774c218a 318 }
silvermist 1:cc3c774c218a 319 if((neww <= 135) && (neww >= 91)){
silvermist 1:cc3c774c218a 320 return -90;
silvermist 1:cc3c774c218a 321 }
silvermist 1:cc3c774c218a 322 if((neww <= 180) && (neww >= 136)){
silvermist 1:cc3c774c218a 323 return 180;
silvermist 1:cc3c774c218a 324 }
siwakon 0:77a7d1a1c6db 325 }
siwakon 0:77a7d1a1c6db 326 else{
silvermist 1:cc3c774c218a 327 if((neww <= 0) && (neww >= -45)){
silvermist 1:cc3c774c218a 328 return 180;
siwakon 0:77a7d1a1c6db 329 }
silvermist 1:cc3c774c218a 330 if((neww <= -46) && (neww >= -90)){
silvermist 1:cc3c774c218a 331 return -90;
siwakon 0:77a7d1a1c6db 332 }
silvermist 1:cc3c774c218a 333 if((neww <= -91) && (neww >= -135)){
silvermist 1:cc3c774c218a 334 return 90;
siwakon 0:77a7d1a1c6db 335 }
silvermist 1:cc3c774c218a 336 if((neww <= -136) && (neww >= -180)){
silvermist 1:cc3c774c218a 337 return 0;
siwakon 0:77a7d1a1c6db 338 }
siwakon 0:77a7d1a1c6db 339 }
siwakon 0:77a7d1a1c6db 340 }
silvermist 1:cc3c774c218a 341
siwakon 0:77a7d1a1c6db 342
silvermist 1:cc3c774c218a 343 int roo(int a1){
silvermist 1:cc3c774c218a 344
silvermist 1:cc3c774c218a 345
silvermist 1:cc3c774c218a 346 //if( ((yaw_z > -40) && (yaw_z < 0)) && ((roll_x > -30) && (roll_x < 30)) && ((pitch_y > -20) && (pitch_y < 30)) ){
silvermist 1:cc3c774c218a 347 // //R x+
silvermist 1:cc3c774c218a 348 // Rotate_Roll = roll_x;
silvermist 1:cc3c774c218a 349 // Rotate_Pitch = pitch_y;
silvermist 1:cc3c774c218a 350 // Rotate_Yaw = yaw_z;
silvermist 1:cc3c774c218a 351 //
silvermist 1:cc3c774c218a 352 //
silvermist 1:cc3c774c218a 353 // }
silvermist 1:cc3c774c218a 354 if ( ((yaw_z > -150) && (yaw_z < -110))&& ((roll_x > 60) && (roll_x < 100)) && ((pitch_y > -10) && (pitch_y < 10)) ){
silvermist 1:cc3c774c218a 355
silvermist 1:cc3c774c218a 356 Rotate_Roll = 0; // 0,90,2
silvermist 1:cc3c774c218a 357 Rotate_Pitch = sata0(pitch_y);
silvermist 1:cc3c774c218a 358 Rotate_Yaw = 180;
silvermist 1:cc3c774c218a 359 pc.printf("0,90,2 \n\n\n\r");
silvermist 1:cc3c774c218a 360 }
silvermist 1:cc3c774c218a 361 else if ( ((yaw_z > -140) && (yaw_z < -100))&& ((roll_x > -100) && (roll_x < -60)) && ((pitch_y > -10) && (pitch_y < 10)) ){
silvermist 1:cc3c774c218a 362 Rotate_Roll = 0; // 0,-90,2
silvermist 1:cc3c774c218a 363 Rotate_Pitch = sata0(pitch_y);
silvermist 1:cc3c774c218a 364 Rotate_Yaw = 180;
silvermist 1:cc3c774c218a 365 pc.printf("0,-90,2 \n\n\n\r");
silvermist 1:cc3c774c218a 366 }
silvermist 1:cc3c774c218a 367 else if( ((yaw_z > 60) && (yaw_z < 100))&& ((roll_x > 60) && (roll_x < 100)) && ((pitch_y > -10) && (pitch_y < 10)) ){
silvermist 1:cc3c774c218a 368 Rotate_Roll = 180; // 180,90,2
silvermist 1:cc3c774c218a 369 Rotate_Pitch = sata0(pitch_y);
silvermist 1:cc3c774c218a 370 Rotate_Yaw = 0;
silvermist 1:cc3c774c218a 371 pc.printf("180,90,2 \n\n\n\r");
silvermist 1:cc3c774c218a 372 }
silvermist 1:cc3c774c218a 373 else if ( ((yaw_z > 50) && (yaw_z < 90))&& ((roll_x > -110) && (roll_x < -70)) && ((pitch_y > -15) && (pitch_y < 15)) ){
silvermist 1:cc3c774c218a 374 Rotate_Roll = 180; // 180,-90,2
silvermist 1:cc3c774c218a 375 Rotate_Pitch = sata0(pitch_y);
silvermist 1:cc3c774c218a 376 Rotate_Yaw = 0;
silvermist 1:cc3c774c218a 377 pc.printf("180,-90,2 \n\n\n\r");
silvermist 1:cc3c774c218a 378 }
silvermist 1:cc3c774c218a 379 else if( ((a1 > 0) && (a1 < 9999)) && ((pitch_y > -85) && (pitch_y < -45)) ){
silvermist 1:cc3c774c218a 380 Rotate_Roll = sata0(roll_x); // 90,0,3
silvermist 1:cc3c774c218a 381 Rotate_Pitch = sata0(pitch_y);
silvermist 1:cc3c774c218a 382 Rotate_Yaw = sata0(yaw_z);
silvermist 1:cc3c774c218a 383 pc.printf("90,0,3 \n\n\n\r");
silvermist 1:cc3c774c218a 384 }
silvermist 1:cc3c774c218a 385 else if( ((a1 > 0) && (a1 < 9999)) && ((pitch_y > 60) && (pitch_y < 85)) ){
silvermist 1:cc3c774c218a 386 Rotate_Roll = sata0(roll_x); // -90,0,3
silvermist 1:cc3c774c218a 387 Rotate_Pitch = sata0(pitch_y);
silvermist 1:cc3c774c218a 388 Rotate_Yaw = sata0(yaw_z);
silvermist 1:cc3c774c218a 389 pc.printf("-90,0,3 \n\n\n\r");
silvermist 1:cc3c774c218a 390 }
silvermist 1:cc3c774c218a 391 else if( ((a1> 0) && (a1 < 9999)) && ((pitch_y > -85) && (pitch_y < -45)) ){
silvermist 1:cc3c774c218a 392 Rotate_Roll = sata0(roll_x); // 90,180,3
silvermist 1:cc3c774c218a 393 Rotate_Pitch = sata0(pitch_y);
silvermist 1:cc3c774c218a 394 Rotate_Yaw = sata0(yaw_z);
silvermist 1:cc3c774c218a 395 pc.printf("90,180,3 \n\n\n\r");
silvermist 1:cc3c774c218a 396 }
silvermist 1:cc3c774c218a 397 else if( ((a1 > 0) && (a1 < 9999)) && ((pitch_y > 60) && (pitch_y < 85)) ){
silvermist 1:cc3c774c218a 398 Rotate_Roll = sata0(roll_x); // 90,180,3
silvermist 1:cc3c774c218a 399 Rotate_Pitch = sata0(pitch_y);
silvermist 1:cc3c774c218a 400 Rotate_Yaw = sata0(yaw_z);
silvermist 1:cc3c774c218a 401 pc.printf("90,180,3 \n\n\n\r");
silvermist 1:cc3c774c218a 402 }
silvermist 1:cc3c774c218a 403 else if( ((yaw_z > -105) && (yaw_z < -75))&& ((roll_x > -20) && (roll_x < 35)) && ((pitch_y > -20) && (pitch_y < 30)) ){
silvermist 1:cc3c774c218a 404 //B y+ 180,180,1
silvermist 1:cc3c774c218a 405 Rotate_Roll = sata0(roll_x);
silvermist 1:cc3c774c218a 406 Rotate_Pitch = sata0(pitch_y);
silvermist 1:cc3c774c218a 407 Rotate_Yaw = sata0(yaw_z);
silvermist 1:cc3c774c218a 408 pc.printf("180,180,1 \n\n\n\r");
silvermist 1:cc3c774c218a 409 }
silvermist 1:cc3c774c218a 410 else if( ((yaw_z > 140) && (yaw_z < 175)) && ((roll_x > -40) && (roll_x < 25)) && ((pitch_y > -35) && (pitch_y < 25)) ){
silvermist 1:cc3c774c218a 411 //F y- 0,0,1
silvermist 1:cc3c774c218a 412
silvermist 1:cc3c774c218a 413 Rotate_Roll = sata0(roll_x);
silvermist 1:cc3c774c218a 414 Rotate_Pitch = sata0(pitch_y);
silvermist 1:cc3c774c218a 415 Rotate_Yaw = sata0(yaw_z);
silvermist 1:cc3c774c218a 416 pc.printf("0,0,1 \n\n\n\r");
silvermist 1:cc3c774c218a 417 }
silvermist 1:cc3c774c218a 418 // else if( ((yaw_z > -155) && (yaw_z < -130)) && ((roll_x > -25) && (roll_x < 10)) && ((pitch_y > -20) && (pitch_y < 15)) ){
silvermist 1:cc3c774c218a 419 // //L x- 180,180,1
silvermist 1:cc3c774c218a 420 //
silvermist 1:cc3c774c218a 421 // Rotate_Roll = roll_x;
silvermist 1:cc3c774c218a 422 // Rotate_Pitch = pitch_y;
silvermist 1:cc3c774c218a 423 // Rotate_Yaw = yaw_z;
silvermist 1:cc3c774c218a 424 // pc.printf("180,180,1 \n\n\n\r");
silvermist 1:cc3c774c218a 425 //
silvermist 1:cc3c774c218a 426 // }
silvermist 1:cc3c774c218a 427 //else if( ((yaw_z > -150) && (yaw_z < -88)) && ((roll_x > 70) && (roll_x < 110)) && ((pitch_y > -20) && (pitch_y < 35))){
silvermist 1:cc3c774c218a 428 // z++; //T z+
silvermist 1:cc3c774c218a 429 // ztd = 1;
silvermist 1:cc3c774c218a 430 // Rotate_Roll = roll_x
silvermist 1:cc3c774c218a 431 // Rotate_Pitch = pitch_y
silvermist 1:cc3c774c218a 432 // Rotate_Yaw = yaw_z
silvermist 1:cc3c774c218a 433 //
silvermist 1:cc3c774c218a 434 // }
silvermist 1:cc3c774c218a 435 // else if( ((yaw_z > -125) && (yaw_z < -100)) && ((roll_x > -100) && (roll_x < -75)) && ((pitch_y > -25) && (pitch_y < 35))){
silvermist 1:cc3c774c218a 436 // z--; //D z-
silvermist 1:cc3c774c218a 437 // ztd = -1;
silvermist 1:cc3c774c218a 438 // Rotate_Roll = roll_x
silvermist 1:cc3c774c218a 439 // Rotate_Pitch = pitch_y
silvermist 1:cc3c774c218a 440 // Rotate_Yaw = yaw_z
silvermist 1:cc3c774c218a 441 // }
silvermist 1:cc3c774c218a 442 //
silvermist 1:cc3c774c218a 443 else if( ((yaw_z > -80) && (yaw_z < -40)) && ((roll_x > 160) || (roll_x < -160)) && ((pitch_y > -15) && (pitch_y < 15))){
silvermist 1:cc3c774c218a 444 //180,0,4
silvermist 1:cc3c774c218a 445 Rotate_Roll = sata0(roll_x);
silvermist 1:cc3c774c218a 446 Rotate_Pitch = sata0(pitch_y);
silvermist 1:cc3c774c218a 447 Rotate_Yaw = sata0(yaw_z);
silvermist 1:cc3c774c218a 448 pc.printf("180,0,4");
silvermist 1:cc3c774c218a 449
silvermist 1:cc3c774c218a 450 }
silvermist 1:cc3c774c218a 451 // else if( ((yaw_z > 160) || (yaw_z < -165)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -10) && (pitch_y < 10)) ){
silvermist 1:cc3c774c218a 452 // x++;
silvermist 1:cc3c774c218a 453 // xlr = 1;
silvermist 1:cc3c774c218a 454 // Rotate_Roll = roll_x
silvermist 1:cc3c774c218a 455 // Rotate_Pitch = pitch_y
silvermist 1:cc3c774c218a 456 // Rotate_Yaw = yaw_z
silvermist 1:cc3c774c218a 457 // }
silvermist 1:cc3c774c218a 458 else if( ((yaw_z > 100) && (yaw_z < 130)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -20) && (pitch_y < 15))){
silvermist 1:cc3c774c218a 459 //0,180,4
silvermist 1:cc3c774c218a 460 Rotate_Roll = sata0(roll_x);
silvermist 1:cc3c774c218a 461 Rotate_Pitch = sata0(pitch_y);
silvermist 1:cc3c774c218a 462 Rotate_Yaw = sata0(yaw_z);
silvermist 1:cc3c774c218a 463 pc.printf("0,180,4");
silvermist 1:cc3c774c218a 464 }
silvermist 1:cc3c774c218a 465 // else if( ((yaw_z > 45) && (yaw_z < 65)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -15) && (pitch_y < 15)) ){
silvermist 1:cc3c774c218a 466 // x--; //L x-
silvermist 1:cc3c774c218a 467 // xlr = -1;
silvermist 1:cc3c774c218a 468 // Rotate_Roll = roll_x
silvermist 1:cc3c774c218a 469 // Rotate_Pitch = pitch_y
silvermist 1:cc3c774c218a 470 // Rotate_Yaw = yaw_z
silvermist 1:cc3c774c218a 471 //
silvermist 1:cc3c774c218a 472 // }
silvermist 1:cc3c774c218a 473
silvermist 1:cc3c774c218a 474 return (1);
silvermist 1:cc3c774c218a 475
silvermist 1:cc3c774c218a 476 }