Diff: main.cpp

Revision:

9:01146bd5850c

Parent:

8:959fe4717597

--- a/main.cpp	Mon Nov 18 06:18:13 2019 +0000
+++ b/main.cpp	Tue Nov 19 06:16:20 2019 +0000
@@ -83,7 +83,7 @@
 int o=1;
 int old_right1=0;
 int old_left1=0;
-int p=5;
+int p=6;
 int q=0;
 int old_ue1=0;
 int old_shita=0;
@@ -741,82 +741,93 @@
 void kaisyu(){
     if(q==0){
         if(kaisyu_arm!=0){
-        if(old_square!=square){
-            old_square=square;
-            if(square==1){
-                if(x==0){
-                    /*
-                    data_servo=0x02;
-                    send_data(0x18,data_servo);
-                    */
-                    p=1;
-                    x=1;
-                    timer2.start();
-                }else if(x==1){
-                    p=4;
-                    x=0;
+            if(old_square!=square){
+                old_square=square;
+                if(square==1){
+                    if(x==0&&p==6){
+                        /*
+                        data_servo=0x02;
+                        send_data(0x18,data_servo);
+                        */
+                        p=1;
+                        x=1;
+                        timer2.start();
+                    }else if(x==1&&p==3){
+                        p=4;
+                        x=0;
+                    }
                 }
             }
-        }
         
-         switch(p){
-            
-            case 1:
-            y=0;
-            data_servo=0x07;
-            send_data(0x18,data_servo);
-            //timer2.start();
-            if(timer2.read()>1.5f){
-                timer2.stop();
-                timer2.reset();
-                p=2;
-            }
-            break;
-            
-            case 2:
-            data7=0x00;
-            send_data(0x24,data7);
-            if(get_data_rs232 & 0x10){
-                data7=0x10;
+             switch(p){
+                
+                case 1:
+                y=0;
+                data_servo=0x07;
+                send_data(0x18,data_servo);
+                //timer2.start();
+                if(timer2.read()>1.5f){
+                    timer2.stop();
+                    timer2.reset();
+                    p=2;
+                }
+                break;
+                
+                case 2:
+                data7=0x00;
                 send_data(0x24,data7);
-                p=3;
-            }
-            break;
-            
-            case 3:
-            /*
-            if(triangle!=old_triangle){
-                old_triangle=triangle;
-                if(triangle==1){
-                    if(y==0){
-                        data_servo=0x08;
-                        send_data(0x18,data_servo);
-                        y=1;
-                    }else if(y==1){
-                        data_servo=0x07;
-                        send_data(0x18,data_servo);
-                        y=0;
+                if(get_data_rs232 & 0x10){
+                    data7=0x10;
+                    send_data(0x24,data7);
+                    p=3;
+                }
+                break;
+                
+                case 3:
+                /*
+                if(triangle!=old_triangle){
+                    old_triangle=triangle;
+                    if(triangle==1){
+                        if(y==0){
+                            data_servo=0x08;
+                            send_data(0x18,data_servo);
+                            y=1;
+                        }else if(y==1){
+                            data_servo=0x07;
+                            send_data(0x18,data_servo);
+                            y=0;
+                        }
                     }
-                }
-            }*/
-
-            break;
-            
-            case 4:
-            data7=0xff;
-            send_data(0x24,data7);
-            if(get_data_rs232 & 0x20){
-                data7=0x10;
+                }*/
+    
+                break;
+                
+                case 4:
+                data7=0xff;
                 send_data(0x24,data7);
                 p=5;
+                /*
+                if(get_data_rs232 & 0x20){
+                    data7=0x10;
+                    send_data(0x24,data7);
+                    p=5;
+                }*/
+                break;
+                
+                case 5:
+                if(get_data_rs232 & 0x20){
+                    data7=0x10;
+                    send_data(0x24,data7);
+                    p=6;
+                }
+                break;
+                
+                default:
+                    p=6;
+                break;
+                
+                  
             }
-            break;
-            
-            default:
-                p=5;
-            break;
-            
-          }    
         }
     }
 }