From d85e34d74253e444498fd891044cd8e97881ae70 Mon Sep 17 00:00:00 2001
From: kou029w <kou029w@gmail.com>
Date: Tue, 2 Oct 2012 02:24:29 +0900
Subject: [PATCH] =?UTF-8?q?=E5=B9=B3=E8=A1=8C=E6=A8=99=E7=9A=84=E3=82=92?=
 =?UTF-8?q?=E5=8C=BA=E5=88=A5=E3=81=A7=E3=81=8D=E3=81=A6=E3=81=AA=E3=81=8B?=
 =?UTF-8?q?=E3=81=A3=E3=81=9F=E3=81=AE=E3=81=A7?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 arduino/hanzo2_1/hanzo2_1.ino | 65 ++++++++++++++++++++++-------------
 1 file changed, 41 insertions(+), 24 deletions(-)

diff --git a/arduino/hanzo2_1/hanzo2_1.ino b/arduino/hanzo2_1/hanzo2_1.ino
index f8b571b..3e4268d 100644
--- a/arduino/hanzo2_1/hanzo2_1.ino
+++ b/arduino/hanzo2_1/hanzo2_1.ino
@@ -21,12 +21,13 @@
 const unsigned long LAP_COUNT = 854;
 const unsigned long TARGET_NONE_COUNT = LAP_COUNT/10;
 
-const int LANCE_ANGLE0 =   0; //度
-const int LANCE_ANGLE1 =  43;
-const int LANCE_ANGLE2 =  54;
-const int LANCE_ANGLE3 = -33;
-const int LANCE_ANGLE4 =  34;
-const unsigned int LANCE_INTERVAL = 70; //ms
+const int LANCE_ANGLE_PARALLEL1       =  43;  //度
+const int LANCE_ANGLE_PARALLEL2       =  42;
+const int LANCE_ANGLE_LEFT_VERTICAL   = -33;
+const int LANCE_ANGLE_RIGHT_VERTICAL  =  34;
+
+const int LANCE_INTERVAL_DEGREE = 10;  //ぷるぷるメソッドのための間隔(度)
+const unsigned long LANCE_INTERVAL = 6*LANCE_INTERVAL_DEGREE; //ms
 
 const byte SPEED_DEFAULT = 255;
 
@@ -72,8 +73,6 @@ enum {
 
 const byte MASK_MODE_TRACE         = 0b01111110;
 const byte MASK_CHECK_MARKER       = 0b10000001;
-const byte MASK_CHECK_MARKER_RIGHT = 0b00000001;
-const byte MASK_CHECK_MARKER_LEFT  = 0b10000000;
 
 /**************************************/
 
@@ -84,7 +83,7 @@ Motor motorL;
 
 byte mode;
 volatile unsigned long counter = 0;
-unsigned long counterOld;
+unsigned long modeChangeCounter;
 
 /**************************************/
 
@@ -150,6 +149,10 @@ void lance(int degree){
   servoLance.write(LANCE_DEGREE_DEFAULT - degree);
 }
 
+int lanceRead(){
+  return LANCE_DEGREE_DEFAULT - servoLance.read();
+}
+
 /* ロータリーエンコーダーの変化を見る
   todo:外部割り込み使ってみたい、、、
 */
@@ -166,17 +169,23 @@ int checkMarker(byte sensor){
   int mode = MODE_TRACE;
   switch(sensor & MASK_CHECK_MARKER){
   case 0b10000001:
-    counterOld = counter;
+    modeChangeCounter = counter;
     mode = MODE_LEFT;
     break;
   case 0b00000001:
+    modeChangeCounter = counter;
     if(counter%LAP_COUNT > LAP_COUNT/2){
       mode = MODE_TARGET_RIGHT_VERTICAL;
-    }else{
+    }else if(counter%LAP_COUNT < LAP_COUNT/6){
       mode = MODE_TARGET_PARALLEL1;
+    }else if(counter%LAP_COUNT < LAP_COUNT/2){
+      mode = MODE_TARGET_PARALLEL2;
+    }else{
+      mode = MODE_TARGET_RIGHT_VERTICAL;
     }
     break;
   case 0b10000000:
+    modeChangeCounter = counter;
     mode = MODE_TARGET_LEFT_VERTICAL;
     break;
   }
@@ -243,19 +252,19 @@ void loop(){
   /* 全体の動作を決定する */
   switch(mode){
   case MODE_TARGET_RIGHT_VERTICAL:  //右垂直標的
-    lance(LANCE_ANGLE4);
+    lance(LANCE_ANGLE_RIGHT_VERTICAL);
     mode = MODE_TRACE;
     break;
   case MODE_TARGET_LEFT_VERTICAL: //左垂直標的
-    lance(LANCE_ANGLE3);
+    lance(LANCE_ANGLE_LEFT_VERTICAL);
     mode = MODE_TRACE;
     break;
   case MODE_TARGET_PARALLEL1: //平行標的1
-    lance(LANCE_ANGLE1);
+    lance(LANCE_ANGLE_PARALLEL1);
     mode = MODE_TRACE;
     break;
   case MODE_TARGET_PARALLEL2: //平行標的2
-    lance(LANCE_ANGLE2);
+    lance(LANCE_ANGLE_PARALLEL2);
     mode = MODE_TRACE;
     break;
   case MODE_TRACE:
@@ -271,8 +280,8 @@ void loop(){
     break;
   case MODE_LEFT: //左カーブ
     trace(sensor);
-    lance(LANCE_ANGLE0);
-    if(counter - counterOld > TARGET_NONE_COUNT) mode = MODE_TRACE;
+    lance(0);
+    if(counter - modeChangeCounter > TARGET_NONE_COUNT) mode = MODE_TRACE;
     break;
   case MODE_TARGET_CYLINDER:  //円筒標的
     digitalWrite(PIN_LED, HIGH);
@@ -280,26 +289,34 @@ void loop(){
     lance(-90);
     motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
     if(sensor&MASK_CHECK_MARKER == 0b10000001){
-      counterOld = counter;
-    }else if( counter - counterOld > LAP_COUNT/21 ){
+      modeChangeCounter = counter;
+    }else if( counter - modeChangeCounter > LAP_COUNT/21 ){
       mode = MODE_STOP;
     }
     break;
   case MODE_STOP: //停止
+    digitalWrite(PIN_LED, HIGH);
     motorL.mode(STOP);
     motorR.mode(STOP);
     break;
   }
+  
   /* 平行標的を狙う時は、ランスをぷるぷるする */
   static unsigned long millis_lance = 0;
   if(millis() - millis_lance > LANCE_INTERVAL){
     millis_lance = millis();
-    switch(servoLance.read()){
-    case LANCE_DEGREE_DEFAULT - LANCE_ANGLE1:
-      lance(LANCE_ANGLE2);
+    switch(lanceRead()){
+    case LANCE_ANGLE_PARALLEL1:
+      lance(LANCE_ANGLE_PARALLEL1 - LANCE_INTERVAL_DEGREE);
       break;
-    case LANCE_DEGREE_DEFAULT - LANCE_ANGLE2:
-      lance(LANCE_ANGLE1);
+    case LANCE_ANGLE_PARALLEL1 - LANCE_INTERVAL_DEGREE:
+      lance(LANCE_ANGLE_PARALLEL1);
+      break;
+    case LANCE_ANGLE_PARALLEL2:
+      lance(LANCE_ANGLE_PARALLEL2 - LANCE_INTERVAL_DEGREE);
+      break;
+    case LANCE_ANGLE_PARALLEL2 - LANCE_INTERVAL_DEGREE:
+      lance(LANCE_ANGLE_PARALLEL2);
       break;
     }
   }