Diff: main.cpp

Revision:

3:7bd1afb46094

Parent:

2:3176040a4777

Child:

4:e3d739bf09b4

--- a/main.cpp	Tue Sep 11 04:39:47 2018 +0000
+++ b/main.cpp	Tue Sep 11 04:48:43 2018 +0000
@@ -295,6 +295,31 @@
         }
         if(i==3) {
             susumu_xl(1,1,709);
+        }
+        if(i==4) {
+            susumu_ang(1,1/3,0);
+        }
+        if(i==5) {
+            susumu_y(1,1,1700);
+        }
+        if(i==6) {
+            motorR.stop();
+            motorL.stop();
+            t.start();
+            if(t<1) {
+                motor_f=0.82;
+                motor_b=0;
+            } else {
+                motor_f=0;
+                motor_b=0;
+                printf("finish");
+                t.reset();
+                i++;
+            }
+        }//gatiasari
+
+
+        if(i==7) {
             t.start();
             if(t<1) {
                 motor_f=0;
@@ -305,127 +330,103 @@
                 printf("finish");
                 t.reset();
             }
-            if(i==4) {
-                susumu_ang(1,1/3,0);
-            }
-            if(i==5) {
-                susumu_y(1,1,1700);
+            if(angle>=-89) {
+                target_R=BASIC_SPEED/5;
+                target_L=BASIC_SPEED/(-5);
             }
-            if(i==6) {
-                motorR.stop();
-                motorL.stop();
-                t.start();
-                if(t<1) {
-                    motor_f=0.82;
-                    motor_b=0;
-                } else {
-                    motor_f=0;
-                    motor_b=0;
-                    printf("finish");
-                    t.reset();
-                    i++;
-                }
-            }//gatiasari
-
-
-            if(i==7) {
-                if(angle>=-89) {
-                    target_R=BASIC_SPEED/5;
-                    target_L=BASIC_SPEED/(-5);
-                }
-                if(angle<=-91) {
-                    target_R=BASIC_SPEED/(-5);
-                    target_L=BASIC_SPEED/-5;
-                }
-                if(angle>-91&angle<-89) {
-                    motorR.stop();
-                    motorL.stop();
-                    wait(0.5);
-                    if(angle>-91&angle<-89) {
-                        i++;
-                    }
-                }
-            }//kakudo tyousei
-            if(i==8) {
-                susumu_xr(1,1,700);
+            if(angle<=-91) {
+                target_R=BASIC_SPEED/(-5);
+                target_L=BASIC_SPEED/-5;
             }
-            if(i==9) {
-                susumu_ang(1/3,1,0);
-            }
-            if(i==10) {
-                susumu_y(1,1,2200);
-            }
-            if(i==11) {
-                susumu_ang(1/3,1,90);
-            }
-            if(i==12) {
-                motorR.stop();
-                motorL.stop();
-                servo.pulsewidth_us(1000);
-                wait(0.5);
-                i++;
-            }
-            if(i==13) {
-                if(angle>=91) {
-                    target_R=BASIC_SPEED/5;
-                    target_L=BASIC_SPEED/(-5);
-                }
-                if(angle<=89) {
-                    target_R=BASIC_SPEED/(-5);
-                    target_L=BASIC_SPEED/5;
-                }
-                if(angle>89&angle<91) {
-                    motorR.stop();
-                    motorL.stop();
-                    wait(0.1);
-                    if(angle>89&angle<91) {
-                        i++;
-                    }
-                }
-            }
-            if(i==14) {
-                susumu_xl(1,1,1000);
-            }
-            if(i==15) {
+            if(angle>-91&angle<-89) {
                 motorR.stop();
                 motorL.stop();
                 wait(0.5);
-                denjiben=1;
-                wait(0.5);
-                servo.pulsewidth_us(1500);
-                wait(0.5);
-                i++;
-            }
-            if(i==16) {
-                susumu_xr(-1,-1,700);
+                if(angle>-91&angle<-89) {
+                    i++;
+                }
             }
-            if(i==17) {
-                susumu_ang(-1/3,-1,0);
-            }
-            if(i==18) {
-                susumu_y(-1,-1,2000);
+        }//kakudo tyousei
+        if(i==8) {
+            susumu_xr(1,1,700);
+        }
+        if(i==9) {
+            susumu_ang(1/3,1,0);
+        }
+        if(i==10) {
+            susumu_y(1,1,2200);
+        }
+        if(i==11) {
+            susumu_ang(1/3,1,90);
+        }
+        if(i==12) {
+            motorR.stop();
+            motorL.stop();
+            servo.pulsewidth_us(1000);
+            wait(0.5);
+            i++;
+        }
+        if(i==13) {
+            if(angle>=91) {
+                target_R=BASIC_SPEED/5;
+                target_L=BASIC_SPEED/(-5);
             }
-            if(i==19) {
-                susumu_ang(-1/3,-1,-90);
+            if(angle<=89) {
+                target_R=BASIC_SPEED/(-5);
+                target_L=BASIC_SPEED/5;
             }
-            if(i==20) {
-                susumu_xl(-1,-1,1100);
-            }
-            if(i==21) {
+            if(angle>89&angle<91) {
                 motorR.stop();
                 motorL.stop();
-                servo.pulsewidth_us(1500);
-                wait(0.5);
-                denjiben=0;
-                wait(0.5);
+                wait(0.1);
+                if(angle>89&angle<91) {
+                    i++;
+                }
+            }
+        }
+        if(i==14) {
+            susumu_xl(1,1,1000);
+        }
+        if(i==15) {
+            motorR.stop();
+            motorL.stop();
+            wait(0.5);
+            denjiben=1;
+            wait(0.5);
+            servo.pulsewidth_us(1500);
+            wait(0.5);
+            i++;
+        }
+        if(i==16) {
+            susumu_xr(-1,-1,700);
+        }
+        if(i==17) {
+            susumu_ang(-1/3,-1,0);
+        }
+        if(i==18) {
+            susumu_y(-1,-1,2000);
+        }
+        if(i==19) {
+            susumu_ang(-1/3,-1,-90);
+        }
+        if(i==20) {
+            susumu_xl(-1,-1,1100);
+        }
+        if(i==21) {
+            motorR.stop();
+            motorL.stop();
+            servo.pulsewidth_us(1500);
+            wait(0.5);
+            denjiben=0;
+            wait(0.5);
 
-                break;
-            }
+            break;
+        }
 
 
-        }//while
-        tgt(0,0);
-        return 0;
-    }
+    }//while
+    tgt(0,0);
+    return 0;
+
 }//intmain