Diff: main.cpp

Revision:

4:951fabc31ba6

Parent:

3:f707552ec50a

diff -r f707552ec50a -r 951fabc31ba6 main.cpp
--- a/main.cpp	Sun Jun 03 07:04:34 2018 +0000
+++ b/main.cpp	Mon Jun 11 06:51:49 2018 +0000
@@ -282,6 +282,7 @@
     return(ret);
 }
 
+//ライントレース関数
 void line(void)
 {
     ModeLed = 0;
@@ -303,6 +304,7 @@
 #if DBG
         pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
 #endif
+
 #if 1
         switch(line)
         {
@@ -340,6 +342,84 @@
                 break;
         }
 #endif
+
+    }
+    ModeLed = 1;
+    left = 0.0;
+    right = 0.0;
+    wait(1);
+}
+
+//動作パターン関数（工事中）
+void wb_control(void)
+{
+    ModeLed = 0;
+    wait(1);
+    while(sw1 != 0)
+    {
+#if 0
+        int line = get_line(0) ? 0 : 1;
+            line |= get_line(1) ? 0 : 2;
+            line |= get_line(2) ? 0 : 4;
+            line |= get_line(3) ? 0 : 8;
+#else
+        int line = get_line(0) ? 1 : 0;
+            line |= get_line(1) ? 2 : 0;
+            line |= get_line(2) ? 4 : 0;
+            line |= get_line(3) ? 8 : 0;
+#endif
+
+#if DBG
+        pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
+#endif
+
+//改造中のソース
+
+#if 1
+        switch(line)
+        {
+            case 1:                 //直進モード上から下へ ○○○● とりあえず書いてみたパターン
+                if(ay<0){
+                    left = 1.0;
+                    right = 1.0;
+                    }
+                    else if(ay>0){
+                        left = 1.0;
+                        right = 1.0;
+                        }
+                
+                break;
+            case 3:                 //直進モード下から上へ ○○●●
+                left = 1.0;
+                right = -0.5;
+                break;
+            case 2:                 //回転モード上から下へ○○●○
+                left = 1.0;
+                right = 0.5;
+                break;
+            case 6:                 //回転モード下から上へ○●●○
+                left = 1.0;
+                right = 1.0;
+                break;
+            case 4:                 //後進モード上から下へ○●○○
+                left = 0.5;
+                right = 1.0;
+                break;
+            case 12:                //後進モード下から上へ●●○○
+                left = -0.5;
+                right = 1.0;
+                break;
+            case 8:                 // ●○○○
+                left = -1.0;
+                right = 1.0;
+                break;
+            default:
+                left = 1.0;
+                right = 1.0;
+                break;
+        }
+#endif
+
     }
     ModeLed = 1;
     left = 0.0;
@@ -437,7 +517,8 @@
         {
             bValue = 1;
             line_mode = 1;
-            line();       
+            //line();
+            wb_control(); //動作モード関数
             line_mode = 0;
             bValue = 0;
         }