diff --git a/hanzo4.ino b/hanzo4.ino index 91444ec..8a4fb93 100644 --- a/hanzo4.ino +++ b/hanzo4.ino @@ -8,14 +8,17 @@ #include #include "Motor.h" -const unsigned long LAP_DISTANCE = 28500; //ロータリーエンコーダーのカウント数/周 -const unsigned long TURN_DISTANCE = 2000; //カーブ中でのカウント数 +const unsigned long LAP_DISTANCE = 28510; //ロータリーエンコーダーのカウント数/周 +const unsigned long TURN_DISTANCE = 4700; //カーブ中でのカウント数 -const int LANCE_ANGLE_PARALLEL_1 = 55; //度 -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 int LANCE_ANGLE_E = 45; //度 +const int LANCE_ANGLE_F = 62; //度 +const int LANCE_ANGLE_A = 29; //度 +const int LANCE_ANGLE_B = -30; //度 +const int LANCE_ANGLE_C = 32; //度 +const int LANCE_ANGLE_D = -40; //度 +//const unsigned char SPEED_DEFAULT = 0; const unsigned char SPEED_DEFAULT = 127; //const float STEERING_KP = 3.8; @@ -26,7 +29,7 @@ const unsigned int STEERING_MIN = (STEERING_CENTER-780); //us const unsigned int STEERING_MAX = (STEERING_CENTER+780); //us //Servo : SC-0352 -const unsigned int LANCE_CENTER = 1450; //us +const unsigned int LANCE_CENTER = 1530; //us const unsigned int LANCE_MIN = (LANCE_CENTER-780); //us const unsigned int LANCE_MAX = (LANCE_CENTER+780); //us @@ -75,10 +78,12 @@ enum mode_t { MODE_STOP, //停止 MODE_STRAIGHT, //まっすぐ進む MODE_TURN, //左に曲がる - MODE_ATTACK_PARALLEL_1, //平行標的1 - MODE_ATTACK_PARALLEL_2, //平行標的2 - MODE_ATTACK_VERTICAL_R, //右垂直標的 - MODE_ATTACK_VERTICAL_L, //左垂直標的 + MODE_ATTACK_E, //平行標的1 + MODE_ATTACK_F, //平行標的2 + MODE_ATTACK_A, //右垂直標的 + MODE_ATTACK_B, //左垂直標的 + MODE_ATTACK_C, //右垂直標的 + MODE_ATTACK_D, //左垂直標的 MODE_ATTACK_CYLINDER //円筒標的 } mode; @@ -89,7 +94,7 @@ mode; void trace(){ switch(sensor&MASK_LINE){ case 0b01000000: - steering(-22); + steering(-21); motorL.speed(-speed/2); motorR.speed(speed/2); break; @@ -99,7 +104,7 @@ void trace(){ motorR.speed(speed/2); break; case 0b00100000: - steering(-18); + steering(-19); motorL.speed(-speed/2); motorR.speed(speed/2); break; @@ -150,7 +155,7 @@ void trace(){ void modeSet(){ if(sensor == 0) errorCount++; else errorCount = 0; - if(distance > LAP_DISTANCE*5 || errorCount > 10000){ + if(distance > LAP_DISTANCE*5 || errorCount > 5000){ mode = MODE_STOP; return; } @@ -165,17 +170,27 @@ void modeSet(){ break; case 0b00000001: modeChangedDistance = distance; - if(distance%LAP_DISTANCE < 1429){ //まずは、E的をねらいたい - mode = MODE_ATTACK_PARALLEL_1; - }else if(distance%LAP_DISTANCE < 3333){ //次に、F的をねらいたい - mode = MODE_ATTACK_PARALLEL_2; - }else{ //後半は垂直標的をねらいたい - mode = MODE_ATTACK_VERTICAL_R; + if(distance%LAP_DISTANCE < LAP_DISTANCE/8){ //まずは、E的をねらいたい + mode = MODE_ATTACK_E; + }else if(distance%LAP_DISTANCE < LAP_DISTANCE/4){ //次に、F的をねらいたい + mode = MODE_ATTACK_F; + }else if(distance%LAP_DISTANCE > LAP_DISTANCE/2){ //後半は垂直標的をねらいたい + if(distance%LAP_DISTANCE < LAP_DISTANCE*0.6){ + mode = MODE_ATTACK_A; + }else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.85){ + mode = MODE_ATTACK_C; + } } break; case 0b10000000: modeChangedDistance = distance; - if(distance%LAP_DISTANCE > 5000) mode = MODE_ATTACK_VERTICAL_L; + if(distance%LAP_DISTANCE < LAP_DISTANCE/2){ + ; + }else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.80){ + mode = MODE_ATTACK_B; + }else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.85){ + mode = MODE_ATTACK_D; + } break; } } @@ -233,34 +248,50 @@ void loop(){ mode = MODE_STRAIGHT; } break; - case MODE_ATTACK_PARALLEL_1: //平行標的1 + case MODE_ATTACK_E: //平行標的1 trace(); - lance(LANCE_ANGLE_PARALLEL_1); - if(distance - modeChangedDistance > 1200){ //マーカーから少し進んで、叩く - lance(LANCE_ANGLE_PARALLEL_1 + 10); + lance(LANCE_ANGLE_E); + if(distance - modeChangedDistance > 1000){ //マーカーから少し進んで、叩く + lance(LANCE_ANGLE_E + 5); mode = MODE_STRAIGHT; } break; - case MODE_ATTACK_PARALLEL_2: //平行標的2 + case MODE_ATTACK_F: //平行標的2 trace(); - lance(LANCE_ANGLE_PARALLEL_2); - if(distance - modeChangedDistance > 1200){ //マーカーから少し進んで、叩く - lance(LANCE_ANGLE_PARALLEL_2 + 10); + lance(LANCE_ANGLE_F); + if(distance - modeChangedDistance > 1000){ //マーカーから少し進んで、叩く + lance(LANCE_ANGLE_F + 5); mode = MODE_STRAIGHT; } break; - case MODE_ATTACK_VERTICAL_R: //右垂直標 + case MODE_ATTACK_A: //右垂直標 trace(); - lance(LANCE_ANGLE_VERTICAL_R); - if(distance - modeChangedDistance > 300){ //マーカーから少し進んで、まっすぐに戻す + lance(LANCE_ANGLE_A); + if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す lance(0); mode = MODE_STRAIGHT; } break; - case MODE_ATTACK_VERTICAL_L: //左垂直標 + case MODE_ATTACK_C: //右垂直標 trace(); - lance(LANCE_ANGLE_VERTICAL_L); - if(distance - modeChangedDistance > 300){ //マーカーから少し進んで、まっすぐに戻す + lance(LANCE_ANGLE_C); + if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す + lance(0); + mode = MODE_STRAIGHT; + } + break; + case MODE_ATTACK_B: //左垂直標 + trace(); + lance(LANCE_ANGLE_B); + if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す + lance(0); + mode = MODE_STRAIGHT; + } + break; + case MODE_ATTACK_D: //左垂直標 + trace(); + lance(LANCE_ANGLE_D); + if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す lance(0); mode = MODE_STRAIGHT; }