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 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;
}
}