309 lines
7.7 KiB
C++
309 lines
7.7 KiB
C++
/*
|
|
競技用ランサーロボット
|
|
半蔵 2.1
|
|
|
|
[7 654321 0]
|
|
| |
|
|
| |
|
|
[]---{}---[]
|
|
/ .\
|
|
/ | \
|
|
[]+--{}--+[]
|
|
|
|
(c)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
|
|
*/
|
|
|
|
#include <Servo.h>
|
|
#include "Motor.h"
|
|
|
|
//#define LINE_BLACK //白地・黒ライン有効
|
|
|
|
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 byte SPEED_DEFAULT = 255;
|
|
|
|
const int HANDLE_DEGREE_DEFAULT = 90; //度
|
|
const unsigned int HANDLE_MIN = ( 800+30); //us
|
|
const unsigned int HANDLE_MAX = (2300+30); //us
|
|
|
|
const int LANCE_DEGREE_DEFAULT = 90; //度
|
|
const unsigned int LANCE_MIN = 544; //us
|
|
const unsigned int LANCE_MAX = 2400; //us
|
|
|
|
const byte PIN_LED = 4;
|
|
const byte PIN_ROT = 2;
|
|
|
|
const byte PIN_SENSOR_0 = 12;
|
|
const byte PIN_SENSOR_1 = 13;
|
|
const byte PIN_SENSOR_2 = 14;
|
|
const byte PIN_SENSOR_3 = 15;
|
|
const byte PIN_SENSOR_4 = 16;
|
|
const byte PIN_SENSOR_5 = 17;
|
|
const byte PIN_SENSOR_6 = 18;
|
|
const byte PIN_SENSOR_7 = 19;
|
|
|
|
const byte PIN_SERVO_HANDLE = 10;
|
|
const byte PIN_SERVO_LANCE = 9;
|
|
|
|
const byte PIN_MOTOR_LEFT_1 = 5;
|
|
const byte PIN_MOTOR_LEFT_2 = 7;
|
|
const byte PIN_MOTOR_RIGHT_1 = 6;
|
|
const byte PIN_MOTOR_RIGHT_2 = 8;
|
|
|
|
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;
|
|
const byte MASK_CHECK_MARKER_RIGHT = 0b00000001;
|
|
const byte MASK_CHECK_MARKER_LEFT = 0b10000000;
|
|
|
|
/**************************************/
|
|
|
|
Servo servoHandle;
|
|
Servo servoLance;
|
|
Motor motorR;
|
|
Motor motorL;
|
|
|
|
byte mode;
|
|
unsigned long counterOld;
|
|
|
|
/**************************************/
|
|
|
|
void trace(byte sensor){
|
|
switch(sensor&MASK_MODE_TRACE){
|
|
case 0b01000000:
|
|
handle(-20);
|
|
motorMode(GO, GO, SPEED_DEFAULT/4, SPEED_DEFAULT);
|
|
// lance(0);
|
|
break;
|
|
case 0b01100000:
|
|
handle(-16);
|
|
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
|
// lance(0);
|
|
break;
|
|
case 0b00100000:
|
|
handle(-12);
|
|
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
|
// lance(0);
|
|
break;
|
|
case 0b00110000:
|
|
handle(-5);
|
|
motorMode(GO, GO, SPEED_DEFAULT, 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:
|
|
handle(5);
|
|
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
|
break;
|
|
case 0b00000100:
|
|
handle(7);
|
|
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
|
break;
|
|
case 0b00000110:
|
|
handle(10);
|
|
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
|
break;
|
|
case 0b00000010:
|
|
handle(12);
|
|
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
|
break;
|
|
}
|
|
}
|
|
|
|
int checkMarker(byte sensor){
|
|
int mode = MODE_TRACE;
|
|
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;
|
|
}
|
|
return mode;
|
|
}
|
|
|
|
void motorMode(byte motorLeftMode, byte motorRightMode, byte motorLeftSpeed, byte motorRightSpeed){
|
|
motorL.mode( motorLeftMode, motorLeftSpeed);
|
|
motorR.mode(motorRightMode, motorRightSpeed);
|
|
}
|
|
|
|
void handle(int degree){
|
|
servoHandle.write(HANDLE_DEGREE_DEFAULT + degree);
|
|
}
|
|
|
|
void lance(int degree){
|
|
servoLance.write(LANCE_DEGREE_DEFAULT - degree);
|
|
}
|
|
|
|
/* ロータリーエンコーダーの変化を見る
|
|
todo:外部割り込み使ってみたい、、、
|
|
*/
|
|
volatile unsigned long counter = 0;
|
|
void count(){
|
|
static byte rot[2] = {0}; //rot[0]:今の状態, rot[1]:前回の状態
|
|
rot[0] = digitalRead(PIN_ROT);
|
|
if(rot[0] != rot[1]){
|
|
rot[1] = rot[0];
|
|
counter++;
|
|
}
|
|
}
|
|
|
|
void sensorInit(){
|
|
pinMode(PIN_SENSOR_0, INPUT);
|
|
pinMode(PIN_SENSOR_1, INPUT);
|
|
pinMode(PIN_SENSOR_2, INPUT);
|
|
pinMode(PIN_SENSOR_3, INPUT);
|
|
pinMode(PIN_SENSOR_4, INPUT);
|
|
pinMode(PIN_SENSOR_5, INPUT);
|
|
pinMode(PIN_SENSOR_6, INPUT);
|
|
pinMode(PIN_SENSOR_7, INPUT);
|
|
}
|
|
|
|
byte sensorRead(){
|
|
byte sensor = 0;
|
|
if(digitalRead(PIN_SENSOR_0)) sensor += 0b00000001;
|
|
if(digitalRead(PIN_SENSOR_1)) sensor += 0b00000010;
|
|
if(digitalRead(PIN_SENSOR_2)) sensor += 0b00000100;
|
|
if(digitalRead(PIN_SENSOR_3)) sensor += 0b00001000;
|
|
if(digitalRead(PIN_SENSOR_4)) sensor += 0b00010000;
|
|
if(digitalRead(PIN_SENSOR_5)) sensor += 0b00100000;
|
|
if(digitalRead(PIN_SENSOR_6)) sensor += 0b01000000;
|
|
if(digitalRead(PIN_SENSOR_7)) sensor += 0b10000000;
|
|
return sensor;
|
|
}
|
|
|
|
void setup() {
|
|
//サーボ
|
|
servoHandle.attach(PIN_SERVO_HANDLE, HANDLE_MIN, HANDLE_MAX);
|
|
servoLance.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
|
|
handle(0);
|
|
lance(0);
|
|
//モーター
|
|
motorL.attach( PIN_MOTOR_LEFT_1, PIN_MOTOR_LEFT_2);
|
|
motorR.attach(PIN_MOTOR_RIGHT_1, PIN_MOTOR_RIGHT_2);
|
|
motorL.mode(STOP);
|
|
motorR.mode(STOP);
|
|
//センサー
|
|
sensorInit();
|
|
//エンコーダー
|
|
pinMode(PIN_ROT, INPUT);
|
|
//LED
|
|
pinMode(PIN_LED, OUTPUT);
|
|
digitalWrite(PIN_LED, HIGH);
|
|
//モード
|
|
mode = MODE_TRACE;
|
|
delay(1000);
|
|
/* スタート */
|
|
digitalWrite(PIN_LED, LOW);
|
|
handle(0);
|
|
lance(0);
|
|
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
|
}
|
|
|
|
void loop(){
|
|
/* センサーを見る */
|
|
byte sensor = 0;
|
|
#ifdef LINE_BLACK //黒ライン
|
|
sensor = sensorRead();
|
|
#else
|
|
sensor = ~sensorRead();
|
|
#endif
|
|
count(); //ロータリーエンコーダーの変化を見る
|
|
/* 全体の動作を決定する */
|
|
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);
|
|
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: //停止
|
|
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_ANGLE1:
|
|
lance(LANCE_ANGLE2);
|
|
break;
|
|
case LANCE_ANGLE2:
|
|
lance(LANCE_ANGLE1);
|
|
break;
|
|
}
|
|
}
|
|
}
|