nebel/2014-git-work
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

P制御を実装してみる

Browse source
This commit is contained in:
Nebel 2012-10-02 01:31:00 +09:00
parent 7dc227a97a
commit 4f2d3a25c1
1 changed files with 11 additions and 10 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

21
arduino/hanzo2_1/hanzo2_1.ino
View file

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