From 440326dbe4790ad4acc9c81bb00677e8540c5bab Mon Sep 17 00:00:00 2001 From: kou029w Date: Tue, 9 Oct 2012 20:03:53 +0900 Subject: [PATCH] =?UTF-8?q?=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88=E3=82=92?= =?UTF-8?q?=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- arduino/hanzo2_1/hanzo2_1.ino | 175 ++++++++++++++++++---------------- 1 file changed, 94 insertions(+), 81 deletions(-) diff --git a/arduino/hanzo2_1/hanzo2_1.ino b/arduino/hanzo2_1/hanzo2_1.ino index 6fd2f13..a2c7ab5 100644 --- a/arduino/hanzo2_1/hanzo2_1.ino +++ b/arduino/hanzo2_1/hanzo2_1.ino @@ -16,15 +16,13 @@ #include #include "Motor.h" -//#define LINE_BLACK //白地・黒ライン有効 - -const unsigned long LAP_COUNT = 848; -const unsigned long TARGET_NONE_COUNT = LAP_COUNT*90/848; +const unsigned long LAP_COUNT = 848; //カウント/周 +const unsigned long TARGET_NONE_COUNT = LAP_COUNT*90/848; //カーブを出るカウント 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 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; @@ -68,8 +66,8 @@ enum { MODE_TARGET_CYLINDER //円筒標的 }; -const byte MASK_MODE_TRACE = 0b01111110; -const byte MASK_CHECK_MARKER = 0b10000001; +const byte MASK_MODE_TRACE = 0b01111110; +const byte MASK_CHECK_MARKER = 0b10000001; /**************************************/ @@ -84,55 +82,7 @@ unsigned long modeChangeCounter; /**************************************/ -void traceLeft(byte sensor){ - switch(sensor&MASK_MODE_TRACE){ - case 0b01000000: - handle(-5*HANDLE_KP); - motorMode(GO, GO, 0, SPEED_DEFAULT); - break; - case 0b01100000: - handle(-4*HANDLE_KP); - motorMode(GO, GO, 0, SPEED_DEFAULT); - break; - case 0b00100000: - handle(-3*HANDLE_KP); - motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); - break; - case 0b00110000: - handle(-2*HANDLE_KP); - motorMode(GO, GO, SPEED_DEFAULT, 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: - 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: @@ -182,23 +132,83 @@ void trace(byte sensor){ } } +/* MODE_LEFT時の再加速用 */ +void traceLeft(byte sensor){ + switch(sensor&MASK_MODE_TRACE){ + case 0b01000000: + handle(-5*HANDLE_KP); + motorMode(GO, GO, 0, SPEED_DEFAULT); + break; + case 0b01100000: + handle(-4*HANDLE_KP); + motorMode(GO, GO, 0, SPEED_DEFAULT); + break; + case 0b00100000: + handle(-3*HANDLE_KP); + motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); + break; + case 0b00110000: + handle(-2*HANDLE_KP); + motorMode(GO, GO, SPEED_DEFAULT, 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: + 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 motorMode(byte leftMode, byte rightMode, byte leftSpeed, byte rightSpeed){ motorL.mode( leftMode, leftSpeed); motorR.mode(rightMode, rightSpeed); } +/* ステアリングを中央からdegree[度]だけ動かす + <- - 0 + -> + | | + | | + []---{}---[] + / .\ +*/ void handle(int degree){ servoHandle.write(HANDLE_DEGREE_DEFAULT + degree); } +/* ランスを中央からdegree[度]だけ動かす + <- - 0 + -> + / .\ + / | \ + []+--{}--+[] +*/ void lance(int degree){ servoLance.write(LANCE_DEGREE_DEFAULT - degree); } -int lanceRead(){ - return LANCE_DEGREE_DEFAULT - servoLance.read(); -} - /* ロータリーエンコーダーの変化を見る todo:外部割り込み使ってみたい、、、 */ @@ -211,6 +221,7 @@ void count(){ } } +/* マーカーを読んで、モードを返す */ int checkMarker(byte sensor){ int mode = MODE_TRACE; switch(sensor & MASK_CHECK_MARKER){ @@ -220,11 +231,11 @@ int checkMarker(byte sensor){ break; case 0b00000001: modeChangeCounter = counter; - if(counter%LAP_COUNT > LAP_COUNT/2){ + if(counter%LAP_COUNT > LAP_COUNT/2){ //後半は垂直標的をねらいたい mode = MODE_TARGET_RIGHT_VERTICAL; - }else if(counter%LAP_COUNT < LAP_COUNT/7){ + }else if(counter%LAP_COUNT < LAP_COUNT/7){ //まずは、E的をねらいたい mode = MODE_TARGET_PARALLEL1; - }else if(counter%LAP_COUNT < LAP_COUNT/3){ + }else if(counter%LAP_COUNT < LAP_COUNT/3){ //次に、F的をねらいたい mode = MODE_TARGET_PARALLEL2; }else{ mode = MODE_TARGET_RIGHT_VERTICAL; @@ -238,6 +249,7 @@ int checkMarker(byte sensor){ return mode; } +/* 前方センサーの初期化 */ void sensorInit(){ pinMode(PIN_SENSOR_0, INPUT); pinMode(PIN_SENSOR_1, INPUT); @@ -249,6 +261,7 @@ void sensorInit(){ pinMode(PIN_SENSOR_7, INPUT); } +/* センサーの状態を返す */ byte sensorRead(){ byte sensor = 0; //黒が1、白が0 if(digitalRead(PIN_SENSOR_0)) sensor += 0b00000001; @@ -308,7 +321,7 @@ void loop(){ case MODE_TARGET_PARALLEL1: //平行標的1 trace(sensor); lance(LANCE_ANGLE_PARALLEL1); - if(counter - modeChangeCounter > 24){ + if(counter - modeChangeCounter > 24){ //マーカーから少し進んで、叩く lance(LANCE_ANGLE_PARALLEL1+8); mode = MODE_TRACE; } @@ -316,7 +329,7 @@ void loop(){ case MODE_TARGET_PARALLEL2: //平行標的2 trace(sensor); lance(LANCE_ANGLE_PARALLEL2); - if(counter - modeChangeCounter > 26){ + if(counter - modeChangeCounter > 26){ //マーカーから少し進んで、叩く lance(LANCE_ANGLE_PARALLEL2+8); mode = MODE_TRACE; } @@ -324,14 +337,14 @@ void loop(){ case MODE_TRACE: trace(sensor); mode = checkMarker(sensor); - if(counter > LAP_COUNT*10){ + if(counter > LAP_COUNT*10){ //10周したら停止 mode = MODE_STOP; } break; case MODE_LEFT: //左カーブ - if(counter - modeChangeCounter < TARGET_NONE_COUNT/2) trace(sensor); - else traceLeft(sensor); // trace(sensor); + if(counter - modeChangeCounter < TARGET_NONE_COUNT/2) trace(sensor); + else traceLeft(sensor); //カーブの後半は加速したい if(counter < LAP_COUNT) lance(30); //1周目は円筒標的をねらいたい else lance(-30); if(counter - modeChangeCounter > TARGET_NONE_COUNT){ @@ -339,17 +352,17 @@ void loop(){ mode = MODE_TRACE; } break; - case MODE_TARGET_CYLINDER: //円筒標的 - digitalWrite(PIN_LED, HIGH); - handle(0); - lance(-90); - motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); - if(sensor&MASK_CHECK_MARKER == 0b10000001){ - modeChangeCounter = counter; - }else if( counter - modeChangeCounter > LAP_COUNT/21 ){ - mode = MODE_STOP; - } - break; +// case MODE_TARGET_CYLINDER: //円筒標的 +// digitalWrite(PIN_LED, HIGH); +// handle(0); +// lance(-90); +// motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); +// if(sensor&MASK_CHECK_MARKER == 0b10000001){ +// modeChangeCounter = counter; +// }else if( counter - modeChangeCounter > LAP_COUNT/21 ){ +// mode = MODE_STOP; +// } +// break; case MODE_STOP: //停止 digitalWrite(PIN_LED, HIGH); motorL.mode(STOP);