Diff: linetracer.cpp

Revision:

0:e9af471e2a2b

diff -r 000000000000 -r e9af471e2a2b linetracer.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/linetracer.cpp	Thu Nov 16 06:15:58 2017 +0000
@@ -0,0 +1,76 @@
+#include "linetracer.h"
+#define K 0.000001 //ハンドルのききの良さを決める定数
+DigitalOut test1(PTB8);
+DigitalOut test2(PTB9);
+DigitalOut test3(PTB10);
+DigitalOut test4(PTB11);
+
+void linetracerInit(){
+    servoInit();
+    sensorInit();
+    motorInit();
+    start();
+}
+
+void linetracerLoop(){
+    float pulse;
+    pulse = 0.00155;
+    int reaction = 0;
+    int flag = 0;
+    while(1){
+        reaction = getReaction();
+        /*
+        if((reaction & 0b000011) == 0b000000){
+                flag++;
+                if(flag >= 100){
+                    wait(0.3);
+                    stop();
+                    while(1){
+                        test1 = 1;
+                    }
+                }
+        }
+        else{
+            flag = 0;
+            test1 = 0;
+        }
+        */
+        
+        
+        if((reaction & 0b100000) == 0b100000){
+            rotate(handle,pulse - K,&pulse);
+            test1 = 1;
+        }
+        
+        
+        if((reaction & 0b000100) == 0b000100){
+            rotate(handle,pulse + K,&pulse);
+            test2 = 1;
+        }
+        
+        if((reaction & 0b010000) == 0b010000){
+            rotate(handle,pulse - K / 3.0,&pulse);
+            test3 = 1;
+        }
+        
+        if((reaction & 0b001000) == 0b001000){
+            rotate(handle,pulse + K / 3.0,&pulse);
+            test4 = 1;
+        }
+        
+        test1 = 0;
+        test2 = 0;
+        test3 = 0;
+        test4 = 0;
+        
+        /*
+        if((reaction & 0b010100) == 0b000000){
+            rotate(handle,pulse + K * 0.5,&pulse);
+        }
+        
+        if((reaction & 0b1010000) == 0b000000){
+            rotate(handle,pulse - K * 0.5,&pulse);
+        }
+        */
+    }
+}