diff --git a/arduino/hanzo2_1/hanzo2_1.ino b/arduino/hanzo2_1/hanzo2_1.ino index 62df437..f8b571b 100644 --- a/arduino/hanzo2_1/hanzo2_1.ino +++ b/arduino/hanzo2_1/hanzo2_1.ino @@ -33,6 +33,7 @@ const byte SPEED_DEFAULT = 255; const int HANDLE_DEGREE_DEFAULT = 90; //度 const unsigned int HANDLE_MIN = ( 800+30); //us const unsigned int HANDLE_MAX = (2300+30); //us +const float HANDLE_KP = 4.0; const int LANCE_DEGREE_DEFAULT = 90; //度 const unsigned int LANCE_MIN = ( 544-62); //us @@ -90,23 +91,23 @@ unsigned long counterOld; void trace(byte sensor){ switch(sensor&MASK_MODE_TRACE){ case 0b01000000: - handle(-20); + handle(-5*HANDLE_KP); motorMode(GO, GO, 0, SPEED_DEFAULT); break; case 0b01100000: - handle(-16); + handle(-4*HANDLE_KP); motorMode(GO, GO, SPEED_DEFAULT/4, SPEED_DEFAULT); break; case 0b00100000: - handle(-12); + handle(-3*HANDLE_KP); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); break; case 0b00110000: - handle(-5); + handle(-2*HANDLE_KP); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); break; case 0b00010000: - handle(-2); + handle(-1*HANDLE_KP); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); break; case 0b00011000: @@ -114,23 +115,23 @@ void trace(byte sensor){ motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); break; case 0b00001000: - handle(2); + handle(1*HANDLE_KP); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); break; case 0b00001100: - handle(5); + handle(2*HANDLE_KP); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); break; case 0b00000100: - handle(7); + handle(3*HANDLE_KP); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); break; case 0b00000110: - handle(10); + handle(4*HANDLE_KP); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); break; case 0b00000010: - handle(12); + handle(5*HANDLE_KP); motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); break; }