OX
dsafa
Dependencies: mbed
Diff: main.cpp
- Revision:
- 1:29b8881ac539
- Parent:
- 0:77a7d1a1c6db
diff -r 77a7d1a1c6db -r 29b8881ac539 main.cpp
--- a/main.cpp Sat Dec 03 18:21:47 2016 +0000
+++ b/main.cpp Mon Dec 05 09:09:14 2016 +0000
@@ -2,6 +2,7 @@
#include "zmu9250.h"
#include "math.h"
+Ticker flipper;
ZMU9250 a;
Serial pc(D1, D0);
@@ -15,291 +16,455 @@
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 dete[50];
+ int indx=0;
+ int ii;
+bool anach = false;
+
+char data[50];
+char elem;
+
+Serial mas(D8,D2);
+
+
+
+
+// state 0 = initial / start.
+// 1 = waiting for limit switch.
+
+unsigned int turn = 0; // turn.
+
+unsigned int x = 0, y = 0, z = 0;
+
+int helperRoll = 0, helperPitch=0,helperYaw=0;
+
+unsigned int state = 0;
+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++)
+ {
+ mas.putc(arrayi[i]);
+ wait(it);
+ }
+}
-DigitalIn op_roll(USER_BUTTON);
+ 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];
+
+ }
+ }
+ }
+ }
+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;
+ }
+
+ }
+}
int main(){
-
- while(1){
+pc.printf("KKKKKKKKKKKKKKKKKKKKKKKKKKKK");
+flipper.attach(&getStr,1.0);
+ while(1){
+
wait(0.1);
a.Update();
yaw_z = a.Yaw();
pitch_y = a.Pitch();
roll_x = a.Roll();
-// pc.printf("Yaw_z %f\t Pitch_y %f\t Roll_x %f\n", yaw_z, pitch_y, roll_x);
+ 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
- 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
+ // 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\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);
+// }
+
+
+
+ Sent("hely\n",0.1,5); // tell the other box, that we want helper roll.
+ while((dete[0]!='h')||(dete[1]!='e')||(dete[2]!='l')||(dete[3]!='y'))
+ {
+ pc.printf("srt %s\n\r",dete);
}
-
+ helperYaw = int( ((int(dete[4+ii])-48)*100)+((int(dete[5+ii])-48)*10)+(int(dete[6+ii])-48) ); //get roll from another box
+ pc.printf("help %d\n\n\n\n\n\n\n\t",helperYaw);
}
}
-
- int cube1(int x, int y, int 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)) ){
- x++; //R x+
- xlr = 1;
-
-
- }
- if( ((yaw_z > -101) && (yaw_z < 35)) && ((roll_x > -40) && (roll_x < 40)) ){
- y--; //B y+
- yfb = -1;
- }
- if( ((yaw_z > 110) && (yaw_z < 179)) && ((roll_x > -40) && (roll_x < 40)) ){
- y++; //F y-
- yfb = 1;
-
- }
- if( ((yaw_z > -180) && (yaw_z < -100)) && ((roll_x > -40) && (roll_x < 40)) ){
- x--; //L x-
- xlr = -1;
-
- }
- if((roll_x > 41) && (roll_x < 130) ){
- z++; //T z+
- ztd = 1;
-
- }
- if( (roll_x > -130) && (roll_x < -41)){
- z--; //D z-
- ztd = -1;
-
- }
-
- }
-
-
-
- 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;
- }
- else{
- return 0;
- }
- }
-
-
- 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( ((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((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';
- }
-
- }
- }
-
- 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 cube1(int x, int y, int z){
+//
+// // if( ((pitch_y > -30) && (pitch_y < 30)) ){ // vCC L leg D
+//
+// if( ((yaw_z > -40) && (yaw_z < 0)) && ((roll_x > -30) && (roll_x < 30)) && ((pitch_y > -20) && (pitch_y < 30)) ){
+// x++; //R x+
+// xlr = 1;
+// }
+// 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;
+// }
+// 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;
+//
+// }
+// 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;
+//
+// }
+// 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;
+//
+//
+// }
+// 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 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{
+// if((neww <= 0) && (neww >= -45)){
+// return 180;
+// }
+// if((neww <= -46) && (neww >= -90)){
+// return -90;
+// }
+// if((neww <= -91) && (neww >= -135)){
+// return 90;
+// }
+// if((neww <= -136) && (neww >= -180)){
+// return 0;
+// }
+// }
+// }
+//
+//
+// 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( ((al > 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