413 lines
9.5 KiB
Arduino
413 lines
9.5 KiB
Arduino
|
/*
|
|||
|
競技用ランサーロボット
|
|||
|
半蔵 2.1 - PR用
|
|||
|
|
|||
|
[7 654321 0]
|
|||
|
| |
|
|||
|
| |
|
|||
|
[]---{}---[]
|
|||
|
/ .\
|
|||
|
/ | \
|
|||
|
[]+--{}--+[]
|
|||
|
|
|||
|
(C)2013 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
|
|||
|
*/
|
|||
|
|
|||
|
#include <Servo.h>
|
|||
|
#include "Motor.h"
|
|||
|
#include "pitches.h"
|
|||
|
|
|||
|
void disk0(){
|
|||
|
/*
|
|||
|
$pu1
|
|||
|
l12 o5
|
|||
|
c4f4b-4agfg8.g16
|
|||
|
<c4e-4dc>b-<c4>a4b-4
|
|||
|
f8.d16
|
|||
|
$pu2
|
|||
|
f2.
|
|||
|
*/
|
|||
|
|
|||
|
int melodyDisk0[] = {
|
|||
|
NOTE_C4, NOTE_F4, NOTE_AS4, NOTE_A4, NOTE_G4, NOTE_F4, NOTE_G4,
|
|||
|
NOTE_C5, NOTE_DS5,NOTE_D5, NOTE_C5,NOTE_AS4,
|
|||
|
NOTE_C5,NOTE_A4,NOTE_AS4,
|
|||
|
NOTE_F4,NOTE_D4,NOTE_F4
|
|||
|
};
|
|||
|
|
|||
|
// note durations: 4 = quarter note, 8 = eighth note, etc.:
|
|||
|
int noteDurationsDisk0[] = {
|
|||
|
4,4,4,11,11,11,4,
|
|||
|
4,4,11,11,11,
|
|||
|
4,4,4,
|
|||
|
6,11,2
|
|||
|
};
|
|||
|
|
|||
|
for (int thisNote = 0; thisNote < 18; thisNote++) {
|
|||
|
int noteDuration = 1300/noteDurationsDisk0[thisNote];
|
|||
|
tone(3, melodyDisk0[thisNote],noteDuration);
|
|||
|
int pauseBetweenNotes = noteDuration * 1.30;
|
|||
|
delay(pauseBetweenNotes);
|
|||
|
noTone(3);
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
void disk1(){
|
|||
|
int melodyDisk1[] = {NOTE_B5,NOTE_D6,NOTE_G6};
|
|||
|
for (int thisNote = 0; thisNote < 3; thisNote++) {
|
|||
|
tone(3, melodyDisk1[thisNote]);
|
|||
|
delay(100);
|
|||
|
noTone(3);
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
|
|||
|
const unsigned long LAP_DISTANCE = 500; //ロータリーエンコーダーのカウント数/周
|
|||
|
const unsigned long TURN_DISTANCE = 50; //カーブ中でのカウント数
|
|||
|
|
|||
|
const int LANCE_ANGLE_PARALLEL_1 = 90 - 62; //度
|
|||
|
const int LANCE_ANGLE_PARALLEL_2 = 90 - 50; //度
|
|||
|
const int LANCE_ANGLE_VERTICAL_L = 90 - 120; //度
|
|||
|
const int LANCE_ANGLE_VERTICAL_R = 90 - 62; //度
|
|||
|
|
|||
|
const byte SPEED_DEFAULT = 0xff;
|
|||
|
|
|||
|
//Servo : SC-0352
|
|||
|
// 60deg : 1265 us
|
|||
|
// 90deg : 1535 us
|
|||
|
//120deg : 1805 us
|
|||
|
const int STEERING_DEFAULT = 90; //度
|
|||
|
const unsigned int STEERING_MIN = ( 800+30); //us
|
|||
|
const unsigned int STEERING_MAX = (2300+30); //us
|
|||
|
|
|||
|
const int LANCE_DEFAULT = 90; //度
|
|||
|
const unsigned int LANCE_MIN = ( 544-62); //us
|
|||
|
const unsigned int LANCE_MAX = (2400-62); //us
|
|||
|
|
|||
|
const byte PIN_LED = 4;
|
|||
|
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_STEERING = 10;
|
|||
|
const byte PIN_SERVO_LANCE = 9;
|
|||
|
|
|||
|
const byte MASK_LINE = 0b01111110;
|
|||
|
const byte MASK_MARKER = 0b10000001;
|
|||
|
|
|||
|
/**************************************/
|
|||
|
|
|||
|
Servo steeringServo;
|
|||
|
Servo lanceServo;
|
|||
|
Motor motorR;
|
|||
|
Motor motorL;
|
|||
|
|
|||
|
//ラインが1、地面が0、LSBが左端、MSBが右端
|
|||
|
byte sensor = 0x00;
|
|||
|
|
|||
|
volatile unsigned long distance = 0;
|
|||
|
unsigned long modeChangedDistance = 0;
|
|||
|
|
|||
|
/**************************************/
|
|||
|
|
|||
|
enum mode_t {
|
|||
|
MODE_STOP, //停止
|
|||
|
MODE_STRAIGHT, //まっすぐ進む
|
|||
|
MODE_TURN, //曲がる
|
|||
|
MODE_TURN_LEFT, //左に曲がる
|
|||
|
MODE_TURN_RIGHT,//右に曲がる
|
|||
|
MODE_ATTACK_PARALLEL_1, //平行標的1
|
|||
|
MODE_ATTACK_PARALLEL_2, //平行標的2
|
|||
|
MODE_ATTACK_VERTICAL_R, //右垂直標的
|
|||
|
MODE_ATTACK_VERTICAL_L, //左垂直標的
|
|||
|
MODE_ATTACK_CYLINDER //円筒標的
|
|||
|
}
|
|||
|
mode;
|
|||
|
|
|||
|
/**************************************/
|
|||
|
|
|||
|
/* センサーを読んで、車体の動作を決定する */
|
|||
|
void trace(){
|
|||
|
switch(sensor&MASK_LINE){
|
|||
|
case 0b01000000:
|
|||
|
steering(-20);
|
|||
|
motorL.speed(SPEED_DEFAULT/2);
|
|||
|
motorR.speed(SPEED_DEFAULT);
|
|||
|
break;
|
|||
|
case 0b01100000:
|
|||
|
steering(-15);
|
|||
|
motorL.speed(SPEED_DEFAULT/2);
|
|||
|
motorR.speed(SPEED_DEFAULT);
|
|||
|
break;
|
|||
|
case 0b00100000:
|
|||
|
steering(-11);
|
|||
|
motorL.speed(SPEED_DEFAULT);
|
|||
|
motorR.speed(SPEED_DEFAULT);
|
|||
|
break;
|
|||
|
case 0b00110000:
|
|||
|
steering(-8);
|
|||
|
motorL.speed(SPEED_DEFAULT);
|
|||
|
motorR.speed(SPEED_DEFAULT);
|
|||
|
break;
|
|||
|
case 0b00010000:
|
|||
|
steering(-4);
|
|||
|
motorL.speed(SPEED_DEFAULT);
|
|||
|
motorR.speed(SPEED_DEFAULT);
|
|||
|
break;
|
|||
|
case 0b00011000:
|
|||
|
steering(0);
|
|||
|
motorL.speed(SPEED_DEFAULT);
|
|||
|
motorR.speed(SPEED_DEFAULT);
|
|||
|
break;
|
|||
|
case 0b00001000:
|
|||
|
steering(4);
|
|||
|
motorL.speed(SPEED_DEFAULT);
|
|||
|
motorR.speed(SPEED_DEFAULT);
|
|||
|
break;
|
|||
|
case 0b00001100:
|
|||
|
steering(8);
|
|||
|
motorL.speed(SPEED_DEFAULT);
|
|||
|
motorR.speed(SPEED_DEFAULT);
|
|||
|
break;
|
|||
|
case 0b00000100:
|
|||
|
steering(11);
|
|||
|
motorL.speed(SPEED_DEFAULT);
|
|||
|
motorR.speed(SPEED_DEFAULT);
|
|||
|
break;
|
|||
|
case 0b00000110:
|
|||
|
steering(15);
|
|||
|
motorL.speed(SPEED_DEFAULT);
|
|||
|
motorR.speed(SPEED_DEFAULT);
|
|||
|
break;
|
|||
|
case 0b00000010:
|
|||
|
steering(20);
|
|||
|
motorL.speed(SPEED_DEFAULT);
|
|||
|
motorR.speed(SPEED_DEFAULT);
|
|||
|
break;
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
/* ステアリングを中央からa[度]だけ動かす
|
|||
|
<- - 0 + ->
|
|||
|
| |
|
|||
|
| |
|
|||
|
[]---{}---[]
|
|||
|
/ .\
|
|||
|
*/
|
|||
|
void steering(int a){
|
|||
|
steeringServo.write(STEERING_DEFAULT + a);
|
|||
|
}
|
|||
|
|
|||
|
/* ランスを中央からa[度]だけ動かす
|
|||
|
<- - 0 + ->
|
|||
|
/ .\
|
|||
|
/ | \
|
|||
|
[]+--{}--+[]
|
|||
|
*/
|
|||
|
void lance(int a){
|
|||
|
lanceServo.write(LANCE_DEFAULT - a);
|
|||
|
}
|
|||
|
|
|||
|
/* センサーのアレイの初期化 */
|
|||
|
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 b=0; //黒が1、白が0
|
|||
|
// if(analogRead(PIN_SENSOR_0)>511) b += 0b00000001; //右端
|
|||
|
// if(analogRead(PIN_SENSOR_1)>511) b += 0b00000010;
|
|||
|
if(analogRead(PIN_SENSOR_2)>511) b += 0b00000100;
|
|||
|
if(analogRead(PIN_SENSOR_3)>511) b += 0b00001000;
|
|||
|
if(analogRead(PIN_SENSOR_4)>511) b += 0b00010000;
|
|||
|
if(analogRead(PIN_SENSOR_5)>511) b += 0b00100000;
|
|||
|
if(analogRead(PIN_SENSOR_6)>511) b += 0b01000000;
|
|||
|
if(analogRead(PIN_SENSOR_7)>511) b += 0b10000000; //左端
|
|||
|
return sensor = b;
|
|||
|
}
|
|||
|
|
|||
|
void modeSet(){
|
|||
|
static int i = 0;
|
|||
|
if(sensor == 0){
|
|||
|
i++;
|
|||
|
}else i=0;
|
|||
|
if(i>0){
|
|||
|
mode = MODE_TURN_LEFT;
|
|||
|
return;
|
|||
|
}
|
|||
|
|
|||
|
switch(sensor&MASK_MARKER){
|
|||
|
// default:
|
|||
|
// case 0b00000000:
|
|||
|
// mode = MODE_STRAIGHT;
|
|||
|
// break;
|
|||
|
// case 0b10000001:
|
|||
|
// modeChangedDistance = distance;
|
|||
|
// mode = MODE_TURN;
|
|||
|
// break;
|
|||
|
case 0b00000001:
|
|||
|
modeChangedDistance = distance;
|
|||
|
mode = MODE_ATTACK_PARALLEL_1;
|
|||
|
break;
|
|||
|
case 0b10000000:
|
|||
|
modeChangedDistance = distance;
|
|||
|
mode = MODE_ATTACK_VERTICAL_L;
|
|||
|
break;
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
/* ロータリーエンコーダーの変化を見る */
|
|||
|
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];
|
|||
|
distance++;
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
void startBeep(){
|
|||
|
tone(PIN_BUZZER,2000);
|
|||
|
delay(100);
|
|||
|
tone(PIN_BUZZER,1000);
|
|||
|
delay(100);
|
|||
|
noTone(PIN_BUZZER);
|
|||
|
}
|
|||
|
|
|||
|
void setup(){
|
|||
|
//サーボ
|
|||
|
steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERING_MAX);
|
|||
|
lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
|
|||
|
steering(0);
|
|||
|
lance(0);
|
|||
|
//モーター
|
|||
|
motorL.attach(5,7);
|
|||
|
motorR.attach(6,8);
|
|||
|
motorL.mode(STOP);
|
|||
|
motorR.mode(STOP);
|
|||
|
//センサー
|
|||
|
sensorInit();
|
|||
|
//エンコーダー
|
|||
|
pinMode(PIN_ROT, INPUT);
|
|||
|
//ブザー
|
|||
|
pinMode(PIN_BUZZER, OUTPUT);
|
|||
|
//LED
|
|||
|
pinMode(PIN_LED, OUTPUT);
|
|||
|
digitalWrite(PIN_LED, HIGH);
|
|||
|
/* 停止 */
|
|||
|
motorL.mode(STOP);
|
|||
|
motorR.mode(STOP);
|
|||
|
delay(1000);
|
|||
|
disk1();
|
|||
|
delay(1000);
|
|||
|
/* スタート */
|
|||
|
digitalWrite(PIN_LED, LOW);
|
|||
|
mode = MODE_STRAIGHT;
|
|||
|
// Serial.begin(9600);
|
|||
|
// while(1){
|
|||
|
// count();
|
|||
|
// Serial.println(distance);
|
|||
|
// }
|
|||
|
}
|
|||
|
|
|||
|
void loop(){
|
|||
|
count();
|
|||
|
if(distance > 500){
|
|||
|
mode = MODE_STOP;
|
|||
|
}
|
|||
|
sensorRead();
|
|||
|
switch(mode){
|
|||
|
default:
|
|||
|
case MODE_STOP: //停止
|
|||
|
motorL.mode(STOP);
|
|||
|
motorR.mode(STOP);
|
|||
|
digitalWrite(PIN_LED, HIGH);
|
|||
|
delay(1000);
|
|||
|
lance(0);
|
|||
|
steering(0);
|
|||
|
delay(500);
|
|||
|
disk0();
|
|||
|
// delay(1000);
|
|||
|
// tone(PIN_BUZZER,2000);
|
|||
|
// delay(500);
|
|||
|
// noTone(PIN_BUZZER);
|
|||
|
while(1){
|
|||
|
;
|
|||
|
};
|
|||
|
break;
|
|||
|
case MODE_STRAIGHT: //まっすぐ進む
|
|||
|
trace();
|
|||
|
modeSet();
|
|||
|
break;
|
|||
|
case MODE_TURN_LEFT: //左に曲がる
|
|||
|
lance(-70);
|
|||
|
if(distance - modeChangedDistance < TURN_DISTANCE){
|
|||
|
steering(-30);
|
|||
|
motorL.speed(SPEED_DEFAULT/2);
|
|||
|
motorR.speed(SPEED_DEFAULT);
|
|||
|
}else mode = MODE_STOP;
|
|||
|
break;
|
|||
|
case MODE_TURN_RIGHT://右に曲がる
|
|||
|
lance(0);
|
|||
|
if(distance - modeChangedDistance < TURN_DISTANCE){
|
|||
|
steering(30);
|
|||
|
motorL.speed(SPEED_DEFAULT);
|
|||
|
motorR.speed(SPEED_DEFAULT/2);
|
|||
|
}else mode = MODE_STOP;
|
|||
|
break;
|
|||
|
case MODE_ATTACK_PARALLEL_1: //平行標的1
|
|||
|
trace();
|
|||
|
lance(LANCE_ANGLE_PARALLEL_1);
|
|||
|
if(distance - modeChangedDistance > 255){ //マーカーから少し進んで、叩く
|
|||
|
lance(LANCE_ANGLE_PARALLEL_1 + 10);
|
|||
|
mode = MODE_STRAIGHT;
|
|||
|
}
|
|||
|
break;
|
|||
|
case MODE_ATTACK_PARALLEL_2: //平行標的2
|
|||
|
trace();
|
|||
|
lance(LANCE_ANGLE_PARALLEL_2);
|
|||
|
if(distance - modeChangedDistance > 283){ //マーカーから少し進んで、叩く
|
|||
|
lance(LANCE_ANGLE_PARALLEL_2 + 10);
|
|||
|
mode = MODE_STRAIGHT;
|
|||
|
}
|
|||
|
break;
|
|||
|
case MODE_ATTACK_VERTICAL_R: //右垂直標
|
|||
|
trace();
|
|||
|
lance(LANCE_ANGLE_VERTICAL_R);
|
|||
|
if(distance - modeChangedDistance > 300){ //マーカーから少し進んで、まっすぐに戻す
|
|||
|
lance(0);
|
|||
|
mode = MODE_STRAIGHT;
|
|||
|
}
|
|||
|
break;
|
|||
|
case MODE_ATTACK_VERTICAL_L: //左垂直標
|
|||
|
trace();
|
|||
|
lance(LANCE_ANGLE_VERTICAL_L);
|
|||
|
if(distance - modeChangedDistance > 5){ //マーカーから少し進んで、まっすぐに戻す
|
|||
|
lance(0);
|
|||
|
mode = MODE_STRAIGHT;
|
|||
|
}
|
|||
|
break;
|
|||
|
case MODE_ATTACK_CYLINDER: //円筒標的
|
|||
|
trace();
|
|||
|
mode = MODE_STRAIGHT;
|
|||
|
break;
|
|||
|
}
|
|||
|
}
|