nebel/hanzo
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

最終版

Browse source
This commit is contained in:
Nebel 2013-03-31 16:25:59 +09:00
parent 9c7651bf9a
commit 6ee50d2c0e
2 changed files with 208 additions and 124 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

235
hanzo3.ino
View file

@ -9,33 +9,32 @@
#include "Motor.h" #include "Motor.h"
const unsigned long LAP_DISTANCE = 10000; //ロータリーエンコーダーのカウント数/周 const unsigned long LAP_DISTANCE = 10000; //ロータリーエンコーダーのカウント数/周
const unsigned long TURN_DISTANCE = LAP_DISTANCE*90/848; //カーブ中でのカウント数 const unsigned long TURN_DISTANCE = 1500; //カーブ中でのカウント数
/*ここまだ*/ const int LANCE_ANGLE_PARALLEL_1 = 90 - 62; //度
const int LANCE_ANGLE_PARALLEL_1 = 90 - 53; //度 const int LANCE_ANGLE_PARALLEL_2 = 90 - 50; //度
const int LANCE_ANGLE_PARALLEL_2 = 90 - 43; //度
const int LANCE_ANGLE_VERTICAL_L = 90 - 120; //度 const int LANCE_ANGLE_VERTICAL_L = 90 - 120; //度
const int LANCE_ANGLE_VERTICAL_R = 90 - 57; //度 const int LANCE_ANGLE_VERTICAL_R = 90 - 62; //度
const byte SPEED_DEFAULT = 128; const byte SPEED_DEFAULT = 0x5f;
// Servo : DHCM (2013-03-13) // Servo : DHCM (2013-03-13)
// 60deg : 1250 us // 60deg : 1250 us
// 90deg : 1440 us // 90deg : 1440 us
//120deg : 1630 us //120deg : 1630 us
const int STEERING_DEFAULT = 90; //度 const int STEERING_DEFAULT = 90; //度
const unsigned int STEERING_MIN = (1440-570); //us const unsigned int STEERING_MIN = (1450-570); //us
const unsigned int STEERING_MAX = (1440+570); //us const unsigned int STEERING_MAX = (1450+570); //us
const float STEERING_KP = 4.0; //const float STEERING_KP = 3.8;
//Servo : SC-0352 //Servo : SC-0352
// 60deg : 1265 us // 60deg : 1265 us
// 90deg : 1535 us // 90deg : 1535 us
//120deg : 1805 us //120deg : 1805 us
const int LANCE_DEFAULT = 90; //度 const int LANCE_DEFAULT = 90; //度
const unsigned int LANCE_MIN = (1535-780); //us const unsigned int LANCE_MIN = (1620-780); //us
const unsigned int LANCE_MAX = (1535+780); //us const unsigned int LANCE_MAX = (1620+780); //us
const byte PIN_BUZZER = 3; const byte PIN_BUZZER = 3;
const byte PIN_ROT = 2; const byte PIN_ROT = 2;
@ -77,47 +76,41 @@ enum mode_t {
MODE_STOP, //停止 MODE_STOP, //停止
MODE_STRAIGHT, //まっすぐ進む MODE_STRAIGHT, //まっすぐ進む
MODE_TURN, //左に曲がる MODE_TURN, //左に曲がる
MODE_ATTACK //標的を狙う MODE_ATTACK_PARALLEL_1, //平行標的1
MODE_ATTACK_PARALLEL_2, //平行標的2
MODE_ATTACK_VERTICAL_R, //右垂直標的
MODE_ATTACK_VERTICAL_L, //左垂直標的
MODE_ATTACK_CYLINDER //円筒標的
} }
mode; mode;
enum target_t {
TARGET_NONE,
TARGET_PARALLEL_1, //平行標的1
TARGET_PARALLEL_2, //平行標的2
TARGET_VERTICAL_R, //右垂直標的
TARGET_VERTICAL_L, //左垂直標的
TARGET_CYLINDER //円筒標的
}
target;
/**************************************/ /**************************************/
/* センサーを読んで、車体の動作を決定する */ /* センサーを読んで、車体の動作を決定する */
void trace(){ void trace(){
switch(sensor&MASK_LINE){ switch(sensor&MASK_LINE){
case 0b01000000: case 0b01000000:
steering(-5*STEERING_KP); steering(-19);
motorL.speed(0); motorL.speed(0);
motorR.speed(SPEED_DEFAULT*0.7); motorR.speed(SPEED_DEFAULT*0.7);
break; break;
case 0b01100000: case 0b01100000:
steering(-4*STEERING_KP); steering(-15);
motorL.speed(0); motorL.speed(0);
motorR.speed(SPEED_DEFAULT*0.7); motorR.speed(SPEED_DEFAULT*0.7);
break; break;
case 0b00100000: case 0b00100000:
steering(-3*STEERING_KP); steering(-11);
motorL.speed(0); motorL.speed(0);
motorR.speed(SPEED_DEFAULT); motorR.speed(SPEED_DEFAULT);
break; break;
case 0b00110000: case 0b00110000:
steering(-2*STEERING_KP); steering(-4);
motorL.speed(0); motorL.speed(SPEED_DEFAULT);
motorR.speed(SPEED_DEFAULT); motorR.speed(SPEED_DEFAULT);
break; break;
case 0b00010000: case 0b00010000:
steering(-1); steering(-2);
motorL.speed(SPEED_DEFAULT); motorL.speed(SPEED_DEFAULT);
motorR.speed(SPEED_DEFAULT); motorR.speed(SPEED_DEFAULT);
break; break;
@ -127,27 +120,27 @@ void trace(){
motorR.speed(SPEED_DEFAULT); motorR.speed(SPEED_DEFAULT);
break; break;
case 0b00001000: case 0b00001000:
steering(1); steering(2);
motorL.speed(SPEED_DEFAULT); motorL.speed(SPEED_DEFAULT);
motorR.speed(SPEED_DEFAULT); motorR.speed(SPEED_DEFAULT);
break; break;
case 0b00001100: case 0b00001100:
steering(2*STEERING_KP); steering(4);
motorL.speed(SPEED_DEFAULT); motorL.speed(SPEED_DEFAULT);
motorR.speed(SPEED_DEFAULT); motorR.speed(SPEED_DEFAULT);
break; break;
case 0b00000100: case 0b00000100:
steering(3*STEERING_KP); steering(11);
motorL.speed(0); motorL.speed(SPEED_DEFAULT);
motorR.speed(0); motorR.speed(SPEED_DEFAULT);
break; break;
case 0b00000110: case 0b00000110:
steering(4*STEERING_KP); steering(15);
motorL.speed(0); motorL.speed(0);
motorR.speed(0); motorR.speed(0);
break; break;
case 0b00000010: case 0b00000010:
steering(5*STEERING_KP); steering(19);
motorL.speed(0); motorL.speed(0);
motorR.speed(0); motorR.speed(0);
break; break;
@ -155,22 +148,22 @@ void trace(){
} }
/* ステアリングを中央からa[度]だけ動かす /* ステアリングを中央からa[度]だけ動かす
<- - 0 + -> <- - 0 + ->
| | | |
| | | |
[]---{}---[] []---{}---[]
/ .\ / .\
*/ */
void steering(int a){ void steering(int a){
steeringServo.write(STEERING_DEFAULT - a); steeringServo.write(STEERING_DEFAULT - a);
} }
/* ランスを中央からa[度]だけ動かす /* ランスを中央からa[度]だけ動かす
<- - 0 + -> <- - 0 + ->
/ .\ / .\
/ | \ / | \
[]+--{}--+[] []+--{}--+[]
*/ */
void lance(int a){ void lance(int a){
lanceServo.write(LANCE_DEFAULT - a); lanceServo.write(LANCE_DEFAULT - a);
} }
@ -204,7 +197,7 @@ byte sensorRead(){
} }
void modeSet(){ void modeSet(){
if(distance > LAP_DISTANCE*5){ //5周したら停止 if(distance > 90000){
mode = MODE_STOP; mode = MODE_STOP;
return; return;
} }
@ -212,85 +205,24 @@ void modeSet(){
default: default:
case 0b00000000: case 0b00000000:
mode = MODE_STRAIGHT; mode = MODE_STRAIGHT;
target = TARGET_NONE; break;
case 0b10000001: case 0b10000001:
modeChangedDistance = distance; modeChangedDistance = distance;
mode = MODE_TURN; mode = MODE_TURN;
target = TARGET_NONE;
break; break;
case 0b00000001: case 0b00000001:
modeChangedDistance = distance; modeChangedDistance = distance;
mode = MODE_ATTACK; if(distance%LAP_DISTANCE < 1429){ //まずは、E的をねらいたい
if(distance%LAP_DISTANCE < LAP_DISTANCE/7){ //まずは、E的をねらいたい mode = MODE_ATTACK_PARALLEL_1;
target = TARGET_PARALLEL_1; }else if(distance%LAP_DISTANCE < 3333){ //次に、F的をねらいたい
}else if(distance%LAP_DISTANCE < LAP_DISTANCE/3){ //次に、F的をねらいたい mode = MODE_ATTACK_PARALLEL_2;
target = TARGET_PARALLEL_2; }else if(distance%LAP_DISTANCE){ //後半は垂直標的をねらいたい
}else{ //後半は垂直標的をねらいたい mode = MODE_ATTACK_VERTICAL_R;
target = TARGET_VERTICAL_R;
} }
break; break;
case 0b10000000: case 0b10000000:
modeChangedDistance = distance; modeChangedDistance = distance;
mode = MODE_ATTACK; if(distance%LAP_DISTANCE > 5000) mode = MODE_ATTACK_VERTICAL_L;
target = TARGET_VERTICAL_L;
break;
}
}
void stop(){
motorL.mode(STOP);
motorR.mode(STOP);
}
void straight(){
trace();
modeSet();
}
void turn(){
trace();
lance(0);
if(distance - modeChangedDistance > TURN_DISTANCE){
modeSet();
}
}
void attack(){
switch(target){
default:
case TARGET_NONE:
trace();
modeSet();
break;
case TARGET_PARALLEL_1: //平行標的1
trace();
lance(LANCE_ANGLE_PARALLEL_1);
if(distance - modeChangedDistance > 24){ //マーカーから少し進んで、叩く
lance(LANCE_ANGLE_PARALLEL_1+8);
modeSet();
}
break;
case TARGET_PARALLEL_2: //平行標的2
trace();
lance(LANCE_ANGLE_PARALLEL_2);
if(distance - modeChangedDistance > 26){ //マーカーから少し進んで、叩く
lance(LANCE_ANGLE_PARALLEL_2+8);
modeSet();
}
break;
case TARGET_VERTICAL_R: //右垂直標
trace();
lance(LANCE_ANGLE_VERTICAL_R);
modeSet();
break;
case TARGET_VERTICAL_L: //左垂直標
trace();
lance(LANCE_ANGLE_VERTICAL_L);
modeSet();
break;
case TARGET_CYLINDER: //円筒標的
trace();
modeSet();
break; break;
} }
} }
@ -299,9 +231,17 @@ void rot(){
distance++; distance++;
} }
void startBeep(){
tone(PIN_BUZZER,2000);
delay(100);
tone(PIN_BUZZER,1000);
delay(100);
noTone(PIN_BUZZER);
}
void setup(){ void setup(){
//サーボ //サーボ
steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERGING_MAX); steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERING_MAX);
lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX); lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
steering(0); steering(0);
lance(0); lance(0);
@ -314,12 +254,16 @@ void setup(){
sensorInit(); sensorInit();
//エンコーダー //エンコーダー
pinMode(PIN_ROT, INPUT); pinMode(PIN_ROT, INPUT);
attachInterrupt(0, rot, FALLING); attachInterrupt(0, rot, RISING);
//ブザー
pinMode(PIN_BUZZER, OUTPUT);
startBeep();
/* 3秒停止 */ /* 3秒停止 */
stop(); motorL.mode(STOP);
motorR.mode(STOP);
delay(3000); delay(3000);
/* スタート */ /* スタート */
modeSet(); mode = MODE_STRAIGHT;
} }
void loop(){ void loop(){
@ -327,16 +271,61 @@ void loop(){
switch(mode){ switch(mode){
default: default:
case MODE_STOP: //停止 case MODE_STOP: //停止
stop(); motorL.mode(STOP);
motorR.mode(STOP);
tone(PIN_BUZZER, 2000);
delay(2000);
noTone(PIN_BUZZER);
while(1){
;
};
break; break;
case MODE_STRAIGHT: //まっすぐ進む case MODE_STRAIGHT: //まっすぐ進む
straight(); trace();
modeSet();
break; break;
case MODE_TURN: //左に曲がる case MODE_TURN: //左に曲がる
turn(); trace();
lance(0);
if(distance - modeChangedDistance > TURN_DISTANCE){
mode = MODE_STRAIGHT;
}
break; break;
case MODE_ATTACK: //標的を狙う case MODE_ATTACK_PARALLEL_1: //平行標的1
attack(); 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 > 300){ //マーカーから少し進んで、まっすぐに戻す
lance(0);
mode = MODE_STRAIGHT;
}
break;
case MODE_ATTACK_CYLINDER: //円筒標的
trace();
mode = MODE_STRAIGHT;
break; break;
} }
} }

95
pitches.h Normal file
View file

@ -0,0 +1,95 @@
/*************************************************
* Public Constants
*************************************************/
#define NOTE_B0 31
#define NOTE_C1 33
#define NOTE_CS1 35
#define NOTE_D1 37
#define NOTE_DS1 39
#define NOTE_E1 41
#define NOTE_F1 44
#define NOTE_FS1 46
#define NOTE_G1 49
#define NOTE_GS1 52
#define NOTE_A1 55
#define NOTE_AS1 58
#define NOTE_B1 62
#define NOTE_C2 65
#define NOTE_CS2 69
#define NOTE_D2 73
#define NOTE_DS2 78
#define NOTE_E2 82
#define NOTE_F2 87
#define NOTE_FS2 93
#define NOTE_G2 98
#define NOTE_GS2 104
#define NOTE_A2 110
#define NOTE_AS2 117
#define NOTE_B2 123
#define NOTE_C3 131
#define NOTE_CS3 139
#define NOTE_D3 147
#define NOTE_DS3 156
#define NOTE_E3 165
#define NOTE_F3 175
#define NOTE_FS3 185
#define NOTE_G3 196
#define NOTE_GS3 208
#define NOTE_A3 220
#define NOTE_AS3 233
#define NOTE_B3 247
#define NOTE_C4 262
#define NOTE_CS4 277
#define NOTE_D4 294
#define NOTE_DS4 311
#define NOTE_E4 330
#define NOTE_F4 349
#define NOTE_FS4 370
#define NOTE_G4 392
#define NOTE_GS4 415
#define NOTE_A4 440
#define NOTE_AS4 466
#define NOTE_B4 494
#define NOTE_C5 523
#define NOTE_CS5 554
#define NOTE_D5 587
#define NOTE_DS5 622
#define NOTE_E5 659
#define NOTE_F5 698
#define NOTE_FS5 740
#define NOTE_G5 784
#define NOTE_GS5 831
#define NOTE_A5 880
#define NOTE_AS5 932
#define NOTE_B5 988
#define NOTE_C6 1047
#define NOTE_CS6 1109
#define NOTE_D6 1175
#define NOTE_DS6 1245
#define NOTE_E6 1319
#define NOTE_F6 1397
#define NOTE_FS6 1480
#define NOTE_G6 1568
#define NOTE_GS6 1661
#define NOTE_A6 1760
#define NOTE_AS6 1865
#define NOTE_B6 1976
#define NOTE_C7 2093
#define NOTE_CS7 2217
#define NOTE_D7 2349
#define NOTE_DS7 2489
#define NOTE_E7 2637
#define NOTE_F7 2794
#define NOTE_FS7 2960
#define NOTE_G7 3136
#define NOTE_GS7 3322
#define NOTE_A7 3520
#define NOTE_AS7 3729
#define NOTE_B7 3951
#define NOTE_C8 4186
#define NOTE_CS8 4435
#define NOTE_D8 4699
#define NOTE_DS8 4978