aa

Dependencies:   mbed TrapezoidControl QEI

Revision:
39:f89166d7411c
Parent:
38:391a0c8e4c16
Child:
40:7f93984e8ba1
diff -r 391a0c8e4c16 -r f89166d7411c System/Process/Process.cpp
--- a/System/Process/Process.cpp	Thu Sep 26 09:56:28 2019 +0000
+++ b/System/Process/Process.cpp	Sat Sep 28 02:49:10 2019 +0000
@@ -106,6 +106,7 @@
 
 void LedMode(int led)
 {
+    /*
     switch(led) {
         case 1:
             POTENTIOMETER::dio[0]= 1;
@@ -132,7 +133,7 @@
             POTENTIOMETER::dio[3]= 0;
             break;
     }
-    /*
+    */
     if(led/8>=1) {
         POTENTIOMETER::dio[3]= 1;
         led=led%8;
@@ -160,13 +161,13 @@
     } else {
         POTENTIOMETER::dio[0]= 0;
     }
-    */
 }
 int Twsh;
 bool UP_flag = false;
 bool SW_flag = false;
 bool Air_flag = false;
 bool zyouge;
+bool zyougedo;
 bool dz1=true;
 bool dz1i=false;
 bool dz2=true;
@@ -187,9 +188,9 @@
 int count=100000;//wait代替え
 
 ///*********PWM調整用*********///
-int fast =40;
-int normal = 30;
-int slow = 20;
+int fast =80;
+int normal = 60;
+int slow = 40;
 
 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
 
@@ -378,6 +379,9 @@
     if(DWSW) {
         zyouge=true;
     }
+    if(DOSW) {
+        zyougedo=true;
+    }
     if(AIRSW) {
         if(SW_flag==false) {
             if(Air_flag==false) {
@@ -413,9 +417,15 @@
                 zyouge=false;
             }
         }
-    } else {
-        motor[MOTOR_0].pwm = 100;
-        motor[MOTOR_0].dir = BRAKE;
+    }
+    if(zyougedo) {
+        motor[MOTOR_0].pwm = 50;
+        motor[MOTOR_0].dir = BACK;
+        if(DOLS) {
+            motor[MOTOR_0].pwm = 100;
+            motor[MOTOR_0].dir = BRAKE;
+            zyougedo=false;
+        }
     }
 }
 
@@ -1081,6 +1091,9 @@
         motor[TIRE_BL].dir = BACK;
         if(g[3]==0) {
             count=0;
+            if(Twsh==2) {
+                solenoid.solenoid2 = SOLENOID_OFF;
+            }
             mode=40;
         }
     } else if(mode==40) {
@@ -1412,7 +1425,7 @@
 #if USE_PROCESS_NUM>3
 static void Process3()
 {
-    
+
 }
 #endif
 
@@ -1422,13 +1435,13 @@
     LedMode(4);
     count++;
     if(mode==1) {//スタートゾーンから白線検知
-        motor[TIRE_FR].pwm = normal;
+        motor[TIRE_FR].pwm = 40;
         motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = normal;
+        motor[TIRE_FL].pwm = 40;
         motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = normal;
+        motor[TIRE_BR].pwm = 40;
         motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = normal;
+        motor[TIRE_BL].pwm = 35;
         motor[TIRE_BL].dir = BACK;
         if(g[2]==0) {
             count=100000;
@@ -1436,13 +1449,13 @@
             mode=3;
         }
     } else if(mode==3) { //横ライントレースから縦ライントレースへ
-        motor[TIRE_FR].pwm = normal;
+        motor[TIRE_FR].pwm = 40;
         motor[TIRE_FR].dir = FOR;
         motor[TIRE_FL].pwm = 0;
         motor[TIRE_FL].dir = FREE;
         motor[TIRE_BR].pwm = 0;
         motor[TIRE_BR].dir = FREE;
-        motor[TIRE_BL].pwm = normal;
+        motor[TIRE_BL].pwm = 40;
         motor[TIRE_BL].dir = BACK;
         if(g[0]==0) {
             mode=10;
@@ -1563,7 +1576,7 @@
                 break;
         }
         if(g[0]!=98&&mtc==true) {
-            switch(g[1]) {
+            switch(g[0]) {
                 case 0:
                     mtc=false;
                     break;
@@ -1608,6 +1621,9 @@
             count=0;
             cross=0;
             mode=20;
+            fast=40;
+            normal=30;
+            slow=20;
         }
     } else if(mode==20) {
 
@@ -1774,6 +1790,9 @@
             mode=30;
             cros=0;
             zyouge=true;
+            fast=80;
+            normal=60;
+            slow=40;
         }
     } else if(mode==30) {
         motor[TIRE_FR].pwm = 60;
@@ -1782,15 +1801,13 @@
         motor[TIRE_FL].dir = BACK;
         motor[TIRE_BR].pwm = 60;
         motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 60;
+        motor[TIRE_BL].pwm = 50;
         motor[TIRE_BL].dir = BACK;
         if(g[1]==98) {
             mode=31;
-            count=0;
         }
-        if(counts==false&&g[3]==0) {
-            counts=false;
-            mode=31;
+        if(g[3]==0) {
+            mode=32;
             count=0;
             cros=0;
         }//ゆっくりモードに入れなかった時の保険
@@ -1931,21 +1948,37 @@
         }
         */
     } else if(mode==31) {
-        motor[TIRE_FR].pwm = 0;
-        motor[TIRE_FR].dir = FREE;
-        motor[TIRE_FL].pwm = 30;
+        motor[TIRE_FR].pwm = 15;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 15;
         motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = 30;
+        motor[TIRE_BR].pwm = 15;
         motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 0;
-        motor[TIRE_BL].dir = FREE;
+        motor[TIRE_BL].pwm = 15;
+        motor[TIRE_BL].dir = BACK;
         if(g[3]==0) {
             count=0;
+            mode=32;
+        }
+    } else if(mode==32) {
+        motor[TIRE_FR].pwm = 30;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = 30;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 30;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 30;
+        motor[TIRE_BL].dir = FOR;
+        if(g[3]==0) {
+            count=0;
+            if(Twsh==2) {
+                solenoid.solenoid2 = SOLENOID_OFF;
+            }
             mode=40;
         }
     } else if(mode==40) {
 
-        if(LIB) {
+        if(LIF) {
             mode=42;
         }
 
@@ -1978,7 +2011,7 @@
                     count=0;
                 }
                 mtc2=true;
-                if(cross==3) {
+                if(cross==2) {
                     mode=41;
                     count=0;
                     cross=0;
@@ -2068,51 +2101,59 @@
                     mtc2=false;
                     break;
                 case 255:
-                    motor[TIRE_FL].pwm += 5;
+                    motor[TIRE_FR].pwm += 5;
                     mtc2=false;
                     break;
                 case 253:
-                    motor[TIRE_FL].pwm += 10;
+                    motor[TIRE_FR].pwm += 10;
                     mtc2=false;
                     break;
                 case 254:
-                    motor[TIRE_FL].pwm += 20;
-                    motor[TIRE_BL].pwm = 0;
+                    motor[TIRE_FR].pwm += 20;
+                    motor[TIRE_BR].pwm = 0;
                     mtc2=false;
                     break;
                 case 1:
-                    motor[TIRE_BL].pwm += 5;
+                    motor[TIRE_BR].pwm += 5;
                     mtc2=false;
                     break;
                 case 3:
-                    motor[TIRE_BL].pwm += 10;
+                    motor[TIRE_BR].pwm += 10;
                     mtc2=false;
                     break;
                 case 2:
-                    motor[TIRE_BL].pwm += 20;
-                    motor[TIRE_FL].pwm = 0;
+                    motor[TIRE_BR].pwm += 20;
+                    motor[TIRE_FR].pwm = 0;
                     mtc2=false;
                     break;
             }
         }
     } else if(mode==41) {
-        motor[TIRE_FR].pwm = normal;
+        motor[TIRE_FR].pwm = 30;
         motor[TIRE_FR].dir = BACK;
         motor[TIRE_FL].pwm = 0;
         motor[TIRE_FL].dir = FREE;
         motor[TIRE_BR].pwm = 0;
         motor[TIRE_BR].dir = FREE;
-        motor[TIRE_BL].pwm = normal;
+        motor[TIRE_BL].pwm = 30;
         motor[TIRE_BL].dir = FOR;
         if(g[1]==0) {
             count=0;
             mode=70;
+            normal=60;
+            fast=80;
+            slow=40;
         }
-
     } else if(mode==70)
         switch(g[0]) {
             case 99:
                 if(count>=100000) {
+                    cross++;
+                    mode=100;
+                    count=0;
+                }
+                if(cross==3) {
+                    cross=0;
                     mode=100;
                 }
                 break;
@@ -2205,7 +2246,7 @@
                 break;
         }
     if(g[1]!=98&&mtc==true) {
-        switch(g[0]) {
+        switch(g[1]) {
             case 0:
                 mtc=false;
                 break;
@@ -2245,18 +2286,47 @@
         motor[TIRE_BR].dir = BACK;
         motor[TIRE_BL].pwm = 20;
         motor[TIRE_BL].dir = FOR;
-    }
-    /* else {
-        motor[TIRE_FR].pwm = 0;
+        if(StertSW) {
+            SW_flag = true;
+        }
+        if(SW_flag ==true)
+            motor[TIRE_FR].pwm = 100;
         motor[TIRE_FR].dir = BRAKE;
-        motor[TIRE_FL].pwm = 0;
+        motor[TIRE_FL].pwm = 100;
         motor[TIRE_FL].dir = BRAKE;
-        motor[TIRE_BR].pwm = 0;
+        motor[TIRE_BR].pwm = 100;
         motor[TIRE_BR].dir = BRAKE;
-        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].pwm = 100;
         motor[TIRE_BL].dir = BRAKE;
+        fast=60;
+        normal=40;
+        slow=20;
+        mode =1;
+        cross=0;//十字数える用
+        cros=0;
+        count=100000;//wait代替え
+        UP_flag = false;
+        SW_flag = false;
+        Air_flag = false;
+        zyouge=false;
+        zyougedo=false;
+        dz1=true;
+        dz1i=false;
+        dz2=true;
+        dz2i=false;
+        dz3=true;
+        dz3i=false;
+        dz4=true;
+        dz4i=false;
+        through=false;
+        counts=false;
+        mtc=false;
+        mtc2 = false;
+        current = 0;
+        if(StertSW==false) {
+            SW_flag = false;
+        }
     }
-    */
 }
 #endif