Chawisorn samrit
kor bork wa cop koy ma
Fork of
testIMU2_copy2
by 
OX
Diff: main.cpp
- Revision:
- 1:cc3c774c218a
- Parent:
- 0:77a7d1a1c6db
diff -r 77a7d1a1c6db -r cc3c774c218a main.cpp
--- a/main.cpp Sat Dec 03 18:21:47 2016 +0000
+++ b/main.cpp Mon Dec 05 08:48:17 2016 +0000
@@ -4,6 +4,16 @@
ZMU9250 a;
Serial pc(D1, D0);
+Serial mas(D8,D2);
+Ticker flipper;
+
+char dete[50];
+ int indx=0;
+ int ii;
+bool anach = false;
+
+char data[50];
+char elem;
float yaw_z, pitch_y, roll_x;
int ir,ip,iy;
@@ -15,34 +25,208 @@
int cube1(int x, int y, int z);
int seccon(float roll ,float pitch, float yaw);
int round(int rt_x, int rt_y, int rt_z);
+int sata0(int neww);
+int roo(int a1);
+
+int Rotate_Roll, Rotate_Pitch, Rotate_Yaw, A, B, C ;
+int rmap[5][5][5];
+int map[5][5][5];
+char yaw_buf1 = 0;
+char yaw_buf2 = 0;
+char yaw_buf3 = 0;
+char inttost[50];
+
+DigitalIn op_roll(USER_BUTTON);
+ int Cross(int degree)
+ {
+ if (degree == 0)
+ {
+ return 1;
+ }else if (degree == 180)
+ {
+ return -1;
+ }else
+ {
+ return 0;
+ }
+ }
+
+ int Zine(int degree)
+ {
+ if (degree == 90)
+ {
+ return 1;
+ }else if (degree == -90)
+ {
+ return -1;
+ }else
+ {
+ return 0;
+ }
+ }
+void Sent(char arrayi[],float it,int siz)
+
+{
+ for (int i = 0;i<siz;i++)
+ {
+ pc.printf("%c",arrayi[i]);
+ mas.putc(arrayi[i]);
+ wait(it);
+ }
+}
-DigitalIn op_roll(USER_BUTTON);
+void getStr()
+{
+ pc.printf("x\n");
+ while (mas.readable())
+ {
+
+ elem = mas.getc();
+ pc.printf("char is %d\n\r",elem);
+ if ((elem == '\n'))
+ {
+ pc.printf("string is %s\n\r",data);
+ for(int i=0;i<indx;i++)
+ {
+ if (indx > 2)
+ {
+ dete[i] = data[i];
+ }
+ }
+ for(int i=0;i<30;i++)
+ {
+ data[i] = '\0';
+ }
+ indx = 0;
+ break;
+ }else
+ {
+ data[indx] = elem;
+ indx += 1;
+ }
+
+ }
+}
+
+ void Rotate(int role,int pit,int yaw)
+ {pc.printf("role %d\t pit %d\t yaw %d\n\n\n\r",role , pit ,yaw);
+ int i=2,j=3,k=4;
+ if ((role == 90) || (role == -90))
+ {
+ /*for (int i=0;i<5;i++)
+ {
+ for (int j=0;j<5;j++)
+ {
+ for(int k=0;k<5;k++)
+ {*/
+ A = ((i-2)*Cross(pit))-((k-2)*Zine(pit) )+2;
+ B = ((j-2)*Cross(yaw))-((i-2)*Zine(pit)*Zine(yaw))-((k-2)*Zine(yaw)*Cross(pit) )+2;
+ C = ((j-2)*Zine(yaw))+((i-2)*Cross(yaw)*Zine(pit))+((k-2)*Cross(yaw)*Cross(pit) )+2;
+ pc.printf("from %d %d %d to %d %d %d\t=1 \n\n\n\r",i,j,k,A,B,C);
+
+ // }
+ //}
+ //}
+ }else if ((pitch == 90) || (pitch == -90))
+ {
+ /*for (int i=0;i<5;i++)
+ {
+ for (int j=0;j<5;j++)
+ {
+ for(int k=0;k<5;k++)
+ {*/
+ A = ((i-2)*Cross(yaw))+((j-2)*Zine(role)*Zine(yaw))+((k-2)*Zine(yaw)*Cross(role))+2;
+ B = ((j-2)*Cross(role))-((k-2)*Zine(role))+2;
+ C = -((i-2)*Zine(yaw))+((j-2)*Cross(yaw)*Zine(role))+((k-2)*Cross(yaw)*Cross(role))+2;
+ pc.printf("from %d %d %d to %d %d %d\t=2 \n\n\n\r",i,j,k,A,B,C);
+
+ // }
+ //}
+ // }
+ }else
+ {
+ /*for (int i=0;i<5;i++)
+ {
+ for (int j=0;j<5;j++)
+ {
+ for(int k=0;k<5;k++)
+ {*/
+ A = ((i-2)*Cross(yaw))-((j-2)*Zine(yaw))+2;
+ B = ((i-2)*Zine(yaw))+((j-2)*Cross(yaw))+2;
+ C = k;
+ pc.printf("from %d %d %d to %d %d %d\t=3 \n\n\n\r",i,j,k,A,B,C);
+
+ //}
+ //}
+ // }
+ }
+ for (int i=0;i<5;i++)
+ {
+ for (int j=0;j<5;j++)
+ {
+ for(int k=0;k<5;k++)
+ {
+
+ map[i][j][k]= rmap[i][j][k];
+
+ }
+ }
+ }
+ }
+
int main(){
+flipper.attach(&getStr,1.0);
while(1){
+ pc.printf("ddddd");
+
wait(0.1);
a.Update();
yaw_z = a.Yaw();
pitch_y = a.Pitch();
roll_x = a.Roll();
+ // pc.printf("data %d\n",dete);
+// pc.printf("data %c\n",yaw_buf);
-// pc.printf("Yaw_z %f\t Pitch_y %f\t Roll_x %f\n", yaw_z, pitch_y, roll_x);
-
+
+
+ if ((dete[0]=='h')||(dete[1]=='e')||(dete[2]=='l')||(dete[3]=='y')){
+
+ //Sent("999\n",0.1,5);
+
+ sprintf(inttost,"%d",int(yaw_z));
+ //yaw_buf1=int((int(yaw_z)%1000)/100)+48;
+// yaw_buf2=int((int(yaw_z)%100)/10)+48;
+// yaw_buf3=int((int(yaw_z)%10))+48;
+// Sent("hely",0.1,4);
+// Sent(yaw_buf1,0.1,1);
+// Sent(yaw_buf2,0.1,1);
+ Sent(inttost,0.1,5);
+// pc.printf("data %c\n",yaw_buf);
+ Sent("\n",0.1,1);
+
+ }
+
+
+ pc.printf("Yaw_z %f\t Pitch_y %f\t Roll_x %f\n\n\n\r", yaw_z, pitch_y, roll_x);
+
//l = cube1(3,3,3);
-
- //pc.printf("%d\t Yaw_z %f\t Pitch_y %f\t Roll_x %f\n",l , yaw_z, pitch_y, roll_x); //add
+ //l = roo(10);
+ //Rotate(Rotate_Roll, Rotate_Pitch, Rotate_Yaw);
+ //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
+ //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));
- 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
+ //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
// pc.printf( "%d\n" ,seccon(roll_x, pitch_y, yaw_z));
// pc.printf("========\n"); //it
- if(op_roll == 0){
-
- l = cube1(3,3,3);
- }
+ // if(op_roll == 0){
+//
+// l = cube1(3,3,3);
+// }
}
@@ -50,256 +234,243 @@
int cube1(int x, int y, int z){
- if( ((pitch_y > -30) && (pitch_y < 30)) ){ // vCC L leg D
+ // if( ((pitch_y > -30) && (pitch_y < 30)) ){ // vCC L leg D
- if( ((yaw_z > 36) && (yaw_z < 113)) && ((roll_x > -40) && (roll_x < 40)) ){
+ if( ((yaw_z > -40) && (yaw_z < 0)) && ((roll_x > -30) && (roll_x < 30)) && ((pitch_y > -20) && (pitch_y < 30)) ){
x++; //R x+
xlr = 1;
-
-
}
- if( ((yaw_z > -101) && (yaw_z < 35)) && ((roll_x > -40) && (roll_x < 40)) ){
+ else if( ((yaw_z > -105) && (yaw_z < -75))&& ((roll_x > -20) && (roll_x < 35)) && ((pitch_y > -20) && (pitch_y < 30)) ){
y--; //B y+
yfb = -1;
}
- if( ((yaw_z > 110) && (yaw_z < 179)) && ((roll_x > -40) && (roll_x < 40)) ){
+ else if( ((yaw_z > 140) && (yaw_z < 175)) && ((roll_x > -40) && (roll_x < 25)) && ((pitch_y > -35) && (pitch_y < 25)) ){
y++; //F y-
yfb = 1;
}
- if( ((yaw_z > -180) && (yaw_z < -100)) && ((roll_x > -40) && (roll_x < 40)) ){
+ else if( ((yaw_z > -155) && (yaw_z < -130)) && ((roll_x > -25) && (roll_x < 10)) && ((pitch_y > -20) && (pitch_y < 15)) ){
x--; //L x-
xlr = -1;
}
- if((roll_x > 41) && (roll_x < 130) ){
+ else if( ((yaw_z > -150) && (yaw_z < -88)) && ((roll_x > 70) && (roll_x < 110)) && ((pitch_y > -20) && (pitch_y < 35))){
z++; //T z+
ztd = 1;
}
- if( (roll_x > -130) && (roll_x < -41)){
+ else if( ((yaw_z > -125) && (yaw_z < -100)) && ((roll_x > -100) && (roll_x < -75)) && ((pitch_y > -25) && (pitch_y < 35))){
z--; //D z-
ztd = -1;
}
-
- }
+
+ else if( ((yaw_z > -80) && (yaw_z < -40)) && ((roll_x > 160) || (roll_x < -160)) && ((pitch_y > -15) && (pitch_y < 15))){
+ y++;
+ yfb = 1;
+
+ }
+ else if( ((yaw_z > 160) || (yaw_z < -165)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -10) && (pitch_y < 10)) ){
+ x++;
+ xlr = 1;
+ }
+ else if( ((yaw_z > 100) && (yaw_z < 130)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -20) && (pitch_y < 15))){
+ y--;
+ yfb = -1;
+ }
+ else if( ((yaw_z > 45) && (yaw_z < 65)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -15) && (pitch_y < 15)) ){
+ x--; //L x-
+ xlr = -1;
+
+ }
+ int rt_x , rt_y , rt_z;
+ int a = rt_x - 3;
+ int b = rt_y - 3;
+ int c = rt_z - 3;
+ if(rt == 'x'){
+ rt_x = a + 3; //x
+ rt_y = (b * cos(sata*(PI/180))+ c * sin(sata*(PI/180)) )+ 3;
+ rt_z = (-b * sin(sata*(PI/180))+ c * cos(sata*(PI/180)) ) + 3;
+ }
+ if(rt == 'y'){
+ rt_x = (a * cos(sata*(PI/180)) - c * sin(sata*(PI/180)) )+ 3; // y
+ rt_y = b + 3;
+ rt_z = (a * sin(sata*(PI/180)) + c * cos(sata*(PI/180)) )+ 3;
+ }
+ if(rt == 'z'){
+ rt_x = (a * cos(sata*(PI/180))+ b * sin(sata*(PI/180)) )+ 3; //z
+ rt_y = (b * cos(sata*(PI/180))- a * sin(sata*(PI/180)) )+ 3;
+ rt_z = c + 3;
+ }
+
return (x*100+y*10+z);
}
-
-
-
-
- int seccon(float roll ,float pitch, float yaw){
- if( ( ((yaw > -50) && (yaw < -30)) || ((yaw > -145) && (yaw < -125)) || ((yaw > 140) && (yaw < 160))||((yaw > 0) && (yaw < -20)) )
- && ((roll_x > -10 )&& (roll_x < 10))
- && (( pitch > -10 )&&(pitch < 10)) //L T R D
- ){
- ir = roll;
- ip = pitch;
- iy = yaw;
- return 1;
+ int sata0(int neww){
+ if(neww>=0){
+ if((neww <= 45) && (neww >= 0)){
+ return 0;
+ }
+ if((neww <= 90) && (neww >= 46)){
+ return 90;
+ }
+ if((neww <= 135) && (neww >= 91)){
+ return -90;
+ }
+ if((neww <= 180) && (neww >= 136)){
+ return 180;
+ }
}
else{
- return 0;
+ if((neww <= 0) && (neww >= -45)){
+ return 180;
}
- }
-
-
- int round(int rt_x, int rt_y, int rt_z){
-
- if( (pitch_y > -30) && (pitch_y < 30) ){ // vCC L leg D
-
- if( ((yaw_z > 36) && (yaw_z < 113)) && ( ((roll_x > -40) && (roll_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( ((yaw_z > -74) && (yaw_z < 35)) && ( ((roll_x > -40) && (roll_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((neww <= -46) && (neww >= -90)){
+ return -90;
}
- if( ((yaw_z > 110) && (yaw_z < 179)) &&( ((roll_x > -40) && (roll_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( ((yaw_z > -180) && (yaw_z < -75)) &&( ((roll_x > -40) && (roll_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((neww <= -91) && (neww >= -135)){
+ return 90;
}
- if((roll_x > 41) && (roll_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( (roll_x > -130) && (roll_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';
- }
-
+ if((neww <= -136) && (neww >= -180)){
+ return 0;
}
}
-
- rt_x = rt_x - 3;
- rt_y = rt_y - 3;
- rt_z = rt_z - 3;
- if(rt == 'x'){
- rt_x = rt_x + 3; //x
- rt_y = (rt_y * cos(sata*(PI/180))+ rt_z * sin(sata*(PI/180)) )+ 3;
- rt_z = (-rt_y * sin(sata*(PI/180))+ rt_z * cos(sata*(PI/180)) ) + 3;
- }
- if(rt == 'y'){
- rt_x = (rt_x * cos(sata*(PI/180)) - rt_z * sin(sata*(PI/180)) )+ 3; // y
- rt_y = rt_y + 3;
- rt_z = (rt_x * sin(sata*(PI/180)) + rt_z * cos(sata*(PI/180)) )+ 3;
- }
- if(rt == 'z'){
- rt_x = (rt_x * cos(sata*(PI/180))+ rt_y * sin(sata*(PI/180)) )+ 3; //z
- rt_y = (rt_y * cos(sata*(PI/180))- rt_x * sin(sata*(PI/180)) )+ 3;
- rt_z = rt_z + 3;
}
- pc.printf("rt_x %d\t", rt_x);
- pc.printf("rt_y %d\t", rt_y);
- pc.printf("rt_z %d \n\n\n\n\r", rt_z);
+
- return (rt_x*100 + rt_y*10 + rt_z);
- }
-
\ No newline at end of file
+ int roo(int a1){
+
+
+ //if( ((yaw_z > -40) && (yaw_z < 0)) && ((roll_x > -30) && (roll_x < 30)) && ((pitch_y > -20) && (pitch_y < 30)) ){
+// //R x+
+// Rotate_Roll = roll_x;
+// Rotate_Pitch = pitch_y;
+// Rotate_Yaw = yaw_z;
+//
+//
+// }
+ if ( ((yaw_z > -150) && (yaw_z < -110))&& ((roll_x > 60) && (roll_x < 100)) && ((pitch_y > -10) && (pitch_y < 10)) ){
+
+ Rotate_Roll = 0; // 0,90,2
+ Rotate_Pitch = sata0(pitch_y);
+ Rotate_Yaw = 180;
+ pc.printf("0,90,2 \n\n\n\r");
+ }
+ else if ( ((yaw_z > -140) && (yaw_z < -100))&& ((roll_x > -100) && (roll_x < -60)) && ((pitch_y > -10) && (pitch_y < 10)) ){
+ Rotate_Roll = 0; // 0,-90,2
+ Rotate_Pitch = sata0(pitch_y);
+ Rotate_Yaw = 180;
+ pc.printf("0,-90,2 \n\n\n\r");
+ }
+ else if( ((yaw_z > 60) && (yaw_z < 100))&& ((roll_x > 60) && (roll_x < 100)) && ((pitch_y > -10) && (pitch_y < 10)) ){
+ Rotate_Roll = 180; // 180,90,2
+ Rotate_Pitch = sata0(pitch_y);
+ Rotate_Yaw = 0;
+ pc.printf("180,90,2 \n\n\n\r");
+ }
+ else if ( ((yaw_z > 50) && (yaw_z < 90))&& ((roll_x > -110) && (roll_x < -70)) && ((pitch_y > -15) && (pitch_y < 15)) ){
+ Rotate_Roll = 180; // 180,-90,2
+ Rotate_Pitch = sata0(pitch_y);
+ Rotate_Yaw = 0;
+ pc.printf("180,-90,2 \n\n\n\r");
+ }
+ else if( ((a1 > 0) && (a1 < 9999)) && ((pitch_y > -85) && (pitch_y < -45)) ){
+ Rotate_Roll = sata0(roll_x); // 90,0,3
+ Rotate_Pitch = sata0(pitch_y);
+ Rotate_Yaw = sata0(yaw_z);
+ pc.printf("90,0,3 \n\n\n\r");
+ }
+ else if( ((a1 > 0) && (a1 < 9999)) && ((pitch_y > 60) && (pitch_y < 85)) ){
+ Rotate_Roll = sata0(roll_x); // -90,0,3
+ Rotate_Pitch = sata0(pitch_y);
+ Rotate_Yaw = sata0(yaw_z);
+ pc.printf("-90,0,3 \n\n\n\r");
+ }
+ else if( ((a1> 0) && (a1 < 9999)) && ((pitch_y > -85) && (pitch_y < -45)) ){
+ Rotate_Roll = sata0(roll_x); // 90,180,3
+ Rotate_Pitch = sata0(pitch_y);
+ Rotate_Yaw = sata0(yaw_z);
+ pc.printf("90,180,3 \n\n\n\r");
+ }
+ else if( ((a1 > 0) && (a1 < 9999)) && ((pitch_y > 60) && (pitch_y < 85)) ){
+ Rotate_Roll = sata0(roll_x); // 90,180,3
+ Rotate_Pitch = sata0(pitch_y);
+ Rotate_Yaw = sata0(yaw_z);
+ pc.printf("90,180,3 \n\n\n\r");
+ }
+ else if( ((yaw_z > -105) && (yaw_z < -75))&& ((roll_x > -20) && (roll_x < 35)) && ((pitch_y > -20) && (pitch_y < 30)) ){
+ //B y+ 180,180,1
+ Rotate_Roll = sata0(roll_x);
+ Rotate_Pitch = sata0(pitch_y);
+ Rotate_Yaw = sata0(yaw_z);
+ pc.printf("180,180,1 \n\n\n\r");
+ }
+ else if( ((yaw_z > 140) && (yaw_z < 175)) && ((roll_x > -40) && (roll_x < 25)) && ((pitch_y > -35) && (pitch_y < 25)) ){
+ //F y- 0,0,1
+
+ Rotate_Roll = sata0(roll_x);
+ Rotate_Pitch = sata0(pitch_y);
+ Rotate_Yaw = sata0(yaw_z);
+ pc.printf("0,0,1 \n\n\n\r");
+ }
+ // else if( ((yaw_z > -155) && (yaw_z < -130)) && ((roll_x > -25) && (roll_x < 10)) && ((pitch_y > -20) && (pitch_y < 15)) ){
+// //L x- 180,180,1
+//
+// Rotate_Roll = roll_x;
+// Rotate_Pitch = pitch_y;
+// Rotate_Yaw = yaw_z;
+// pc.printf("180,180,1 \n\n\n\r");
+//
+// }
+ //else if( ((yaw_z > -150) && (yaw_z < -88)) && ((roll_x > 70) && (roll_x < 110)) && ((pitch_y > -20) && (pitch_y < 35))){
+// z++; //T z+
+// ztd = 1;
+// Rotate_Roll = roll_x
+// Rotate_Pitch = pitch_y
+// Rotate_Yaw = yaw_z
+//
+// }
+// else if( ((yaw_z > -125) && (yaw_z < -100)) && ((roll_x > -100) && (roll_x < -75)) && ((pitch_y > -25) && (pitch_y < 35))){
+// z--; //D z-
+// ztd = -1;
+// Rotate_Roll = roll_x
+// Rotate_Pitch = pitch_y
+// Rotate_Yaw = yaw_z
+// }
+//
+ else if( ((yaw_z > -80) && (yaw_z < -40)) && ((roll_x > 160) || (roll_x < -160)) && ((pitch_y > -15) && (pitch_y < 15))){
+ //180,0,4
+ Rotate_Roll = sata0(roll_x);
+ Rotate_Pitch = sata0(pitch_y);
+ Rotate_Yaw = sata0(yaw_z);
+ pc.printf("180,0,4");
+
+ }
+// else if( ((yaw_z > 160) || (yaw_z < -165)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -10) && (pitch_y < 10)) ){
+// x++;
+// xlr = 1;
+// Rotate_Roll = roll_x
+// Rotate_Pitch = pitch_y
+// Rotate_Yaw = yaw_z
+// }
+ else if( ((yaw_z > 100) && (yaw_z < 130)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -20) && (pitch_y < 15))){
+ //0,180,4
+ Rotate_Roll = sata0(roll_x);
+ Rotate_Pitch = sata0(pitch_y);
+ Rotate_Yaw = sata0(yaw_z);
+ pc.printf("0,180,4");
+ }
+// else if( ((yaw_z > 45) && (yaw_z < 65)) && ((roll_x > 165) || (roll_x < -165)) && ((pitch_y > -15) && (pitch_y < 15)) ){
+// x--; //L x-
+// xlr = -1;
+// Rotate_Roll = roll_x
+// Rotate_Pitch = pitch_y
+// Rotate_Yaw = yaw_z
+//
+// }
+
+ return (1);
+
+ }
\ No newline at end of file