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

update

Browse source
This commit is contained in:
Nebel 2012-09-14 16:54:51 +09:00
parent 817a2ecad3
commit ee9bc099ec
1 changed files with 102 additions and 104 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

206
arduino/hanzo2_1/hanzo2_1.ino
View file

@ -17,10 +17,9 @@
#include "Motor.h" #include "Motor.h"
//#define LINE_BLACK //白地・黒ライン有効 //#define LINE_BLACK //白地・黒ライン有効
//#define PIN_SENSOR ( ((PINC<<2) & ~0x03) | ((PINB>>4) & 0x03) )
const unsigned int TARGET_NONE_COUNT = 85;
const unsigned long LAP_COUNT = 854; const unsigned long LAP_COUNT = 854;
const unsigned long TARGET_NONE_COUNT = LAP_COUNT/10;
const int LANCE_ANGLE0 = 0; //度 const int LANCE_ANGLE0 = 0; //度
const int LANCE_ANGLE1 = 43; const int LANCE_ANGLE1 = 43;
@ -59,15 +58,16 @@ const byte PIN_MOTOR_LEFT_2 = 7;
const byte PIN_MOTOR_RIGHT_1 = 6; const byte PIN_MOTOR_RIGHT_1 = 6;
const byte PIN_MOTOR_RIGHT_2 = 8; const byte PIN_MOTOR_RIGHT_2 = 8;
const byte MODE_STOP = 0x10; enum {
const byte MODE_TRACE = 0x11; MODE_STOP, //停止
const byte MODE_LEFT = 0x12; //左カーブ MODE_TRACE, //標準
const byte MODE_RIGHT = 0x13; //右カーブ MODE_LEFT, //左カーブ
MODE_TARGET_PARALLEL1, //平行標的1
const byte TARGET_NONE = 0x40; MODE_TARGET_PARALLEL2, //平行標的2
const byte TARGET_PARALLEL = 0x41; //平行標的 MODE_TARGET_RIGHT_VERTICAL, //右垂直標的
const byte TARGET_VERTICAL = 0x42; //垂直標的 MODE_TARGET_LEFT_VERTICAL, //左垂直標的
const byte TARGET_CYLINDER = 0x43; //円筒標的 MODE_TARGET_CYLINDER //円筒標的
};
const byte MASK_MODE_TRACE = 0b01111110; const byte MASK_MODE_TRACE = 0b01111110;
const byte MASK_CHECK_MARKER = 0b10000001; const byte MASK_CHECK_MARKER = 0b10000001;
@ -82,60 +82,58 @@ Motor motorR;
Motor motorL; Motor motorL;
byte mode; byte mode;
byte target; unsigned long counterOld;
unsigned int counterOld;
char lanceAngle;
/**************************************/ /**************************************/
void trace(byte sensor){ void trace(byte sensor){
switch( sensor & MASK_MODE_TRACE ){ switch(sensor&MASK_MODE_TRACE){
case 0b01000000: case 0b01000000:
handle(-20); handle(-20);
motorMode(GO, GO, 0x40, 0xff); motorMode(GO, GO, SPEED_DEFAULT/4, SPEED_DEFAULT);
lanceAngle = 0; // lance(0);
break; break;
case 0b01100000: case 0b01100000:
handle(-16); handle(-16);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
lanceAngle = 0; // lance(0);
break; break;
case 0b00100000: case 0b00100000:
handle(-12); handle(-12);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
lanceAngle = 0; // lance(0);
break; break;
case 0b00110000: case 0b00110000:
handle(-5); handle(-5);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break; break;
case 0b00010000: case 0b00010000:
handle(-2); handle(-2);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break; break;
case 0b00011000: case 0b00011000:
handle(0); handle(0);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break; break;
case 0b00001000: case 0b00001000:
handle(2); handle(2);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break; break;
case 0b00001100: case 0b00001100:
handle(5); handle(5);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break; break;
case 0b00000100: case 0b00000100:
handle(7); handle(7);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break; break;
case 0b00000110: case 0b00000110:
handle(10); handle(10);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break; break;
case 0b00000010: case 0b00000010:
handle(12); handle(12);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break; break;
} }
} }
@ -166,6 +164,30 @@ void count(){
} }
} }
int checkMarker(byte sensor){
int mode;
switch(sensor & MASK_CHECK_MARKER){
case 0b10000001:
counterOld = counter;
mode = MODE_LEFT;
break;
case 0b00000001:
if(counter%LAP_COUNT > LAP_COUNT/2){
mode = MODE_TARGET_RIGHT_VERTICAL;
}else{
mode = MODE_TARGET_PARALLEL1;
}
break;
case 0b10000000:
mode = MODE_TARGET_LEFT_VERTICAL;
break;
default:
mode = MODE_TRACE;
break;
}
return mode;
}
void sensorInit(){ void sensorInit(){
pinMode(PIN_SENSOR_0, INPUT); pinMode(PIN_SENSOR_0, INPUT);
pinMode(PIN_SENSOR_1, INPUT); pinMode(PIN_SENSOR_1, INPUT);
@ -199,7 +221,8 @@ void setup() {
//モーター //モーター
motorL.attach( PIN_MOTOR_LEFT_1, PIN_MOTOR_LEFT_2); motorL.attach( PIN_MOTOR_LEFT_1, PIN_MOTOR_LEFT_2);
motorR.attach(PIN_MOTOR_RIGHT_1, PIN_MOTOR_RIGHT_2); motorR.attach(PIN_MOTOR_RIGHT_1, PIN_MOTOR_RIGHT_2);
motorMode(STOP, STOP, 0, 0); motorL.mode(STOP);
motorR.mode(STOP);
//センサー //センサー
sensorInit(); sensorInit();
//エンコーダー //エンコーダー
@ -208,108 +231,83 @@ void setup() {
pinMode(PIN_LED, OUTPUT); pinMode(PIN_LED, OUTPUT);
digitalWrite(PIN_LED, HIGH); digitalWrite(PIN_LED, HIGH);
//モード //モード
mode = MODE_STOP; mode = MODE_TRACE;
delay(1000); delay(1000);
/* スタート */ /* スタート */
digitalWrite(PIN_LED, LOW); digitalWrite(PIN_LED, LOW);
mode = MODE_TRACE;
handle(0); handle(0);
lance(0); lance(0);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
} }
void loop(){ void loop(){
//センサーを見る //センサーを見る
byte sensor = 0; byte sensor = 0;
#ifdef LINE_BLACK //黒ライン #ifdef LINE_BLACK //黒ライン
// sensor = PIN_SENSOR;
sensor = sensorRead(); sensor = sensorRead();
#else #else
// sensor = ~PIN_SENSOR;
sensor = ~sensorRead(); sensor = ~sensorRead();
#endif #endif
//ロータリーエンコーダーの変化を見る //ロータリーエンコーダーの変化を見る
count(); count();
//ランスの動作を決定する //平行標的を狙う時は、ランスをぷるぷるする
switch(target){
case TARGET_NONE:
lanceAngle = LANCE_ANGLE0;
if(counter - counterOld > TARGET_NONE_COUNT ) target = 0;
break;
default:
switch( sensor & MASK_CHECK_MARKER ){
case 0x81:
target = TARGET_NONE;
counterOld = counter;
break;
case 0x01:
lanceAngle = LANCE_ANGLE1;
target = TARGET_PARALLEL;
break;
case 0x80:
lanceAngle = LANCE_ANGLE3;
target = TARGET_VERTICAL;
break;
}
break;
}
if( lanceAngle == LANCE_ANGLE1 && counter%LAP_COUNT > LAP_COUNT/2) lanceAngle = LANCE_ANGLE4;
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(lanceAngle){ switch(servoLance.read()){
case LANCE_ANGLE1: case LANCE_ANGLE1:
lanceAngle = LANCE_ANGLE2; lance(LANCE_ANGLE2);
break; break;
case LANCE_ANGLE2: case LANCE_ANGLE2:
lanceAngle = LANCE_ANGLE1; lance(LANCE_ANGLE1);
break; break;
} }
} }
//ランスの動作を決定する
switch(target){
case TARGET_NONE:
lanceAngle = LANCE_ANGLE0;
if(counter - counterOld > TARGET_NONE_COUNT ) target = 0;
break;
case TARGET_PARALLEL:
case TARGET_VERTICAL:
case TARGET_CYLINDER:
default:
switch( sensor & MASK_CHECK_MARKER ){
case 0x81:
target = TARGET_NONE;
counterOld = counter;
break;
case 0x01:
lanceAngle = LANCE_ANGLE1;
target = TARGET_PARALLEL;
break;
case 0x80:
lanceAngle = LANCE_ANGLE3;
target = TARGET_VERTICAL;
break;
}
break;
}
//全体の動作を決定する //全体の動作を決定する
switch(mode){ switch(mode){
case MODE_TARGET_RIGHT_VERTICAL: //右垂直標的
lance(LANCE_ANGLE4);
mode = MODE_TRACE;
break;
case MODE_TARGET_LEFT_VERTICAL: //左垂直標的
lance(LANCE_ANGLE3);
mode = MODE_TRACE;
break;
case MODE_TARGET_PARALLEL1: //平行標的1
lance(LANCE_ANGLE1);
mode = MODE_TRACE;
break;
case MODE_TARGET_PARALLEL2: //平行標的2
lance(LANCE_ANGLE2);
mode = MODE_TRACE;
break;
case MODE_TRACE: case MODE_TRACE:
trace(sensor); trace(sensor);
if( counter > LAP_COUNT*6 - LAP_COUNT/14 ){ // 6周したら、円筒標的を狙いに行く mode = checkMarker(sensor);
//6周する前に、円筒標的を狙いに行く
if( counter > LAP_COUNT*6 - LAP_COUNT/14 ){
mode = MODE_TARGET_CYLINDER;
}
break;
case MODE_LEFT: //左カーブ
trace(sensor);
lance(LANCE_ANGLE0);
if(counter - counterOld > TARGET_NONE_COUNT) 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){
counterOld = counter;
}else if( counter - counterOld > LAP_COUNT/21 ){
mode = MODE_STOP; mode = MODE_STOP;
} }
break; break;
case MODE_STOP: case MODE_STOP: //停止
default: motorL.mode(STOP);
digitalWrite(PIN_LED, HIGH); motorR.mode(STOP);
handle(0); break;
lanceAngle = -90;
motorMode(GO, GO, 0xff, 0xff);
if( counter - counterOld > LAP_COUNT/21 ){
motorMode(STOP, STOP, 0xff, 0xff);
}
} }
lance(lanceAngle);
} }