diff --git a/hanzo3.ino b/hanzo3.ino index 2154561..478c201 100644 --- a/hanzo3.ino +++ b/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; } -} +} diff --git a/pitches.h b/pitches.h new file mode 100644 index 0000000..55c7d54 --- /dev/null +++ b/pitches.h @@ -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 + +