aa

Dependencies:   mbed TrapezoidControl QEI

Revision:
26:be5121da0f77
Parent:
25:38d4964e8aad
Child:
27:545148741493
diff -r 38d4964e8aad -r be5121da0f77 System/Process/Process.cpp
--- a/System/Process/Process.cpp	Tue Sep 17 03:46:34 2019 +0000
+++ b/System/Process/Process.cpp	Tue Sep 17 12:02:53 2019 +0000
@@ -276,7 +276,7 @@
         if(ZoneSW) {
             current = 1;
             Twsh=2;
-        } else {
+        }else{
             current = 1;
             Twsh=3;
         }
@@ -296,7 +296,8 @@
 bool dz4=true;
 bool dz4i=false;
 
-int mode=10;
+//int mode=10;
+int mode =10;
 int mode1=0;
 int mode1g0=100000;
 
@@ -415,22 +416,25 @@
 
 static void Process2()
 {
+//追加
+//Twsh = 2;
 //    printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
     mode1g0++;
+    
     if(mode==1) {
-        motor[TIRE_FR].pwm = 15;
+        motor[TIRE_FR].pwm = 30;
         motor[TIRE_FR].dir = BACK;
-        motor[TIRE_FL].pwm = 15;
+        motor[TIRE_FL].pwm = 30;
         motor[TIRE_FL].dir = FOR;
-        motor[TIRE_BR].pwm = 15;
+        motor[TIRE_BR].pwm = 30;
         motor[TIRE_BR].dir = BACK;
-        motor[TIRE_BL].pwm = 15;
+        motor[TIRE_BL].pwm = 30;
         motor[TIRE_BL].dir = FOR;
-        if(mode1g0>5000&&g[2]==0) {
+        if(mode1g0>100000&&g[2]==0) {//変更5000→100000
             mode1g0=0;
             mode=2;
         }
-    } else if(mode==2) {
+    }else if(mode==2) {
         switch(g[2]) {
             case 98:
                 motor[TIRE_FR].pwm = 30;
@@ -1691,7 +1695,36 @@
         motor[TIRE_BR].dir = FOR;
         motor[TIRE_BL].pwm = 15;
         motor[TIRE_BL].dir = BACK;
-    } else {
+        mode1g0++;
+        if(mode1g0>5000&&g[3]==0) {
+            mode1g0=0;
+            mode=72;
+        }
+    }else if(mode==72) {
+        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;
+        mode1g0++;
+        if(mode1g0>100000&&g[3]==0) {
+            mode1g0=0;
+            mode1=0;
+            mode=80;
+        }
+    }else if(mode==80) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = BRAKE;
+        motor[TIRE_FL].pwm = 0;
+        motor[TIRE_FL].dir = BRAKE;
+        motor[TIRE_BR].pwm = 0;
+        motor[TIRE_BR].dir = BRAKE;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = BRAKE;
+    }else {
         motor[TIRE_FR].pwm = 0;
         motor[TIRE_FR].dir = BRAKE;
         motor[TIRE_FL].pwm = 0;