From 22207c506489fe7f77ca260138462822062753e2 Mon Sep 17 00:00:00 2001
From: kou029w <kou029w@gmail.com>
Date: Fri, 14 Sep 2012 22:47:27 +0900
Subject: [PATCH] =?UTF-8?q?=E7=A7=BB=E5=8B=95?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 arduino/hanzo2_1/hanzo2_1.ino | 71 +++++++++++++++++------------------
 1 file changed, 34 insertions(+), 37 deletions(-)

diff --git a/arduino/hanzo2_1/hanzo2_1.ino b/arduino/hanzo2_1/hanzo2_1.ino
index 835b597..5679e49 100644
--- a/arduino/hanzo2_1/hanzo2_1.ino
+++ b/arduino/hanzo2_1/hanzo2_1.ino
@@ -138,6 +138,27 @@ void trace(byte sensor){
   }
 }
 
+int checkMarker(byte sensor){
+  int mode = MODE_TRACE;
+  switch(sensor & MASK_CHECK_MARKER){
+  case 0b10000001:
+    counterOld = counter;
+    mode = MODE_LEFT;
+    break;
+  case 0b00000001:
+    if(counter%LAP_COUNT > LAP_COUNT/2){
+      mode = MODE_TARGET_RIGHT_VERTICAL;
+    }else{
+      mode = MODE_TARGET_PARALLEL1;
+    }
+    break;
+  case 0b10000000:
+    mode = MODE_TARGET_LEFT_VERTICAL;
+    break;
+  }
+  return mode;
+}
+
 void motorMode(byte motorLeftMode, byte motorRightMode, byte motorLeftSpeed, byte motorRightSpeed){
   motorL.mode( motorLeftMode,  motorLeftSpeed);
   motorR.mode(motorRightMode, motorRightSpeed);
@@ -164,30 +185,6 @@ void count(){
   }
 }
 
-int checkMarker(byte sensor){
-  int mode;
-  switch(sensor & MASK_CHECK_MARKER){
-  case 0b10000001:
-    counterOld = counter;
-    mode = MODE_LEFT;
-    break;
-  case 0b00000001:
-    if(counter%LAP_COUNT > LAP_COUNT/2){
-      mode = MODE_TARGET_RIGHT_VERTICAL;
-    }else{
-      mode = MODE_TARGET_PARALLEL1;
-    }
-    break;
-  case 0b10000000:
-    mode = MODE_TARGET_LEFT_VERTICAL;
-    break;
-  default:
-    mode = MODE_TRACE;
-    break;
-  }
-  return mode;
-}
-
 void sensorInit(){
   pinMode(PIN_SENSOR_0, INPUT);
   pinMode(PIN_SENSOR_1, INPUT);
@@ -250,19 +247,6 @@ void loop(){
 #endif
   //ロータリーエンコーダーの変化を見る
   count();
-  //平行標的を狙う時は、ランスをぷるぷるする
-  static unsigned long millis_lance = 0;
-  if(millis() - millis_lance > LANCE_INTERVAL){
-    millis_lance = millis();
-    switch(servoLance.read()){
-    case LANCE_ANGLE1:
-      lance(LANCE_ANGLE2);
-      break;
-    case LANCE_ANGLE2:
-      lance(LANCE_ANGLE1);
-      break;
-    }
-  }
   //全体の動作を決定する
   switch(mode){
   case MODE_TARGET_RIGHT_VERTICAL:   //右垂直標的
@@ -310,4 +294,17 @@ void loop(){
     motorR.mode(STOP);
     break;
   }
+  //平行標的を狙う時は、ランスをぷるぷるする
+  static unsigned long millis_lance = 0;
+  if(millis() - millis_lance > LANCE_INTERVAL){
+    millis_lance = millis();
+    switch(servoLance.read()){
+    case LANCE_ANGLE1:
+      lance(LANCE_ANGLE2);
+      break;
+    case LANCE_ANGLE2:
+      lance(LANCE_ANGLE1);
+      break;
+    }
+  }
 }