diff --git a/arduino/hanzo2_1/hanzo2_1.ino b/arduino/hanzo2_1/hanzo2_1.ino index e3c6fc7..4bbed9c 100644 --- a/arduino/hanzo2_1/hanzo2_1.ino +++ b/arduino/hanzo2_1/hanzo2_1.ino @@ -19,11 +19,11 @@ #include #include "Motor.h" -/**************************************/ //#define LINE_BLACK //白地・黒ライン有効 #define PIN_SENSOR ( ((PINC<<2) & ~0x03) | ((PINB>>4) & 0x03) ) const unsigned int TARGET_NONE_COUNT = 85; +const unsigned long LAP_COUNT = 854; const int LANCE_ANGLE0 = 0; //度 const int LANCE_ANGLE1 = 43; @@ -34,11 +34,11 @@ const unsigned int LANCE_INTERVAL = 70; //ms const byte SPEED_DEFAULT = 0xff; -const int HANDLE_DEFAULT = 90; //度 +const int HANDLE_DEGREE_DEFAULT = 90; //度 const unsigned int HANDLE_MIN = ( 800+30); //ms const unsigned int HANDLE_MAX = (2300+30); //ms -const int LANCE_DEFAULT = 90; //度 +const int LANCE_DEGREE_DEFAULT = 90; //度 const unsigned int LANCE_MIN = 544; //ms const unsigned int LANCE_MAX = 2400; //ms @@ -58,19 +58,19 @@ const byte PIN_SERVO_HANDLE = 10; const byte PIN_SERVO_LANCE = 9; const byte PIN_MOTOR_LEFT_1 = 5; -const byte PIN_MOTOR_LEFT_2 = 4; +const byte PIN_MOTOR_LEFT_2 = 7; const byte PIN_MOTOR_RIGHT_1 = 6; -const byte PIN_MOTOR_RIGHT_2 = 7; +const byte PIN_MOTOR_RIGHT_2 = 8; -const byte MODE_STOP = 0; -const byte MODE_TRACE = 10; -const byte MODE_RIGHT = 21; // 右カーブ -const byte MODE_LEFT = 22; // 左カーブ +const byte MODE_STOP = 0x10; +const byte MODE_TRACE = 0x11; +const byte MODE_LEFT = 0x12; //左カーブ +const byte MODE_RIGHT = 0x13; //右カーブ -const byte TARGET_NONE = 40; -const byte TARGET_PARALLEL = 41; -const byte TARGET_VERTICAL = 42; -const byte TARGET_CYLINDER = 43; +const byte TARGET_NONE = 0x40; +const byte TARGET_PARALLEL = 0x41; //平行標的 +const byte TARGET_VERTICAL = 0x42; //垂直標的 +const byte TARGET_CYLINDER = 0x43; //円筒標的 const byte MASK_MODE_TRACE = 0b01111110; const byte MASK_CHECK_MARKER = 0b10000001; @@ -87,7 +87,6 @@ Motor motorL; byte mode; byte target; unsigned int counterOld; -char handleAngle; char lanceAngle; /**************************************/ @@ -95,78 +94,76 @@ char lanceAngle; void trace(byte sensor){ switch( sensor & MASK_MODE_TRACE ){ case 0b01000000: - handleAngle = -20; + handle(-20); motorMode(GO, GO, 0x40, 0xff); lanceAngle = 0; break; case 0b01100000: - handleAngle = -16; + handle(-16); motorMode(GO, GO, 0xff, 0xff); lanceAngle = 0; break; case 0b00100000: - handleAngle = -12; + handle(-12); motorMode(GO, GO, 0xff, 0xff); lanceAngle = 0; break; case 0b00110000: - handleAngle = -5; + handle(-5); motorMode(GO, GO, 0xff, 0xff); break; case 0b00010000: - handleAngle = -2; + handle(-2); motorMode(GO, GO, 0xff, 0xff); break; case 0b00011000: - handleAngle = 0; + handle(0); motorMode(GO, GO, 0xff, 0xff); break; case 0b00001000: - handleAngle = 2; + handle(2); motorMode(GO, GO, 0xff, 0xff); break; case 0b00001100: - handleAngle = 5; + handle(5); motorMode(GO, GO, 0xff, 0xff); break; case 0b00000100: - handleAngle = 7; + handle(7); motorMode(GO, GO, 0xff, 0xff); break; case 0b00000110: - handleAngle = 10; + handle(10); motorMode(GO, GO, 0xff, 0xff); break; case 0b00000010: - handleAngle = 12; + handle(12); motorMode(GO, GO, 0xff, 0xff); break; } } -/**************************************/ - void motorMode(byte motorLeftMode, byte motorRightMode, byte motorLeftSpeed, byte motorRightSpeed){ motorL.mode( motorLeftMode, motorLeftSpeed); motorR.mode(motorRightMode, motorRightSpeed); } -void handle(char handle_angle){ - servoHandle.write(HANDLE_DEFAULT + handle_angle); +void handle(int degree){ + servoHandle.write(HANDLE_DEGREE_DEFAULT + degree); } -void lance(char lance_angle){ - servoLance.write(LANCE_DEFAULT - lance_angle); +void lance(int degree){ + servoLance.write(LANCE_DEGREE_DEFAULT - degree); } /* ロータリーエンコーダーの変化を見る */ volatile unsigned long counter = 0; void count(){ - static byte rot[2] = {0}; - rot[1] = digitalRead(PIN_ROT); - if(rot[1] != rot[0]){ + static byte rot[2] = {0}; //rot[0]:今の状態, rot[1]:前回の状態 + rot[0] = digitalRead(PIN_ROT); + if(rot[0] != rot[1]){ + rot[1] = rot[0]; counter++; - rot[0] = rot[1]; } } @@ -226,23 +223,23 @@ void loop(){ if(counter - counterOld > TARGET_NONE_COUNT ) target = 0; break; default: - switch( sensor & MASK_CHECK_MARKER ){ - case 0x81: - target = TARGET_NONE; - counterOld = counter; - break; - case 0x01: - lanceAngle = LANCE_ANGLE1; - target = TARGET_PARALLEL; - break; - case 0x80: - lanceAngle = LANCE_ANGLE3; - target = TARGET_VERTICAL; - break; - } + switch( sensor & MASK_CHECK_MARKER ){ + case 0x81: + target = TARGET_NONE; + counterOld = counter; + break; + case 0x01: + lanceAngle = LANCE_ANGLE1; + target = TARGET_PARALLEL; + break; + case 0x80: + lanceAngle = LANCE_ANGLE3; + target = TARGET_VERTICAL; break; } - if( lanceAngle == LANCE_ANGLE1 && counter%840 > 420) lanceAngle = LANCE_ANGLE4; + break; + } + if( lanceAngle == LANCE_ANGLE1 && counter%LAP_COUNT > LAP_COUNT/2) lanceAngle = LANCE_ANGLE4; static unsigned long millis_lance = 0; if(millis() - millis_lance > LANCE_INTERVAL){ millis_lance = millis(); @@ -255,25 +252,50 @@ void loop(){ break; } } + //ランスの動作を決定する + switch(target){ + case TARGET_NONE: + lanceAngle = LANCE_ANGLE0; + if(counter - counterOld > TARGET_NONE_COUNT ) target = 0; + break; + case TARGET_PARALLEL: + case TARGET_VERTICAL: + case TARGET_CYLINDER: + default: + switch( sensor & MASK_CHECK_MARKER ){ + case 0x81: + target = TARGET_NONE; + counterOld = counter; + break; + case 0x01: + lanceAngle = LANCE_ANGLE1; + target = TARGET_PARALLEL; + break; + case 0x80: + lanceAngle = LANCE_ANGLE3; + target = TARGET_VERTICAL; + break; + } + break; + } //全体の動作を決定する switch(mode){ case MODE_TRACE: trace(sensor); - if( counter > 854*6 - 60 ){ // 6周(1周854カウント)したら、円筒標的を狙いに行く + if( counter > LAP_COUNT*6 - LAP_COUNT/14 ){ // 6周したら、円筒標的を狙いに行く mode = MODE_STOP; } break; case MODE_STOP: default: digitalWrite(PIN_LED, HIGH); - handleAngle = 0; + handle(0); lanceAngle = -90; motorMode(GO, GO, 0xff, 0xff); - if( counter - counterOld > 40 ){ + if( counter - counterOld > LAP_COUNT/21 ){ motorMode(STOP, STOP, 0xff, 0xff); } } - handle(handleAngle); lance(lanceAngle); }