nebel/2014-git-work
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

平行標的を区別できてなかったので

Browse source
This commit is contained in:
Nebel 2012-10-02 02:24:29 +09:00
parent 4f2d3a25c1
commit d85e34d742
1 changed files with 41 additions and 24 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

65
arduino/hanzo2_1/hanzo2_1.ino
View file

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