From 08ab4cd84732767d832cea98a911e596ddd30fdc Mon Sep 17 00:00:00 2001
From: kou029w <kou029w@gmail.com>
Date: Sun, 7 Oct 2012 19:27:57 +0900
Subject: [PATCH] =?UTF-8?q?=E9=A2=A8=E7=B4=8B=E7=A5=AD=E3=83=AD=E3=83=9C?=
 =?UTF-8?q?=E3=83=83=E3=83=88=E3=82=B3=E3=83=B3=E3=83=86=E3=82=B9=E3=83=88?=
 =?UTF-8?q?=E7=94=A8?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 arduino/hanzo2_1/hanzo2_1.ino | 98 ++++++++++++++++++++++++++---------
 1 file changed, 73 insertions(+), 25 deletions(-)

diff --git a/arduino/hanzo2_1/hanzo2_1.ino b/arduino/hanzo2_1/hanzo2_1.ino
index 2338561..6fd2f13 100644
--- a/arduino/hanzo2_1/hanzo2_1.ino
+++ b/arduino/hanzo2_1/hanzo2_1.ino
@@ -18,13 +18,13 @@
 
 //#define LINE_BLACK //白地・黒ライン有効
 
-const unsigned long LAP_COUNT = 854;
-const unsigned long TARGET_NONE_COUNT = LAP_COUNT/10;
+const unsigned long LAP_COUNT = 848;
+const unsigned long TARGET_NONE_COUNT = LAP_COUNT*90/848;
 
-const int LANCE_ANGLE_PARALLEL1       =  90 -  43;  //度
-const int LANCE_ANGLE_PARALLEL2       =  90 -  34;
-const int LANCE_ANGLE_LEFT_VERTICAL   =  90 - 123;
-const int LANCE_ANGLE_RIGHT_VERTICAL  =  90 -  59;
+const int LANCE_ANGLE_PARALLEL1       =  90 -  53;  //度
+const int LANCE_ANGLE_PARALLEL2       =  90 -  43;
+const int LANCE_ANGLE_LEFT_VERTICAL   =  90 - 120;
+const int LANCE_ANGLE_RIGHT_VERTICAL  =  90 -  57;
 
 const byte SPEED_DEFAULT = 255;
 
@@ -84,15 +84,15 @@ unsigned long modeChangeCounter;
 
 /**************************************/
 
-void trace(byte sensor){
+void traceLeft(byte sensor){
   switch(sensor&MASK_MODE_TRACE){
   case 0b01000000:
     handle(-5*HANDLE_KP);
-    motorMode(GO, GO, SPEED_DEFAULT/4, SPEED_DEFAULT/2);
+    motorMode(GO, GO, 0, SPEED_DEFAULT);
     break;
   case 0b01100000:
     handle(-4*HANDLE_KP);
-    motorMode(GO, GO, SPEED_DEFAULT*0.8, SPEED_DEFAULT);
+    motorMode(GO, GO, 0, SPEED_DEFAULT);
     break;
   case 0b00100000:
     handle(-3*HANDLE_KP);
@@ -103,7 +103,7 @@ void trace(byte sensor){
     motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
     break;
   case 0b00010000:
-    handle(-1*HANDLE_KP);
+    handle(-2);
     motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
     break;
   case 0b00011000:
@@ -111,7 +111,56 @@ void trace(byte sensor){
     motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
     break;
   case 0b00001000:
-    handle(1*HANDLE_KP);
+    handle(2);
+    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
+    break;
+  case 0b00001100:
+    handle(2*HANDLE_KP);
+    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
+    break;
+  case 0b00000100:
+    handle(3*HANDLE_KP);
+    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
+    break;
+  case 0b00000110:
+    handle(4*HANDLE_KP);
+    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
+    break;
+  case 0b00000010:
+    handle(5*HANDLE_KP);
+    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
+    break;
+  }
+}
+
+void trace(byte sensor){
+  switch(sensor&MASK_MODE_TRACE){
+  case 0b01000000:
+    handle(-5*HANDLE_KP);
+    motorMode(GO, GO, 0, SPEED_DEFAULT*0.1);
+    break;
+  case 0b01100000:
+    handle(-4*HANDLE_KP);
+    motorMode(GO, GO, 0, SPEED_DEFAULT*0.1);
+    break;
+  case 0b00100000:
+    handle(-3*HANDLE_KP);
+    motorMode(GO, GO, SPEED_DEFAULT*0.333, SPEED_DEFAULT);
+    break;
+  case 0b00110000:
+    handle(-2*HANDLE_KP);
+    motorMode(GO, GO, SPEED_DEFAULT*0.667, SPEED_DEFAULT);
+    break;
+  case 0b00010000:
+    handle(-2);
+    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
+    break;
+  case 0b00011000:
+    handle(0);
+    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
+    break;
+  case 0b00001000:
+    handle(2);
     motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
     break;
   case 0b00001100:
@@ -173,9 +222,9 @@ int checkMarker(byte sensor){
     modeChangeCounter = counter;
     if(counter%LAP_COUNT > LAP_COUNT/2){
       mode = MODE_TARGET_RIGHT_VERTICAL;
-    }else if(counter%LAP_COUNT < LAP_COUNT/6){
+    }else if(counter%LAP_COUNT < LAP_COUNT/7){
       mode = MODE_TARGET_PARALLEL1;
-    }else if(counter%LAP_COUNT < LAP_COUNT/2){
+    }else if(counter%LAP_COUNT < LAP_COUNT/3){
       mode = MODE_TARGET_PARALLEL2;
     }else{
       mode = MODE_TARGET_RIGHT_VERTICAL;
@@ -258,34 +307,33 @@ void loop(){
     break;
   case MODE_TARGET_PARALLEL1: //平行標的1
     trace(sensor);
-    lance(LANCE_ANGLE_PARALLEL1-10);
-    if(counter - modeChangeCounter > 25){
-      lance(LANCE_ANGLE_PARALLEL1);
+    lance(LANCE_ANGLE_PARALLEL1);
+    if(counter - modeChangeCounter > 24){
+      lance(LANCE_ANGLE_PARALLEL1+8);
       mode = MODE_TRACE;
     }
     break;
   case MODE_TARGET_PARALLEL2: //平行標的2
     trace(sensor);
-    lance(LANCE_ANGLE_PARALLEL2-10);
+    lance(LANCE_ANGLE_PARALLEL2);
     if(counter - modeChangeCounter > 26){
-      lance(LANCE_ANGLE_PARALLEL2);
+      lance(LANCE_ANGLE_PARALLEL2+8);
       mode = MODE_TRACE;
     }
     break;
   case MODE_TRACE:
     trace(sensor);
     mode = checkMarker(sensor);
-//    6周する前に、円筒標的を狙いに行く
-//    if( counter > LAP_COUNT*6 - LAP_COUNT/14 ){
-//      mode = MODE_TARGET_CYLINDER;
-//    }
-    if(counter > LAP_COUNT*1){
+    if(counter > LAP_COUNT*10){
       mode = MODE_STOP;
     }
     break;
   case MODE_LEFT: //左カーブ
-    trace(sensor);
-    lance(90);
+    if(counter - modeChangeCounter < TARGET_NONE_COUNT/2) trace(sensor);
+    else traceLeft(sensor);
+//    trace(sensor);
+    if(counter < LAP_COUNT) lance(30);  //1周目は円筒標的をねらいたい
+    else lance(-30);
     if(counter - modeChangeCounter > TARGET_NONE_COUNT){
       lance(0);
       mode = MODE_TRACE;