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 //白地・黒ライン有効
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;