412 lines
9.5 KiB
C++
412 lines
9.5 KiB
C++
/*
|
||
競技用ランサーロボット
|
||
半蔵 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;
|
||
}
|
||
}
|