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-07 19:27:57 +09:00
parent 5c2bf67c5e
commit 08ab4cd847
1 changed files with 73 additions and 25 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

98
arduino/hanzo2_1/hanzo2_1.ino
View file

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