aa

Dependencies:   mbed TrapezoidControl QEI

Revision:
38:391a0c8e4c16
Parent:
37:7cfa39c41245
Child:
39:f89166d7411c
--- a/System/Process/Process.cpp	Wed Sep 25 11:42:58 2019 +0000
+++ b/System/Process/Process.cpp	Thu Sep 26 09:56:28 2019 +0000
@@ -718,6 +718,9 @@
         }
     } else if(mode==20) {
 
+        if(Twsh==2) {
+            solenoid.solenoid2 = SOLENOID_ON;
+        }
         if(counts==false&&g[0]==0) {
             cros++;
             counts=true;
@@ -878,19 +881,32 @@
             mode=30;
             cros=0;
             zyouge=true;
-            if(Twsh==2) {
-                solenoid.solenoid2 = SOLENOID_ON;
-            }
         }
     } else if(mode==30) {
-
-        if(counts==false&&g[3]==0) {
-            counts=false;
+        motor[TIRE_FR].pwm = 62;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = 60;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 60;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 62;
+        motor[TIRE_BL].dir = FOR;
+        if(g[0]==98) {
             mode=31;
+        }
+        if(g[3]==0) {
+            mode=32;
             count=0;
             cros=0;
         }//ゆっくりモードに入れなかった時の保険
-
+        /*
+        if(g[0]==98) {
+            if(Twsh==2) {
+                solenoid.solenoid2 = SOLENOID_OFF;
+            }
+            mode=31;
+            count=0;
+        }
         switch(g[0]) {
             case 98:
                 motor[TIRE_FR].pwm = 30;
@@ -915,66 +931,66 @@
                 }
                 break;
             case 0:
-                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].pwm = 80;
                 motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].pwm = 80;
                 motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].pwm = 80;
                 motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].pwm = 80;
                 motor[TIRE_BL].dir = FOR;
                 mtc=true;
                 break;
             case 255:
-                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].pwm = 80;
                 motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 20;
+                motor[TIRE_FL].pwm = 40;
                 motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 20;
+                motor[TIRE_BR].pwm = 40;
                 motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].pwm = 80;
                 motor[TIRE_BL].dir = FOR;
                 mtc=true;
                 break;
             case 253:
-                motor[TIRE_FR].pwm = 20;
+                motor[TIRE_FR].pwm = 80;
                 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 = 20;
+                motor[TIRE_BL].pwm = 40;
                 motor[TIRE_BL].dir = FOR;
                 mtc=true;
                 break;
             case 254:
-                motor[TIRE_FR].pwm = 30;
+                motor[TIRE_FR].pwm = 50;
                 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 = 30;
+                motor[TIRE_BL].pwm = 50;
                 motor[TIRE_BL].dir = FOR;
                 mtc=true;
                 break;
             case 1:
-                motor[TIRE_FR].pwm = 20;
+                motor[TIRE_FR].pwm = 40;
                 motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].pwm = 80;
                 motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].pwm = 80;
                 motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 20;
+                motor[TIRE_BL].pwm = 40;
                 motor[TIRE_BL].dir = FOR;
                 mtc=true;
                 break;
             case 3:
                 motor[TIRE_FR].pwm = 0;
                 motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = 20;
+                motor[TIRE_FL].pwm = 40;
                 motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 20;
+                motor[TIRE_BR].pwm = 40;
                 motor[TIRE_BR].dir = BACK;
                 motor[TIRE_BL].pwm = 0;
                 motor[TIRE_BL].dir = FREE;
@@ -983,9 +999,9 @@
             case 2:
                 motor[TIRE_FR].pwm = 0;
                 motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = 30;
+                motor[TIRE_FL].pwm = 50;
                 motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 30;
+                motor[TIRE_BR].pwm = 50;
                 motor[TIRE_BR].dir = BACK;
                 motor[TIRE_BL].pwm = 0;
                 motor[TIRE_BL].dir = FREE;
@@ -1024,22 +1040,53 @@
                     mtc=false;
                     break;
             }
+
         }
-    } else if(mode==31) {
-        motor[TIRE_FR].pwm = 30;
+        } else if(mode==31) {
+        motor[TIRE_FR].pwm = 15;
         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 = 30;
+        motor[TIRE_FL].pwm = 10;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 10;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 15;
         motor[TIRE_BL].dir = FOR;
         if(g[3]==0) {
             count=0;
             mode=40;
         }
+        */
+
+    } else if(mode==31) {
+        motor[TIRE_FR].pwm = 15;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = 15;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 15;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 15;
+        motor[TIRE_BL].dir = FOR;
+        if(g[3]==0) {
+            count=0;
+            mode=32;
+        }
+    } else if(mode==32) {
+        motor[TIRE_FR].pwm = 30;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 30;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 30;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 30;
+        motor[TIRE_BL].dir = BACK;
+        if(g[3]==0) {
+            count=0;
+            mode=40;
+        }
     } else if(mode==40) {
-
+        if(LIB) {
+            mode=42;
+        }
         if(counts==false&&g[0]==0) {
             cros++;
             counts=true;
@@ -1048,7 +1095,7 @@
             counts=false;
         }
 
-        if(cros==2) {
+        if(cros==1) {
             // mode=41;
             //count=0;
             //cros=0;
@@ -1069,7 +1116,7 @@
                     count=0;
                 }
                 mtc2=true;
-                if(cross==3) {
+                if(cross==2) {
                     mode=41;
                     count=0;
                     cross=0;
@@ -1199,7 +1246,19 @@
             count=0;
             mode=70;
         }
-
+    } else if(mode==42) {
+        motor[TIRE_FR].pwm = 20;
+        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 = 20;
+        motor[TIRE_BL].dir = BACK;
+        if(g[1]==0) {
+            count=0;
+            mode=70;
+        }
     } else if(mode==70) {
         switch(g[1]) {
             case 99:
@@ -1218,13 +1277,13 @@
                 motor[TIRE_BL].dir = BACK;
                 break;
             case 0:
-                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].pwm = 80;
                 motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].pwm = 80;
                 motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].pwm = 80;
                 motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].pwm = 80;
                 motor[TIRE_BL].dir = BACK;
                 mtc=true;
                 break;
@@ -1353,6 +1412,7 @@
 #if USE_PROCESS_NUM>3
 static void Process3()
 {
+    
 }
 #endif
 
@@ -1551,6 +1611,10 @@
         }
     } else if(mode==20) {
 
+        if(Twsh==2) {
+            solenoid.solenoid2 = SOLENOID_ON;
+        }
+
         if(counts==false&&g[1]==0) {
             cros++;
             counts=true;
@@ -1560,9 +1624,9 @@
         }
 
         if(cros==2) {
-            mode=21;
-            count=0;
-            cros=0;
+            //mode=21;
+            //count=0;
+            //cros=0;
         }//ゆっくりモードに入れなかった時の保険
 
         switch(g[2]) {
@@ -1710,19 +1774,27 @@
             mode=30;
             cros=0;
             zyouge=true;
-            if(Twsh==2) {
-                solenoid.solenoid2 = SOLENOID_ON;
-            }
         }
     } else if(mode==30) {
-
+        motor[TIRE_FR].pwm = 60;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 60;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 60;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 60;
+        motor[TIRE_BL].dir = BACK;
+        if(g[1]==98) {
+            mode=31;
+            count=0;
+        }
         if(counts==false&&g[3]==0) {
             counts=false;
             mode=31;
             count=0;
             cros=0;
         }//ゆっくりモードに入れなかった時の保険
-
+        /*
         switch(g[1]) {
             case 98:
                 motor[TIRE_FR].pwm = 30;
@@ -1857,6 +1929,7 @@
                     break;
             }
         }
+        */
     } else if(mode==31) {
         motor[TIRE_FR].pwm = 0;
         motor[TIRE_FR].dir = FREE;
@@ -1872,6 +1945,10 @@
         }
     } else if(mode==40) {
 
+        if(LIB) {
+            mode=42;
+        }
+
         if(counts==false&&g[0]==0) {
             cros++;
             counts=true;