495 lines
12 KiB
C++
495 lines
12 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"
|
|
|
|
const unsigned long LAP_COUNT = 848; //カウント/周
|
|
const unsigned long TARGET_NONE_COUNT = LAP_COUNT*90/848; //カーブ中でのカウント数
|
|
|
|
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 = 128;
|
|
|
|
// Servo : DHCM (2013-03-13)
|
|
// 60deg : 1250 us
|
|
// 90deg : 1440 us
|
|
//120deg : 1630 us
|
|
const int HANDLE_DEGREE_DEFAULT = 90; //度
|
|
const unsigned int HANDLE_MIN = (1440-570); //us
|
|
const unsigned int HANDLE_MAX = (1440+570); //us
|
|
|
|
const float HANDLE_KP = 4;
|
|
|
|
//Servo : SC-0352
|
|
// 60deg : 1265 us
|
|
// 90deg : 1535 us
|
|
//120deg : 1805 us
|
|
const int LANCE_DEGREE_DEFAULT = 90; //度
|
|
const unsigned int LANCE_MIN = (1535-780); //us
|
|
const unsigned int LANCE_MAX = (1535+780); //us
|
|
|
|
//const byte PIN_BUZZER = 3;
|
|
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 = 9;
|
|
const byte PIN_SERVO_LANCE = 10;
|
|
|
|
const byte PIN_MOTOR_LEFT = 6;
|
|
const byte PIN_MOTOR_RIGHT = 5;
|
|
|
|
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;
|
|
|
|
/**************************************/
|
|
|
|
Servo servoHandle;
|
|
Servo servoLance;
|
|
Motor motorR;
|
|
Motor motorL;
|
|
|
|
byte mode;
|
|
volatile unsigned long counter = 0;
|
|
unsigned long modeChangeCounter;
|
|
|
|
/**************************************/
|
|
|
|
/* センサーを読んで、車体の動作を決定する */
|
|
void trace(byte sensor){
|
|
switch(sensor&MASK_MODE_TRACE){
|
|
case 0b01000000:
|
|
handle(-5*HANDLE_KP);
|
|
motorMode(GO, GO, 0, SPEED_DEFAULT*0.7);
|
|
break;
|
|
case 0b01100000:
|
|
handle(-4*HANDLE_KP);
|
|
motorMode(GO, GO, 0, SPEED_DEFAULT*0.7);
|
|
break;
|
|
case 0b00100000:
|
|
handle(-3*HANDLE_KP);
|
|
motorMode(GO, GO, 0, SPEED_DEFAULT);
|
|
break;
|
|
case 0b00110000:
|
|
handle(-4);
|
|
motorMode(GO, GO, 0, 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(4);
|
|
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
|
break;
|
|
case 0b00000100:
|
|
handle(3*HANDLE_KP);
|
|
motorMode(GO, GO, 0, 0);
|
|
break;
|
|
case 0b00000110:
|
|
handle(4*HANDLE_KP);
|
|
motorMode(GO, GO, 0, 0);
|
|
break;
|
|
case 0b00000010:
|
|
handle(5*HANDLE_KP);
|
|
motorMode(GO, GO, 0, 0);
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* MODE_LEFT時の再加速用trace() */
|
|
void traceLeft(byte sensor){
|
|
switch(sensor&MASK_MODE_TRACE){
|
|
case 0b01000000:
|
|
handle(-5*HANDLE_KP);
|
|
motorMode(GO, GO, 0, SPEED_DEFAULT);
|
|
break;
|
|
case 0b01100000:
|
|
handle(-4*HANDLE_KP);
|
|
motorMode(GO, GO, 0, SPEED_DEFAULT);
|
|
break;
|
|
case 0b00100000:
|
|
handle(-3*HANDLE_KP);
|
|
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
|
break;
|
|
case 0b00110000:
|
|
handle(-2*HANDLE_KP);
|
|
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(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 motorMode(byte leftMode, byte rightMode, byte leftSpeed, byte rightSpeed){
|
|
motorL.mode( leftMode, leftSpeed);
|
|
motorR.mode(rightMode, rightSpeed);
|
|
}
|
|
|
|
/* ステアリングを中央からdegree[度]だけ動かす
|
|
<- - 0 + ->
|
|
| |
|
|
| |
|
|
[]---{}---[]
|
|
/ .\
|
|
*/
|
|
void handle(int degree){
|
|
servoHandle.write(HANDLE_DEGREE_DEFAULT - degree);
|
|
}
|
|
|
|
/* ランスを中央からdegree[度]だけ動かす
|
|
<- - 0 + ->
|
|
/ .\
|
|
/ | \
|
|
[]+--{}--+[]
|
|
*/
|
|
void lance(int degree){
|
|
servoLance.write(LANCE_DEGREE_DEFAULT - degree);
|
|
}
|
|
|
|
/* ロータリーエンコーダーの変化を見る
|
|
todo:外部割り込み使ってみたい、、、
|
|
*/
|
|
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 rotRotEnc(){
|
|
counter++;
|
|
}
|
|
|
|
/* マーカーを読んで、モードを返す */
|
|
int checkMarker(byte sensor){
|
|
int mode = MODE_TRACE;
|
|
switch(sensor & MASK_CHECK_MARKER){
|
|
case 0b10000001:
|
|
modeChangeCounter = counter;
|
|
mode = MODE_LEFT;
|
|
break;
|
|
case 0b00000001:
|
|
modeChangeCounter = counter;
|
|
if(counter%LAP_COUNT > LAP_COUNT/2){ //後半は垂直標的をねらいたい
|
|
mode = MODE_TARGET_RIGHT_VERTICAL;
|
|
}else if(counter%LAP_COUNT < LAP_COUNT/7){ //まずは、E的をねらいたい
|
|
mode = MODE_TARGET_PARALLEL1;
|
|
}else if(counter%LAP_COUNT < LAP_COUNT/3){ //次に、F的をねらいたい
|
|
mode = MODE_TARGET_PARALLEL2;
|
|
}else{
|
|
mode = MODE_TARGET_RIGHT_VERTICAL;
|
|
}
|
|
break;
|
|
case 0b10000000:
|
|
modeChangeCounter = counter;
|
|
mode = MODE_TARGET_LEFT_VERTICAL;
|
|
break;
|
|
}
|
|
return mode;
|
|
}
|
|
|
|
/* 前方センサーの初期化 */
|
|
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; //黒が1、白が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;
|
|
}
|
|
/* センサーの状態を返す */
|
|
//byte sensorRead(){
|
|
// static byte sensor = 0; //黒が0、白が1
|
|
// int pos = 0;
|
|
// int i = 0;
|
|
// if(!digitalRead(PIN_SENSOR_0))i++, pos += -7;
|
|
// if(!digitalRead(PIN_SENSOR_1))i++, pos += -5;
|
|
// if(!digitalRead(PIN_SENSOR_2))i++, pos += -3;
|
|
// if(!digitalRead(PIN_SENSOR_3))i++, pos += -1;
|
|
// if(!digitalRead(PIN_SENSOR_4))i++, pos += 1;
|
|
// if(!digitalRead(PIN_SENSOR_5))i++, pos += 3;
|
|
// if(!digitalRead(PIN_SENSOR_6))i++, pos += 5;
|
|
// if(!digitalRead(PIN_SENSOR_7))i++, pos += 7;
|
|
// if(i!=0) pos = pos/i;
|
|
// else return sensor;
|
|
// switch(pos){
|
|
// case 7:
|
|
// sensor = 0b00000001;
|
|
// break;
|
|
// case 6:
|
|
// sensor = 0b00000011;
|
|
// break;
|
|
// case 5:
|
|
// sensor = 0b00000010;
|
|
// break;
|
|
// case 4:
|
|
// sensor = 0b00000110;
|
|
// break;
|
|
// case 3:
|
|
// sensor = 0b00000100;
|
|
// break;
|
|
// case 2:
|
|
// sensor = 0b00001100;
|
|
// break;
|
|
// case 1:
|
|
// sensor = 0b00001000;
|
|
// break;
|
|
// case 0:
|
|
// sensor = 0b00011000;
|
|
// break;
|
|
// case -1:
|
|
// sensor = 0b00010000;
|
|
// break;
|
|
// case -2:
|
|
// sensor = 0b00110000;
|
|
// break;
|
|
// case -3:
|
|
// sensor = 0b00100000;
|
|
// break;
|
|
// case -4:
|
|
// sensor = 0b01100000;
|
|
// break;
|
|
// case -5:
|
|
// sensor = 0b01000000;
|
|
// break;
|
|
// case -6:
|
|
// sensor = 0b11000000;
|
|
// break;
|
|
// case -7:
|
|
// sensor = 0b10000000;
|
|
// break;
|
|
// }
|
|
// return sensor;
|
|
//}
|
|
|
|
void init_sen() {
|
|
pinMode(12, INPUT);
|
|
pinMode(13, INPUT);
|
|
pinMode(14, INPUT);
|
|
pinMode(15, INPUT);
|
|
pinMode(16, INPUT);
|
|
pinMode(17, INPUT);
|
|
pinMode(18, INPUT);
|
|
pinMode(19, INPUT);
|
|
Serial.begin(9600);
|
|
}
|
|
|
|
void printLine() {
|
|
Serial.print(digitalRead(12));
|
|
Serial.print('\t');
|
|
Serial.print(digitalRead(13));
|
|
Serial.print('\t');
|
|
Serial.print(digitalRead(14));
|
|
Serial.print('\t');
|
|
Serial.print(digitalRead(15));
|
|
Serial.print('\t');
|
|
Serial.print(digitalRead(16));
|
|
Serial.print('\t');
|
|
Serial.print(digitalRead(17));
|
|
Serial.print('\t');
|
|
Serial.print(digitalRead(18));
|
|
Serial.print('\t');
|
|
Serial.print(digitalRead(19));
|
|
Serial.print('\n');
|
|
}
|
|
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);
|
|
motorR.attach(PIN_MOTOR_RIGHT);
|
|
motorL.mode(STOP);
|
|
motorR.mode(STOP);
|
|
//センサー
|
|
sensorInit();
|
|
//エンコーダー
|
|
pinMode(PIN_ROT, INPUT);
|
|
// digitalWrite(PIN_ROT, HIGH);
|
|
attachInterrupt(0, rotRotEnc, FALLING);
|
|
//LED
|
|
// pinMode(PIN_LED, OUTPUT);
|
|
// digitalWrite(PIN_LED, HIGH);
|
|
//モード
|
|
mode = MODE_TRACE;
|
|
delay(3000);
|
|
/* スタート */
|
|
// digitalWrite(PIN_LED, LOW);
|
|
handle(0);
|
|
lance(0);
|
|
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
|
}
|
|
|
|
void loop(){
|
|
// printLine();
|
|
/* センサーを見る */
|
|
byte sensor = 0;
|
|
sensor = ~sensorRead(); //ライン(白)が1、地面(黒)が0
|
|
|
|
for(;;){
|
|
sensor = ~sensorRead(); //ライン(白)が1、地面(黒)が0
|
|
trace(sensor);
|
|
if(millis()>123000){
|
|
motorL.mode(STOP);
|
|
motorR.mode(STOP);
|
|
for(;;){};
|
|
}
|
|
if(counter>10000){
|
|
motorL.mode(STOP);
|
|
motorR.mode(STOP);
|
|
for(;;){};
|
|
}
|
|
}
|
|
count(); //ロータリーエンコーダーの変化を見る
|
|
/* 全体の動作を決定する */
|
|
switch(mode){
|
|
case MODE_TARGET_RIGHT_VERTICAL: //右垂直標的
|
|
lance(LANCE_ANGLE_RIGHT_VERTICAL);
|
|
mode = MODE_TRACE;
|
|
break;
|
|
case MODE_TARGET_LEFT_VERTICAL: //左垂直標的
|
|
lance(LANCE_ANGLE_LEFT_VERTICAL);
|
|
mode = MODE_TRACE;
|
|
break;
|
|
case MODE_TARGET_PARALLEL1: //平行標的1
|
|
trace(sensor);
|
|
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);
|
|
if(counter - modeChangeCounter > 26){ //マーカーから少し進んで、叩く
|
|
lance(LANCE_ANGLE_PARALLEL2+8);
|
|
mode = MODE_TRACE;
|
|
}
|
|
break;
|
|
case MODE_TRACE:
|
|
trace(sensor);
|
|
mode = checkMarker(sensor);
|
|
if(counter > LAP_COUNT*10){ //10周したら停止
|
|
mode = MODE_STOP;
|
|
}
|
|
break;
|
|
case MODE_LEFT: //左カーブ
|
|
// trace(sensor);
|
|
if(counter - modeChangeCounter < TARGET_NONE_COUNT/2) trace(sensor);
|
|
else traceLeft(sensor); //カーブの後半は加速したい
|
|
if(counter < LAP_COUNT) lance(30); //1周目は円筒標的をねらいたい
|
|
else lance(-30);
|
|
if(counter - modeChangeCounter > TARGET_NONE_COUNT){
|
|
lance(0);
|
|
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){
|
|
// modeChangeCounter = counter;
|
|
// }else if( counter - modeChangeCounter > LAP_COUNT/21 ){
|
|
// mode = MODE_STOP;
|
|
// }
|
|
// break;
|
|
case MODE_STOP: //停止
|
|
// digitalWrite(PIN_LED, HIGH);
|
|
motorL.mode(STOP);
|
|
motorR.mode(STOP);
|
|
break;
|
|
}
|
|
}
|