update
This commit is contained in:
parent
817a2ecad3
commit
ee9bc099ec
1 changed files with 102 additions and 104 deletions
|
|
@ -17,10 +17,9 @@
|
|||
#include "Motor.h"
|
||||
|
||||
//#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 TARGET_NONE_COUNT = LAP_COUNT/10;
|
||||
|
||||
const int LANCE_ANGLE0 = 0; //度
|
||||
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_2 = 8;
|
||||
|
||||
const byte MODE_STOP = 0x10;
|
||||
const byte MODE_TRACE = 0x11;
|
||||
const byte MODE_LEFT = 0x12; //左カーブ
|
||||
const byte MODE_RIGHT = 0x13; //右カーブ
|
||||
|
||||
const byte TARGET_NONE = 0x40;
|
||||
const byte TARGET_PARALLEL = 0x41; //平行標的
|
||||
const byte TARGET_VERTICAL = 0x42; //垂直標的
|
||||
const byte TARGET_CYLINDER = 0x43; //円筒標的
|
||||
enum {
|
||||
MODE_STOP, //停止
|
||||
MODE_TRACE, //標準
|
||||
MODE_LEFT, //左カーブ
|
||||
MODE_TARGET_PARALLEL1, //平行標的1
|
||||
MODE_TARGET_PARALLEL2, //平行標的2
|
||||
MODE_TARGET_RIGHT_VERTICAL, //右垂直標的
|
||||
MODE_TARGET_LEFT_VERTICAL, //左垂直標的
|
||||
MODE_TARGET_CYLINDER //円筒標的
|
||||
};
|
||||
|
||||
const byte MASK_MODE_TRACE = 0b01111110;
|
||||
const byte MASK_CHECK_MARKER = 0b10000001;
|
||||
|
|
@ -82,60 +82,58 @@ Motor motorR;
|
|||
Motor motorL;
|
||||
|
||||
byte mode;
|
||||
byte target;
|
||||
unsigned int counterOld;
|
||||
char lanceAngle;
|
||||
unsigned long counterOld;
|
||||
|
||||
/**************************************/
|
||||
|
||||
void trace(byte sensor){
|
||||
switch( sensor & MASK_MODE_TRACE ){
|
||||
switch(sensor&MASK_MODE_TRACE){
|
||||
case 0b01000000:
|
||||
handle(-20);
|
||||
motorMode(GO, GO, 0x40, 0xff);
|
||||
lanceAngle = 0;
|
||||
motorMode(GO, GO, SPEED_DEFAULT/4, SPEED_DEFAULT);
|
||||
// lance(0);
|
||||
break;
|
||||
case 0b01100000:
|
||||
handle(-16);
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
lanceAngle = 0;
|
||||
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
||||
// lance(0);
|
||||
break;
|
||||
case 0b00100000:
|
||||
handle(-12);
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
lanceAngle = 0;
|
||||
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
||||
// lance(0);
|
||||
break;
|
||||
case 0b00110000:
|
||||
handle(-5);
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
||||
break;
|
||||
case 0b00010000:
|
||||
handle(-2);
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
||||
break;
|
||||
case 0b00011000:
|
||||
handle(0);
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
||||
break;
|
||||
case 0b00001000:
|
||||
handle(2);
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
||||
break;
|
||||
case 0b00001100:
|
||||
handle(5);
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
||||
break;
|
||||
case 0b00000100:
|
||||
handle(7);
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
||||
break;
|
||||
case 0b00000110:
|
||||
handle(10);
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
||||
break;
|
||||
case 0b00000010:
|
||||
handle(12);
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
||||
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(){
|
||||
pinMode(PIN_SENSOR_0, INPUT);
|
||||
pinMode(PIN_SENSOR_1, INPUT);
|
||||
|
|
@ -199,7 +221,8 @@ void setup() {
|
|||
//モーター
|
||||
motorL.attach( PIN_MOTOR_LEFT_1, PIN_MOTOR_LEFT_2);
|
||||
motorR.attach(PIN_MOTOR_RIGHT_1, PIN_MOTOR_RIGHT_2);
|
||||
motorMode(STOP, STOP, 0, 0);
|
||||
motorL.mode(STOP);
|
||||
motorR.mode(STOP);
|
||||
//センサー
|
||||
sensorInit();
|
||||
//エンコーダー
|
||||
|
|
@ -208,108 +231,83 @@ void setup() {
|
|||
pinMode(PIN_LED, OUTPUT);
|
||||
digitalWrite(PIN_LED, HIGH);
|
||||
//モード
|
||||
mode = MODE_STOP;
|
||||
mode = MODE_TRACE;
|
||||
delay(1000);
|
||||
/* スタート */
|
||||
digitalWrite(PIN_LED, LOW);
|
||||
mode = MODE_TRACE;
|
||||
handle(0);
|
||||
lance(0);
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
||||
}
|
||||
|
||||
void loop(){
|
||||
//センサーを見る
|
||||
byte sensor = 0;
|
||||
#ifdef LINE_BLACK //黒ライン
|
||||
// sensor = PIN_SENSOR;
|
||||
sensor = sensorRead();
|
||||
#else
|
||||
// sensor = ~PIN_SENSOR;
|
||||
sensor = ~sensorRead();
|
||||
#endif
|
||||
//ロータリーエンコーダーの変化を見る
|
||||
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;
|
||||
if(millis() - millis_lance > LANCE_INTERVAL){
|
||||
millis_lance = millis();
|
||||
switch(lanceAngle){
|
||||
switch(servoLance.read()){
|
||||
case LANCE_ANGLE1:
|
||||
lanceAngle = LANCE_ANGLE2;
|
||||
lance(LANCE_ANGLE2);
|
||||
break;
|
||||
case LANCE_ANGLE2:
|
||||
lanceAngle = LANCE_ANGLE1;
|
||||
lance(LANCE_ANGLE1);
|
||||
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){
|
||||
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:
|
||||
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;
|
||||
}
|
||||
break;
|
||||
case MODE_STOP:
|
||||
default:
|
||||
digitalWrite(PIN_LED, HIGH);
|
||||
handle(0);
|
||||
lanceAngle = -90;
|
||||
motorMode(GO, GO, 0xff, 0xff);
|
||||
if( counter - counterOld > LAP_COUNT/21 ){
|
||||
motorMode(STOP, STOP, 0xff, 0xff);
|
||||
case MODE_STOP: //停止
|
||||
motorL.mode(STOP);
|
||||
motorR.mode(STOP);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
lance(lanceAngle);
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue