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