2013b/Member/michaegon/Mission1
¤ò¥Æ¥ó¥×¥ì¡¼¥È¤Ë¤·¤ÆºîÀ®
[
¥È¥Ã¥×
] [
¿·µ¬
|
°ìÍ÷
|
¸¡º÷
|
ºÇ½ª¹¹¿·
|
¥Ø¥ë¥×
|
¥í¥°¥¤¥ó
]
³«»Ï¹Ô:
Ìܼ¡
#contents
*²ÝÂê1ÆâÍÆ [#vb71dca6]
>¼«ºî¤·¤¿¥³¡¼¥¹¤Ë±è¤Ã¤Æ°ì¼þ¤¹¤ë¤¹¤ë¥í¥Ü¥Ã¥È¤òºîÀ®¤»¤è¡£
>
>¤¿¤À¤·¡¢¼¡¤ÎÅÀ¤ËÃí°Õ¤¹¤ë¤³¤È
>
>°ì¿Í¤Ï»þ·×²ó¤êÍÑ¡¢¤â¤¦°ì¿Í¤ÏȾ»þ·×²ó¤¦¤êÍÑ¤Î¥×¥í¥°¥é¥à¤òºîÀ®¤¹¤ë¤³¤È¡Ê¥í¥Ü¥Ã¥ÈËÜÂΤÏƱ°ì¤Î¤â¤Î¤ò»È¤¦¡Ë¡£
>¼¡¤Î¥¹¥Æ¥Ã¥×¤Ç¥×¥í¥°¥é¥à¤òºîÀ®¤¹¤ë¤³¤È¡£
>¸òº¹ÅÀ¤ò¤¤Á¤ó¤Èǧ¼±¤·¡¢Àµ¤·¤¯È½ÃǤ·¤ÆÄ̲á¤Ç¤¤ë¤è¤¦¤Ë¤¹¤ë¡£
>³Î¼Â¤Ë°ì¼þ¤Ç¤¤ë¤è¤¦¤Ë¤¹¤ë¡£¥¹¥¿¡¼¥ÈÃÏÅÀ¤«¤é½Ðȯ¤·¤Æ¸µ¤Ë¤â¤É¤ë¤è¤¦¤Ë¤¹¤ë¡£
>¶õ¤´Ì¤ò¸òº¹ÅÀ¤«¤é10cm°Ê¾åÎ¥¤·¤¿Àþ¾å¤Ëµ¯¤¡¢¤½¤ì¤ò°ìö¤É¤±¤ÆºÆ¤Ó¸µ¤Î°ÌÃ֤ˤâ¤É¤¹¤è¤¦¤Ë²þÎɤ¹¤ë¡£
>1¸Ä¤Î¶õ¤´Ì¤Ç¤¦¤Þ¤¯¤¤¤Ã¤¿¤é¡¢2¸Ä¤Î¶õ¤´Ì¤Ç¤ä¤Ã¤Æ¤ß¤ë¡£
°Ê¾å[[2013b/Mission1]]¤è¤ê°úÍÑ
&aname(map);
¥³¡¼¥¹¿Þ
#ref(coi.jpg);
*¥á¥ó¥Ð¡¼ [#zab397a9]
[[dyuma>2013b/Member/dyuma/Mission1]]~
¥í¥Ü¥Ã¥ÈÀ½ºî¡Ê+¥í¥Ü¥Ã¥È´ÉÍý¡Ë¡¦¥×¥í¥°¥é¥ß¥ó¥°¤È¤É¤Á¤é¤â¤³¤Ê¤·¤Æ¤¤¤ëÃæ´Ö´ÉÍý¿¦¡£
[[pepo>2013b/Member/pepo/Mission1]]~
¥í¥Ü¥Ã¥È¤òÁȤޤ»¤¿¤éÈà¤Î±¦¤Ë½Ð¤ë¤â¤Î¤Ï¤¤¤Ê¤¤¡£¡Ê¤¿¤À¤·¾ðÊ󹩳زʡË
michaegon~
¤³¤Î¥Ú¡¼¥¸¤ÎÃø¼Ô~
¥×¥í¥°¥é¥à¤ÎÊý¤ò¤ä¤Ã¤Æ¤Þ¤¹¡£
*¥í¥Ü¥Ã¥È³µÍ× [#t8caacae]
º£²ó¤Î²ÝÂê¤Ë¤¢¤¿¤Ã¤Æ¥Á¡¼¥à¤Ç»È¤Ã¤¿¥í¥Ü¥Ã¥È¤ÎÁ°Ì̤ˤϡ¢¥é¥¤¥ó¥È¥ì¡¼¥¹ÍѤθ÷¥»¥ó¥µ¡¼¤È¡¢´ÌǧÃÎÍѤÎĶ²»ÇÈ¥»¥ó¥µ¡¼¤¬1¤Ä¤º¤Ä¤Ä¤¤¤Æ¤¤¤Þ¤¹¡£
**¥¢¡¼¥à [#caac7b24]
#ref(robot.jpg);
º£²ó»È¤Ã¤¿¥¢¡¼¥à¤Ï¡¢Êª¤ò¤Ä¤«¤à¤è¤ê¤Ï³Î¼ÂÀ¤¬¤¢¤ë¤À¤í¤¦¤È¤¤¤¦¤³¤È¤Ç¡¢Êª¤òÊú¤¨¹þ¤à·Á¤Ë¤·¤Þ¤·¤¿¡£~
ÉáÃʤΥ¢¡¼¥à¤Ï¾å¿Þ¤Î¤è¤¦¤Ë¤ª¤ê¤Æ¤¤¤ë¾õÂ֤ǤϤʤ¯¡¢¾å¤¬¤Ã¤¿¾õÂÖ¤ÇÂÔµ¡¤·¤Æ¤¤¤Þ¤¹¡£Êª¤òÊú¤¨¹þ¤à¤È¤¤Ë¤Ï¾å¿Þ¤Î¤è¤¦¤Ë¤ª¤ê¤Æ¤¤Þ¤¹¡£
**¸÷¥»¥ó¥µ¡¼ [#wd2ba5d2]
#ref(light.jpg);
¥é¥¤¥ó¥È¥ì¡¼¥¹ÍѤθ÷¥»¥ó¥µ¡¼¤ÏÀßÃÖ°ÌÃ֤˵¤¤òÇÛ¤ê¤Þ¤·¤¿¡£~
ÀâÌÀ½ñ¤Ë½ñ¤¤¤Æ¤¢¤ëÄ̤ê¤Ëºî¤Ã¤¿¥í¥Ü¥Ã¥È¤À¤È¡¢¸÷¥»¥ó¥µ¡¼¤Î°ÌÃÖ¤¬¥¿¥¤¥ä¤Î¼´¤«¤é±ó¤¤¤¿¤á¡¢µÞ¥«¡¼¥Ö¤òÁö¹Ô¤¹¤ë¤È¤¤Ê¤É¤Ë¡¢¸÷¥»¥ó¥µ¡¼¼«ÂΤϥ«¡¼¥Ö¾å¤Ë¤¢¤Ã¤Æ¤â¥¿¥¤¥ä¡ÊËÜÂΡˤ¬¥³¡¼¥¹¥¢¥¦¥È¤·¤Æ¤·¤Þ¤¦¤³¤È¤¬¤¢¤ê¤Þ¤·¤¿¡£~
º£²ó¤Ï¥¿¥¤¥ä¤Î¼´¤Ë¶ËÎϸ÷¥»¥ó¥µ¡¼¤ò¶á¤Å¤±¤Æ¡¢¥í¥Ü¥Ã¥È¤Î¹½Â¤¤¬¸¶°ø¤Ç¤Î¥³¡¼¥¹¥¢¥¦¥È¤¬µ¯¤³¤é¤Ê¤¤¤è¤¦¤Ëµ¤¤òÉÕ¤±¤Þ¤·¤¿¡£
°Ê²¼¤Ç¤Ï±¦¥¿¥¤¥ä¤ò¥â¡¼¥¿¡¼B¡¢º¸¥¿¥¤¥ä¤ò¥â¡¼¥¿¡¼C¡¢¥¢¡¼¥à¤ò¥â¡¼¥¿¡¼A¡¢¸÷¥»¥ó¥µ¡¼¤ò¥»¥ó¥µ¡¼1¡¢Ä¶²»ÇÈ¥»¥ó¥µ¡¼¤ò¥»¥ó¥µ¡¼2¤ËÀܳ¤·¤Æ¤¤¤ë¤â¤Î¤È¤·¤ÆÀâÌÀ¤·¤Æ¤¤¤¤Þ¤¹¡£
*¥×¥í¥°¥é¥à³µÍ× [#u38c8111]
¤³¤³¤Ç¤Ï¡¢¤³¤Î¥Ú¡¼¥¸¤ÎÃø¼Ômichaegon¤¬ºîÀ®¤·¤¿¥×¥í¥°¥é¥à¤Î³µÍפò·ÇºÜ¤·¤Þ¤¹¡£
¾¥á¥ó¥Ð¡¼¤¬ºîÀ®¤·¤¿¥×¥í¥°¥é¥à¤Ë¤Ä¤¤¤Æ¤Ï¾¥á¥ó¥Ð¡¼¤Î¥Ú¡¼¥¸¤ò¤´Í÷¤¯¤À¤µ¤¤¡£
**PID_4.1.nxc(¥é¥¤¥ó¥È¥ì¡¼¥¹ÍÑ) [#bea8657d]
#define KP 1.55 // ÈæÎ㥲¥¤¥ó
#define KI 0.00005// ÀÑʬ¥²¥¤¥ó
#define KD 0.85// Èùʬ¥²¥¤¥ó
#define DT 1
#define THRESHOLD 50 // ïçÃÍ
#define SPEED1 40 // ¹â®
#define SPEED2 25 // Äã®
#define GO(s,t) OnFwdSync(OUT_BC,s,t); Wait(DT);
#define JUDGE_T 2500 // ¥®¥¢¥Á¥§¥ó¥¸´ð½à»þ´Ö
float pid;
int result;
int dev[2]; // Êк¹
long time;
int flag;
float integral;
/*¡*/
inline int ope_amount(int sensor)
{
dev[0]=dev[1]; // ²áµî¤Î¥»¥ó¥µ¡¼ÃͤÈïçÃͤκ¹
dev[1]=sensor-THRESHOLD; // ¸½ºß¤Î¥»¥ó¥µ¡¼ÃͤÈïçÃͤκ¹
if (dev[1]==dev[0]) { // ¤â¤·¥»¥ó¥µ¡¼ÃͤÎÊѲ½¤¬¤Ê¤¤¾ì¹ç¤Ïintegral¤ò¥ê¥»¥Ã¥È¤¹¤ë
integral=0;
}
integral+=(dev[1]+dev[0])*DT/2.0; // ÀÑʬ¤ò·×»»¤¹¤ë
pid=KP*dev[1]+KI*integral+KD*(dev[1]-dev[0])/DT;
if (-100>pid) {
result=-100;
} else if (100<pid) {
result=100;
} else {
result=pid;
}
return result; // -100¤«¤é100¤Î´Ö¤Ç¥·¥ó¥¯¥íΨ¤òÊÖ¤¹
}
/*¢*/
task gearchange()
{
time=CurrentTick();
until (SENSOR_1<=THRESHOLD || CurrentTick()-time>=JUDGE_T);
if (CurrentTick()-time>=JUDGE_T) {
flag=1;
} else {
flag=2;
}
}
task main()
{
SetSensorLight(S1);
// Äã®ÈÇ
while (true) { // º¸¥¨¥Ã¥¸ÍÑ
if (flag==1) {
stop gearchange;
flag=0;
PlaySound(SOUND_UP);
break;
} else if (flag==2) {
stop gearchange;
flag=0;
}
if (SENSOR_1>THRESHOLD) {
start gearchange;
}
Low:
GO(SPEED2,ope_amount(SENSOR_1));
}
// ¹â®ÍÑ
while (true) { // º¸¥¨¥Ã¥¸
if (flag==1) {
stop gearchange;
flag=0;
PlaySound(SOUND_DOWN);
goto Low;
} else if (flag==2) {
stop gearchange;
flag=0;
}
if (SENSOR_1>THRESHOLD) {
start gearchange;
}
GO(SPEED1,ope_amount(SENSOR_1));
}
}
¢¨¤³¤Î¥×¥í¥°¥é¥à¤Ï»þ·×²ó¤ê¥é¥¤¥ó¥È¥ì¡¼¥¹ÍѤǤ¹¡£
***·Ð°Þ¡¦´ðËÜÊý¿Ë [#f7c7d47f]
PID_4.1.nxc¤Ï¥á¥ó¥Ð¡¼¤Î[[dyuma>#zab397a9]]¤Î¡¢ÀßÄꤷ¤¿ïçÃͤȸ÷¥»¥ó¥µ¡¼¤Î¼Â¬Ãͤκ¹¤Ë¤è¤Ã¤Æ¥í¥Ü¥Ã¥È¤Î¥«¡¼¥ÖÎ̤ò·èÄꤷ¤Æ¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò¤¹¤ë¤È¤¤¤¦¹Í¤¨¤¬¤â¤È¤Ë¤Ê¤Ã¤Æ¤¤¤Þ¤¹¡£~
/*
ïçÃͤȸ÷¥»¥ó¥µ¡¼¤ÎÃͤκ¹¤Ë¤è¤Ã¤Æ¥«¡¼¥ÖÎ̤ò·è¤á¤ë¥×¥í¥°¥é¥à
»þ·×²ó¤ê¥é¥¤¥ó¥È¥ì¡¼¥¹ÍÑ
*/
#define SPEED 30 // Á°¿Ê¥¹¥Ô¡¼¥É
#define THRESHOLD 50 // ïçÃÍ
#define turn_right(s) OnFwd(OUT_C,SPEED); OnRev(OUT_B,s); // ±¦¤Ë¶Ê¤¬¤ë
#define turn_left(s) OnFwd(OUT_B,SPEED); OnRev(OUT_C,s); // º¸¤Ë¶Ê¤¬¤ë
task main()
{
SetSensorLight(S1);
int back; // µÕ²óž¤¹¤ëÊý¤Î¥¿¥¤¥ä¤Î¥¹¥Ô¡¼¥É
while (true) {
while (SENSOR_1<THRESHOLD) { // ¥í¥Ü¥Ã¥È¤¬¥é¥¤¥ó¾å¤Ë¤¤¤ë¤È¤
back=THRESHOLD-SENSOR_1; // ïçÃͤȥ»¥ó¥µ¡¼¼Â¬Ãͤκ¹¤ò¥¹¥Ô¡¼¥É¤Ë¤¹¤ë
turn_right(back); // ¥é¥¤¥ó¾å¤Ë¤¤¤ë¤È¤¤Ï±¦¤Ë¶Ê¤¬¤ë
Wait(1);
}
while (SENSOR_1>=THRESHOLD) { // ¥í¥Ü¥Ã¥È¤¬¥é¥¤¥ó³°¤Ë¤¤¤ë¤È¤
back=SENSOR_1-THRESHOLD;
turn_left(back); // ¥é¥¤¥ó³°¤Ë¤¤¤ë¤È¤¤Ïº¸¤Ë¶Ê¤¬¤ë
Wait(1);
}
}
}
¤³¤Î¥×¥í¥°¥é¥à¤Ë¤Ï¡¢¤Þ¤Ã¤¹¤°¿Ê¤á¤ºÁö¹Ô¤¬¥¸¥°¥¶¥°¤Ë¤Ê¤ê³ê¤é¤«¤Ç¤Ê¤¤¡¢Á°¿Ê¥¹¥Ô¡¼¥É¤¬ÃÙ¤¤¤Ê¤É¤Î·çÅÀ¤¬¤¢¤ê¤Þ¤·¤¿¡£~
²¿¤«²ò·èºö¤Ï¤Ê¤¤¤«Ä´¤Ù¤Æ¤¤¤¿¤È¤³¤í¡¢¸«¤Ä¤±¤¿¤Î¤¬"PIDÀ©¸æ"¤Ç¤¹¡£~
>PIDÀ©¸æ(¥Ô¡¼¥¢¥¤¥Ç¥£¡¼¤»¤¤¤®¤ç = Proportional Integral Derivative Controller)¤Ï¡¢¥Õ¥£¡¼¥É¥Ð¥Ã¥¯À©¸æ¤Î°ì¼ï¤Ç¤¢¤ê¡¢ÆþÎÏÃͤÎÀ©¸æ¤ò½ÐÎÏÃͤÈÌÜɸÃͤȤÎÊк¹¡¢¤½¤ÎÀÑʬ¡¢¤ª¤è¤ÓÈùʬ¤Î3¤Ä¤ÎÍ×ÁǤˤè¤Ã¤Æ¹Ô¤¦ÊýË¡¤Î¤³¤È¤Ç¤¢¤ë¡£
¡ÖPIDÀ©¸æ¡×¡Ø¥Õ¥ê¡¼É´²Ê»öŵ¡¡¥¦¥£¥¥Ú¥Ç¥£¥¢ÆüËܸìÈÇ¡Ù(http://ja.wikipedia.org/wiki/PID%E5%88%B6%E5%BE%A1 )2013ǯ6·î18Æü (²Ð) 17:14 UTC ¤è¤ê°úÍÑ~
º£²ó¤Ï¡¢ÁàºîÎ̤òope¡¢ÊÑ¿ôx,y,z¡¢Äê¿ôKp¡ÊÈæÎ㥲¥¤¥ó¤È¤¤¤¦¡Ë¡¢Ki¡ÊÀÑʬ¥²¥¤¥ó¡Ë¡¢Kd¡ÊÈùʬ¥²¥¤¥ó¡Ë¤òÍѤ¤¤Æ°Ê²¼¤Î¼°¤ò¤¿¤Æ¤ÆÍøÍѤ·¤Þ¤¹¡£&aname(pid);~
#ref(tex1.jpg)
PID_4.1.nxc¤Ç¤Ï¡¢OnFwdSync¤ËÍѤ¤¤ë¥â¡¼¥¿¡¼¤ÎƱ´ü¡Ê¥·¥ó¥¯¥í¡ËΨ¤Ëope¤òÍѤ¤¤Þ¤¹¡£
&ruby(Proportional){ÈæÎã};(P)À©¸æ¤Ç¤Ï¡¢ÈæÎ㥲¥¤¥óKp¤È¡¢ïçÃͤȸ÷¥»¥ó¥µ¡¼¤Î¼Â¬Ãͤκ¹¡ÊÊк¹¡Ëx¤òÍѤ¤¤ÆÁàºîÎ̤ò·èÄꤷ¤Þ¤¹¡£~
¥í¥Ü¥Ã¥È¤Î¸½ºß°ÌÃÖ¤¬ïçÃ͡ʺ£²ó¤Ï¥é¥¤¥ó¤Î¶³¦¾å¡Ë¤«¤éÎ¥¤ì¤ì¤ÐÎ¥¤ì¤ë¤Û¤ÉÈæÎãŪ¤ËÁàºîÎ̤¬Â礤¯¤Ê¤ê¤Þ¤¹¡£
&ruby(Integral){ÀÑʬ};(I)À©¸æ¤Ç¤Ï¡¢ÀÑʬ¥²¥¤¥óKi¤È¡¢²áµî¤Î¸÷¥»¥ó¥µ¡¼¼Â¬ÃͤÎÎßÀÑy¤òÍѤ¤¤Æ¥«¡¼¥ÖÃæ¤Ê¤É¤Î¥í¥Ü¥Ã¥È¤Î°ÌÃÖ¤òÊäÀµ¤·¤Þ¤¹¡£~
ÈæÎã(P)À©¸æ¤À¤±¤Ç¤Ï¡¢¥í¥Ü¥Ã¥È¤¬ïçÃͶáÊÕ¤ò¿Ê¤ó¤Ç¤¤¤ë»þ¤ÏÁàºîÎ̤¬¾®¤µ¤¯¤Ê¤ê¡¢¥é¥¤¥ó¤¬¥«¡¼¥Ö¤òÉÁ¤¤¤Æ¤¤¤¿¤ê¤¹¤ë¤È¡¢¥«¡¼¥Ö¤ËÂбþ¤Ç¤¤º¤Ë¥é¥¤¥ó¡ÊïçÃ͡ˤ«¤é¼ã´³Î¥¤ì¤¿°ÌÃ֤ǰÂÄꤷ¤Æ¤·¤Þ¤¦¤³¤È¤¬¤¢¤ê¤Þ¤¹¡£¤½¤Î¤è¤¦¤Ê¤È¤¤Ë¡¢ÈæÎã(P)À©¸æ¤Ç¤ÏÂбþ¤Ç¤¤Ê¤¤¤è¤¦¤Ê¤ï¤º¤«¤ÊÊк¹¤ò¤âÃßÀѤ·¤Æ¤¤¤¡¢¤½¤ÎÎ̤˱þ¤¸¤ÆÁàºîÎ̤òÁý¤ä¤¹ÀÑʬ(I)À©¸æ¤ò»È¤Ã¤Æ¥í¥Ü¥Ã¥È¤ò¤è¤êïçÃͤ˶ᤤ°ÌÃÖ¤ËÊäÀµ¤·¤Þ¤¹¡£
&ruby(Derivative){ÀÑʬ};(D)À©¸æ¤Ç¤Ï¡¢Èùʬ¥²¥¤¥óKd¤È¡¢¸÷¥»¥ó¥µ¡¼¼Â¬ÃͤÎÊѲ½¡Ê·¹¤¡Ëz¤òÍѤ¤¤Æ¡¢Ì¤Íè¤Ë¥«¡¼¥Ö¤¬¤¢¤ë¤«¤Ê¤É¤òͽ¬¤·¤ÆÁàºîÎ̤ò·èÄꤷ¤Þ¤¹¡£¤³¤ì¤Ë¤è¤Ã¤Æ¥«¡¼¥Ö¤Î»Ï¤Þ¤ê¤Ç¥é¥¤¥ó¤«¤éÂ礤¯³°¤ì¤Æ¤·¤Þ¤¦¤È¤¤¤¦»öÂÖ¤òËɤ®¤Þ¤¹¡£
***KP,KI,KD¤Î·èÄêÊýË¡ [#h5ec5df1]
ÈæÎ㥲¥¤¥óKP,ÀÑʬ¥²¥¤¥óKI,Èùʬ¥²¥¤¥óKD¤ÎºÇŬ¤ÊÃͤò¸«¤Ä¤±¤ëÊýË¡¤Ë¤Ï¡¢Í̾¤Ê¤È¤³¤í¤Ç¤Ï¸Â³¦´¶ÅÙË¡¤Ê¤É¤¬¤¢¤ê¤Þ¤¹¡£~
º£²ó¤Ï¸Â³¦´¶ÅÙË¡¤ò»î¤·¤Æ¤ß¤Þ¤·¤¿¡£
¸Â³¦´¶ÅÙË¡¤Ç¤Ï¡¢¤Þ¤º¤Ï¤¸¤á¤ËKI=KD=0¤È¤·¤ÆPÀ©¸æ¤À¤±¤ÇÁö¹Ô¤µ¤»¤Æ¤ß¤Þ¤¹¡£~
¤³¤Î¤È¤KP¤ÎÃͤò½ù¡¹¤ËÁý¤ä¤·¤Æ¤¤¤Ã¤Æ¡¢¥í¥Ü¥Ã¥È¤¬»ý³Ū¤Ë¿¶Æ°¤¹¤ë¡Êsin¥«¡¼¥Ö¤Î¤è¤¦¤Êµ°Æ»¤òÉÁ¤¤Ê¤¬¤éÁö¹Ô¤·Â³¤±¤ë¡Ë¾õÂ֤ˤʤëKP¤òõ¤·¤Þ¤¹¡Ê¤³¤ì¤òKu¤È¤ª¤¯)¡£~
¤³¤Î¤È¤¤Î¥í¥Ü¥Ã¥È¤Î¿¶Æ°¼þ´ü¤òTu¤È¤ª¤¤¤Æ¡¢Ku,Tu¤«¤éPIDÀ©¸æ¤Ë¤·¤¿¤È¤¤ÎŬÀÚ¤ÊKP,KI,KD¤ò»»½Ð¤·¤Þ¤¹¡£
¤³¤Î¤è¤¦¤Ë¤¦¤Þ¤¯¸Â³¦´¶ÅÙË¡¤¬»È¤¨¤ì¤Ð¤è¤«¤Ã¤¿¤Î¤Ç¤¹¤¬¡¢¼ÂºÝ¤ä¤Ã¤Æ¤ß¤ë¤È¤¦¤Þ¤¯¤¤¤¯¤¤¤Þ¤»¤ó¤Ç¤·¤¿¡£~
¤½¤â¤½¤âKP¤ÎÃͤò¤¤¤¯¤éÂ礤¯¤·¤Æ¤â¿¶Æ°¤¬»ý³¤»¤º¤Ë¤¹¤°¤Ë¿¶Æ°¤¬¸º¿ê¤·¤Æ¤·¤Þ¤¤Ku,Tu¤¬¤¦¤Þ¤¯½Ð¤»¤Ê¤«¤Ã¤¿¤Î¤Ç¡¢º£²ó¤Ï¸Â³¦´¶ÅÙË¡»È¤¦¤Î¤òÃÇÇ°¤·¤Þ¤·¤¿¡£
¤È¤¤¤¦¤³¤È¤Çº£²ó¤Ï½é¤á¤«¤é¼êºî¶È¤ÇÃͤò·è¤á¤Þ¤·¤¿¡£~
PIDÀ©¸æ¤ÎÃæ¤Ç¥á¥¤¥ó¤Ë¤Ê¤ë¤Î¤ÏPÀ©¸æ¤Ç¡¢IDÀ©¸æ¤ÏPÀ©¸æ¤ÎÊä½õ¤Î¤è¤¦¤Ê¤â¤Î¤Ê¤Î¤Ç¡¢¤Þ¤º¤ÏPÀ©¸æ¤Î¤ß¤ÇÁö¹Ô¤µ¤»¤Þ¤¹¡£¤³¤Î»þ¡¢°ìÈÖ³ê¤é¤«¤Ë°ÂÄꤷ¤ÆÁö¹Ô¤·¤¿KP¤ò¸«¤Ä¤±¤Æ»ÈÍѤ·¤Þ¤·¤¿¡£~
¼¡¤Ë¡¢IÀ©¸æ¤ò²Ã¤¨¤Æ¥«¡¼¥Ö¤òÁö¹Ô¤µ¤»¤Þ¤¹¡£IÀ©¸æ¤Ç¤Ï¼ã´³¤Î°ÌÃÖÊäÀµ¤¬¤Ç¤¤ì¤Ð¤¤¤¤¤Î¤Ç¡¢KI¤ÎÃͤϡ¢¥í¥Ü¥Ã¥È¤¬PÀ©¸æ¤Î¤ß¤Î»þ¤è¤ê¤â¥«¡¼¥Ö¤ËµÛ¤¤ÉÕ¤¯¤è¤¦¤ËÁö¹Ô¤·¤Æ¡¢¤¢¤Þ¤êÂ礤¯¤Ê¤¤ÊÕ¤ê¤ËÀßÄꤷ¤Æ¤ª¤¤Þ¤¹¡£~
ºÇ¸å¤ÎDÀ©¸æ¤Ï¡¢µÞ·ã¤Ê¥³¡¼¥¹ÊѲ½¤ËÂбþ¤µ¤»¤ë¤¿¤á¤Î¤â¤Î¤Ç¡¢¤¢¤Þ¤êKD¤ÎÃͤòÂ礤¯¤·¤¹¤®¤ë¤È¡¢¥³¡¼¥¹¤Î¤ï¤º¤«¤ÊÊѲ½¤Ë¤â²á¾ê¤ËÈ¿±þ¤·¤Æ¡¢Áö¹ÔÃæ¤Ë¥í¥Ü¥Ã¥È¤¬ÌµÂ̤ʿ¶Æ°¤ò¤·¤ÆÉÔ°ÂÄê¤ÊÁö¹Ô¤Ë¤Ê¤Ã¤Æ¤·¤Þ¤¤¤Þ¤¹¡£¤è¤Ã¤Æ¡¢KD¤ÎÃͤâ¤Þ¤¢¤êÂ礤¹¤®¤Ê¤¤ÊÕ¤ê¤ËÀßÄꤷ¤Æ¤ª¤¤Þ¤¹¡£
***¡ inline´Ø¿ôope_amount [#k44c6886]
/*PID_4.1.nxc¤Î°ìÉô*/
#define KP 1.55 // ÈæÎ㥲¥¤¥ó
#define KI 0.00005// ÀÑʬ¥²¥¤¥ó
#define KD 0.85// Èùʬ¥²¥¤¥ó
#define DT 1
#define THRESHOLD 50 // ïçÃÍ
float pid;
int result;
int dev[2];
float integral;
inline int ope_amount(int sensor)
{
dev[0]=dev[1]; // ²áµî¤ÎÊк¹
dev[1]=sensor-THRESHOLD; // ¸½ºß¤ÎÊк¹
if (dev[1]==dev[0]) { // ¤â¤·Êк¹ÊѲ½¤¬¤Ê¤¤¾ì¹ç¤Ïintegral¤ò¥ê¥»¥Ã¥È¤¹¤ë
integral=0;
}
integral+=(dev[1]+dev[0])*DT/2.0; // ÀÑʬ¤ò·×»»¤¹¤ë
pid=KP*dev[1]+KI*integral+KD*(dev[1]-dev[0])/DT;
if (-100>pid) {
result=-100;
} else if (100<pid) {
result=100;
} else {
result=pid; // int·¿¤Ë¥¥ã¥¹¥È
}
return result;
}
KP¤ÏÈæÎ㥲¥¤¥ó¡¢KI¤ÏÀÑʬ¥²¥¤¥ó¡¢KP¤ÏÈùʬ¥²¥¤¥ó¡¢DT¤Ï¸÷¥»¥ó¥µ¡¼¤ÎÃͤòµÏ¿¤¹¤ë´Ö³Ö¡Êº£²ó¤Ï1¥ß¥êÉáˤǤ¹¡£~
dev[0]¤Ë¤Ï1¥ß¥êÉÃÁ°¤Î¸÷¥»¥ó¥µ¡¼¤ÎÃͤÈïçÃͤκ¹¡ÊÊк¹¡Ë¡¢dev[1]¤Ë¤Ï¸½ºß¤ÎÊк¹¤òÆþ¤ì¤Þ¤¹¡£~
¼¡¤ÏIÀ©¸æ¤Î¤¿¤á¤Î½àÈ÷¤Ç¤¹¡£integral¤Ë¤Ï¡¢1¥ß¥êÉ䴤ȤË(dev[1]+dev[0])*DT/2¡Ê²¼¿Þ¤Î²«¿§¤¤Éôʬ¡Ë¤ò¤·¤Æ¤¤¤¤Þ¤¹¡ÊÀÑʬ¡Ë¡£~
#ref(integral.jpg)
¤³¤Î¤È¤¡¢integral¤ËÊк¹¤¬ÃßÀѤ·Â³¤±¤ë¤È¡¢IÀ©¸æ¤Ç¤ÎÊäÀµ¤¬É¬Íפʤ¯¤Ê¤Ã¤Æ¤âÁàºîÎ̤¬ÌµÂ̤ËÁý¤¨¤¿¤Þ¤Þ¤Ë¤Ê¤Ã¤Æ¤·¤Þ¤¦¤Î¤Ç¡¢Êк¹ÊѲ½¤¬¤Ê¤¯¤Ê¤Ã¤¿¤È¤¡ÊľÀþ¾å¤Ê¤É¡Ë¤Ë¤Ïintegral¤ò¥ê¥»¥Ã¥È¤¹¤ëµ¡Ç½¤òÉÕ¤±¤Æ¤¢¤ê¤Þ¤¹¡£
ÉâÆ°¾®¿ôÅÀpid¤Ë¤Ï¡¢PIDÀ©¸æ¤Ë¤è¤Ã¤Æ·èÄꤷ¤¿ÁàºîÎ̤òÆþ¤ì¤Þ¤¹¡Ê[[¼°(1)>#pid]]¡Ë¡£~
¤³¤³¤Ç¡¢~
Kp*dev[1]¤ÏPÀ©¸æ¡¢~
Ki*integral¤ÏIÀ©¸æ¡¢~
¤Þ¤¿¡¢DT¤òÈù¾®»þ´Ö¤È¤ß¤Ê¤·¤Æ¸÷¥»¥ó¥µ¡¼ÃÍ¿ä°Ü¤Î¥°¥é¥Õ¤Î·¹¤¤ò(dev[1]-dev[0])/DT¤È¤·¤¿¤È¤¡ÊÈùʬ¡Ë¡¢~
Kd*(dev[1]-dev[0])/DT¤ÏDÀ©¸æ¤Ë¤è¤ëÁàºîÎ̤òɽ¤·¤Æ¤¤¤Þ¤¹¡£
#ref(d.jpg),left
¾å¤Çµá¤á¤¿pid¤òOnFwdSync¤Î¥·¥ó¥¯¥íΨ¡ÊÈϰϤÏ-100¤«¤é100¤Þ¤Ç¤ÎÀ°¿ô¡Ë¤È¤·¤Æ»È¤¤¤¿¤¤¤Î¤Ç¡¢¤³¤Î´Ø¿ô¤ÎºÇ¸å¤Îifʸ¤Ç¡¢int·¿¤Îresult¤Ëpid¤òÂåÆþ¤·¤ÆÀ°¿ôÃͤËÊÑ´¹¤·¤¿¤¢¤È¡¢-100¤«¤é100¤Þ¤Ç¤Ë¼ý¤Þ¤ë¤è¤¦¤ËÃͤòÄ´À°¤·¤Þ¤¹¡£
***¢ ¥á¥¤¥ó¥¿¥¹¥¯¡¦¥µ¥Ö¥¿¥¹¥¯gearchange [#f48902aa]
/*PID_4.1.nxc¤Î°ìÉô*/
#define THRESHOLD 50 // ïçÃÍ
#define SPEED1 40 // ¹â®
#define SPEED2 25 // Äã®
#define GO(s,t) OnFwdSync(OUT_BC,s,t); Wait(DT);
#define JUDGE_T 2500 // ¥®¥¢¥Á¥§¥ó¥¸´ð½à»þ´Ö
int flag;
task gearchange()
{
time=CurrentTick();
until (SENSOR_1<=THRESHOLD || CurrentTick()-time>=JUDGE_T);
if (CurrentTick()-time>=JUDGE_T) {
flag=1;
} else {
flag=2;
}
}
task main()
{
SetSensorLight(S1);
// Äã®Áö¹Ô¥ë¡¼¥×
while (true) { // º¸¥¨¥Ã¥¸ÍÑ
if (flag==1) {
stop gearchange;
flag=0;
PlaySound(SOUND_UP);
break;
} else if (flag==2) {
stop gearchange;
flag=0;
}
if (SENSOR_1>THRESHOLD) {
start gearchange;
}
Low:
GO(SPEED2,ope_amount(SENSOR_1));
}
// ¹â®Áö¹Ô¥ë¡¼¥×
while (true) { // º¸¥¨¥Ã¥¸
if (flag==1) {
stop gearchange;
flag=0;
PlaySound(SOUND_DOWN);
goto Low;
} else if (flag==2) {
stop gearchange;
flag=0;
}
if (SENSOR_1>THRESHOLD) {
start gearchange;
}
GO(SPEED1,ope_amount(SENSOR_1));
}
}
¤â¤È¤â¤È¤Î¥á¥¤¥ó¥¿¥¹¥¯¤Ï¡¢~
task main()
{
while (true) {
GO(SPEED1,ope_amount(SENSOR_1));
}
}
¤À¤±¤Ç¤·¤¿¡£¤³¤ì¤Ç¤â¥é¥¤¥ó¥È¥ì¡¼¥¹¤Ï¤Ç¤¤ë¤Î¤Ç¤¹¤¬¡¢¸òº¹ÅÀ¤äT»úÏ©¤òľ¿Ê¤·¤Æ¤·¤Þ¤¤¡¢¶Ê¤¬¤Ã¤Æ¤¯¤ì¤Þ¤»¤ó¤Ç¤·¤¿¡£¤³¤ì¤Ï¡¢¸òº¹ÅÀ¤äT»úÏ©¤Îľ³ÑÉôʬ¤ËÁàºîÎ̤ÎÊѲ½¤¬Äɤ¤¤Ä¤±¤º¡¢¤Þ¤ÀľÀþ¤À¤Èǧ¼±¤·¤Æ¤¤¤ë¤¦¤Á¤ËÄ̲ᤷ¤Æ¤·¤Þ¤¦¤³¤È¤¬¸¶°ø¤Î¤è¤¦¤Ç¤·¤¿¡£~
¸òº¹ÅÀ¤òľÀþ¤À¤Èǧ¼±¤·¤ÆÄ̲ᤷ¤Æ¤¯¤ì¤ë¤Î¤Ï¤¢¤ê¤¬¤¿¤¤¤Î¤Ç¤¹¤¬¡¢T»úÏ©¤âľ¿Ê¤·¤Æ¤·¤Þ¤¦¤È¥¹¥¿¡¼¥ÈÃÏÅÀ¤«¤é½Ð¤é¤ì¤Ê¤¤¤Î¤Ç¡¢T»úÏ©¤¬¸½¤ì¤ëÉôʬ¤À¤±Â®ÅÙ¤òÍî¤È¤·¤Æ¥«¡¼¥Ö¤È¤·¤Æǧ¼±¤¹¤ë¤è¤¦¤Ë²þÎɤ·¤¿¤Î¤¬º£²ó¤Î¥á¥¤¥ó¥¿¥¹¥¯¤Ç¤¹¡£
¤³¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤µ¤»¤ë¤È¡¢¤Þ¤º¤ÏÄ㮤ǥ饤¥ó¥È¥ì¡¼¥¹¤ò¤¹¤ë̵¸Â¥ë¡¼¥×¤ËÆþ¤ê¤Þ¤¹¡£~
¤³¤Î¥ë¡¼¥×¤Ë¤¤¤ë¸Â¤ê¡¢¥í¥Ü¥Ã¥È¤Ï¸òº¹ÅÀ¤âT»úÏ©¤â¥«¡¼¥Ö¤ÈȽÃǤ·¤Æ¶Ê¤¬¤Ã¤Æ¤¤¤¤Þ¤¹¡£~
¤è¤Ã¤Æ¡¢½é¤á¤ÎT»úÏ©¤Ï̵»ö¤Ë¶Ê¤¬¤ë¤³¤È¤¬¤Ç¤¤Þ¤¹¤¬¡¢¤³¤Î¤Þ¤Þ¤Ç¤Ï¤½¤Î¤¢¤È¤Î¸òº¹ÅÀ¤Ê¤É¤âÄ̲᤻¤º¤Ë¶Ê¤¬¤Ã¤Æ¤·¤Þ¤¤¤Þ¤¹¡£
¤µ¤Æ¡¢½é¤á¤ÎT»úÏ©¤ò¡¢º¸¤Ë¶Ê¤¬¤Ã¤¿¾ì¹ç¤Ï¼¡¤Ëľ³Ñ¥«¡¼¥Ö¤Î³°Â¦¤ò¡¢±¦¤Ë¶Ê¤¬¤Ã¤¿¾ì¹ç¤Ï¼¡¤Ë¥Ø¥¢¥Ô¥ó¥«¡¼¥Ö¤Î³°Â¦¤ò²ó¤ë¤³¤È¤Ë¤Ê¤ê¤Þ¤¹¡£¡Ê[[¥³¡¼¥¹¿Þ>#map]]¡Ë~
¤³¤Î¤È¤¡¢¤É¤¦¤·¤Æ¤â¥í¥Ü¥Ã¥È¤Ï¥é¥¤¥ó¤è¤ê¤â¼ã´³Âç²ó¤ê¤Ë¤Ê¤ë¤¿¤á¡¢£²¡Á£³Éäδ֤º¤Ã¤È¡¢¸÷¥»¥ó¥µ¡¼¤ÎÃͤ¬ïçÃͤè¤ê¤âÂ礤¯¤Ê¤ê¤Þ¤¹¡£~
¤³¤Î¸½¾Ý¤Ï¡¢ÈæÎ㥲¥¤¥ó¤Ê¤É¤òŬÀÚ¤ÊÃͤˤ·¤Æ¤ª¤±¤Ð¡¢º£²ó¤Î¥³¡¼¥¹¤Î¾¤Î¾ì½ê¤Ç¤Ïµ¯¤³¤ê¤Þ¤»¤ó¡£¤³¤Î¤³¤È¤òÍøÍѤ·¤Æ¡¢Â®ÅÙÊѹ¹¤ò¹Ô¤¤¤Þ¤¹¡£
Ä㮥饤¥ó¥È¥ì¡¼¥¹¤Î¥ë¡¼¥×Ãæ¤Ç¡¢¸÷¥»¥ó¥µ¡¼¤ÎÃͤ¬ïçÃͤè¤êÂ礤¯¤Ê¤ë¡Ê¥í¥Ü¥Ã¥È¤¬¥é¥¤¥ó³°¤Ë½Ð¤ë¡Ë¤È¡¢¥µ¥Ö¥¿¥¹¥¯gearchange¤¬ºîÆ°¤·¤Þ¤¹¡£~
¥µ¥Ö¥¿¥¹¥¯gearchange¤Ï¡¢°Ê²¼¤ÎÆ°ºî¤ò¤·¤Þ¤¹¡£
+¸÷¥»¥ó¥µ¡¼¤ÎÃͤ¬ïçÃͤè¤êÂ礤¯¤Ê¤Ã¤Æ¤¤¤ë¡Ê¥í¥Ü¥Ã¥È¤¬¥é¥¤¥ó³°¤Ë¤¤¤ë¡Ë»þ´Ö¤ò·×¬
+¤â¤·¸÷¥»¥ó¥µ¡¼¤ÎÃͤ¬ïçÃͤè¤êÂ礤«¤Ã¤¿»þ´Ö¤¬¡¢~
JUDGE_T°Ê¾å¤À¤Ã¤¿¾ì¹ç¤Ï¡¢flag¤Ë1¤òÂåÆþ~
JUDGE_T°Ê²¼¤À¤Ã¤¿¾ì¹ç¤Ïflag¤Ë2¤òÂåÆþ
¤¹¤ë¤È¡¢¥á¥¤¥ó¥¿¥¹¥¯Æâ¤Îifʸ¤¬¼Â¹Ô¤µ¤ì¤Þ¤¹¡£¤³¤Îifʸ¤Ç¤Ï¡¢
+¥µ¥Ö¥¿¥¹¥¯gearchange¤òÄä»ß¤µ¤»¤ë
+flag¤ò0¤ËÌ᤹
+gearchange¤Ç~
flag¤Ë1¤¬ÂåÆþ¤µ¤ì¤Æ¤¤¤¿¡Ê¥í¥Ü¥Ã¥È¤¬JUDGE_T°Ê¾å¥é¥¤¥ó³°¤Ë¤¤¤¿¡Ë¾ì¹ç¤ÏÄ㮥饤¥ó¥È¥ì¡¼¥¹¤Î¥ë¡¼¥×¤òÈ´¤±¤Æ¹â®¥é¥¤¥ó¥È¥ì¡¼¥¹¤Î¥ë¡¼¥×¤Ø°ÜÆ°~
flag¤Ë2¤¬ÂåÆþ¤µ¤ì¤Æ¤¤¤¿¡Ê¥í¥Ü¥Ã¥È¤¬JUDGE_T¤Ë㤹¤ëÁ°¤Ë¥é¥¤¥ó¾å¤ËÌá¤Ã¤¿¡Ë¾ì¹ç¤Ï¤³¤Î¤Þ¤ÞÄ㮥饤¥ó¥È¥ì¡¼¥¹¤Î¥ë¡¼¥×¤ò·Ñ³
¤³¤Îµ¡Ç½¤Ï¹â®¥é¥¤¥ó¥È¥ì¡¼¥¹¤Î¥ë¡¼¥×¤Ë¤âÅëºÜ¤µ¤ì¤Æ¤¤¤Þ¤¹¡£
¤³¤³¤Ç¡¢¥í¥Ü¥Ã¥È¤¬¥³¡¼¥¹¤ò»þ·×²ó¤ê¤Ë¥é¥¤¥ó¥È¥ì¡¼¥¹¤µ¤»¤Æ¤ß¤Þ¤·¤¿¡£¥í¥Ü¥Ã¥È¤Ï¡¢
+Äã®Áö¹Ô¤Ç»Ï¤Þ¤ê¡¢½é¤á¤ÎT»úÏ©¤òº¸¤Ë¶Ê¤¬¤ë
+¼¡¤Îľ³Ñ¥«¡¼¥Ö¤Î³°Â¦¤ò²ó¤ë~
¤³¤Î¤È¤¡¢JUDGE_T°Ê¾å¥é¥¤¥ó¤«¤é³°¤ì¤Æ¥í¥Ü¥Ã¥È¤¬²ó¤ë¤Î¤Ç¡¢¤³¤³¤ÇÄã®Áö¹Ô¥ë¡¼¥×¤«¤é¹â®Áö¹Ô¥ë¡¼¥×¤ËÀÚ¤êÂؤï¤ë
+¹â®Áö¹ÔÃæ¤Ç¤Ï¡¢¸òº¹ÅÀ¤äT»úÏ©¤òľÀþ¤ÈȽÃǤ·¤ÆÄ̲ᤷ¤Æ¤¤¤¯¤Î¤Ç¡¢¤³¤Î¸å¤Ë¤¢¤ë¸òº¹ÅÀ¤ÏÁ´¤ÆÄ̲á
+£²¤Ä¤á¤Î¸òº¹ÅÀ¤òÈ´¤±¤Æ¥Ø¥¢¥Ô¥ó¥«¡¼¥Ö¤Î³°Â¦¤ò²ó¤ëÉôʬ¤Ëº¹¤·³Ý¤«¤ë~
¥«¡¼¥Ö¤Î¶Ê¤¬¤ê¶ñ¹ç¤ËÄɤ¤¤Ä¤±¤ºJUDGE_T°Ê¾å¥í¥Ü¥Ã¥È¤¬¥é¥¤¥ó¤«¤é³°¤ì¤ë¤Î¤Ç¡¢¹â®Áö¹Ô¤«¤éÄã®Áö¹Ô¤ËÀÚ¤êÂؤï¤ë
+¤½¤Î¸åº¹¤·³Ý¤«¤ëT»úÏ©¤ÏÄã®Áö¹ÔÃæ¤Ê¤Î¤Ç¥«¡¼¥ÖȽÄꤷ¤Æ¶Ê¤¬¤ê¡¢Ìµ»ö¤Ë¥´¡¼¥ë¤ØÅþã
¤³¤Î¤è¤¦¤Ë¥³¡¼¥¹¤ò°ì¼þ¤Ç¤¤Þ¤·¤¿¡£
¤Á¤Ê¤ß¤Ë¡¢
GO(SPEED2,ope_amount(SENSOR_1));
GO(SPEED1,ope_amount(SENSOR_1));
¤ÎÉôʬ¤ò¡¢
GO(SPEED2,-ope_amount(SENSOR_1));
GO(SPEED1,-ope_aount(SENSOR_1));
¤ËÊѤ¨¤ë¤À¤±¤ÇÈ¿»þ·×¼þ¤ê¤Î¥é¥¤¥ó¥È¥ì¡¼¥¹¤â¤Ç¤¤Þ¤·¤¿¡£
**can_move.nxc(´Ì°ÜÆ°´ØÏ¢) [#p5edb615]
/*
ÄêµÁÉô
*/
#define SPEED1 40
#define SPIN RotateMotorEx(OUT_BC,100,180,100,true,true); Wait(2000);
#define CATCH(s) RotateMotor(OUT_A,s,180); Wait(2000); Off(OUT_A);
/*
´Ø¿ô
*/
inline void can_move()
{
Float(OUT_BC);
Wait(100);
Off(OUT_BC);
CATCH(100); // ´Ì¤ò¤Ä¤«¤ó¤À¾õÂÖ
RotateMotorEx(OUT_BC,SPEED1,1080,0,true,true); // ¼ã´³Á°¿Ê,²óž³Ñ¤ÏŬµ¹Êѹ¹¤ò
Wait(100);
Off(OUT_BC);
/*
Àû²ó
²óž³Ñ¤ÏŬµ¹Êѹ¹¤·¤Æ¤¯¤À¤µ¤¤
*/
SPIN;
CATCH(-100);
/*
Àû²ó
²óž³Ñ¤ÏŬµ¹Êѹ¹¤ò
*/
SPIN;
}
/*
task mainÆâÁȤ߹þ¤ßÍÑ
*/
task main()
{
SetSensorLowspeed(S2);
if (SensorUS(S2)<=10) {
can_move();
}
}
¢¨¼ÂºÝ¤Ë¤Ï¤³¤ì¤òPID_4.1.nxc¤ËÁȤ߹þ¤ó¤Ç»ÈÍѤ·¤Þ¤¹¡£
***´ðËÜÊý¿Ë [#naba760e]
¥é¥¤¥ó¥È¥ì¡¼¥¹Ãæ¤ËÁ°Êý¤Ë´Ì¤¬¤¢¤ë¤È¤¡¢Ä¶²»ÇÈ¥»¥ó¥µ¡¼¤Ç¬¤Ã¤Æµ÷Î¥¤¬10cm¤Ë¤Ê¤Ã¤¿¤È¤¤Ë´Ì¤òÆ°¤«¤¹inline´Ø¿ôcan_move¤¬ºîÆ°¤·¤Þ¤¹¡£
***inline´Ø¿ôcan_move [#mdaee0c0]
´Ì¤È¤Îµ÷Î¥¤¬10cm¤Ë¤Ê¤Ã¤¿¤éFloat¤Ç»ß¤Þ¤é¤»¤Þ¤¹¡£SPEED1¤¬40ÄøÅ٤ʤ顢¤³¤ì¤Ç´Ì¤Ë¤Ö¤Ä¤«¤é¤Ê¤¤ÄøÅ٤Ƕá¤Å¤¯¤³¤È¤¬¤Ç¤¤Þ¤¹¡£~
´°Á´¤Ë»ß¤Þ¤Ã¤¿¤é¡¢¥¢¡¼¥à¤ò¤ª¤í¤·¤Æ´Ì¤òÊú¤¨¹þ¤ß¤Þ¤¹¡£¤½¤Î¤Þ¤Þ¼ã´³Á°¿Ê¤·¤¿¤¢¤È¤½¤Î¾ì¤ÇÀû²ó¤·¤Æ´Ì¤¬¸µ¤¢¤Ã¤¿¾ì½ê¤Ë´Ì¤ò¤ª¤¤¤Æ¥¢¡¼¥à¤ò¤¢¤²¤Æ´Ì¤òÊü¤·¤Þ¤¹¡£~
¤½¤Î¸å¤Ë¤â¤¦£±ÅÙ¤½¤Î¾ì¤ÇÀû²ó¤·¤ÆÁ°¤ò¸þ¤¤¤Æ¡¢ËÜÍè¤Î¥é¥¤¥ó¥È¥ì¡¼¥¹¥×¥í¥°¥é¥à¤ØÌá¤ê¡¢¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò³¹Ô¤·¤Þ¤¹¡£
*È¿¾ÊÅÀ¡¦²þÁ±ÅÀ [#p4525d91]
PIDÀ©¸æ¤Ë»È¤¦KP,KI,KD¤ÎºÇŬ¤ÊÃͤòµá¤á¤ë¤Î¤¬Æñ¤·¤«¤Ã¤¿¤Ç¤¹¡£¤³¤ÎÃͤ¬¤ª¤«¤·¤¤¤È¥é¥¤¥ó¥È¥ì¡¼¥¹¤ÎÀ®¸ùΨ¤¬³ÊÃʤËÍî¤Á¤Æ¤·¤Þ¤¦¤Î¤â¶ìÏ«¤·¤Þ¤·¤¿¡£~
Ä´¤Ù¤Æ¤¤¤ë¤Èµá¤áÊý¤¬¤¢¤Ã¤¿¤Î¤Ç¤¹¤¬¡¢¤Ê¤«¤Ê¤«¤½¤ÎÊýË¡¤ò»È¤Ã¤Æ¤â¤¦¤Þ¤¯¤¤¤¤Þ¤»¤ó¤Ç¤·¤¿¡£~
·ë¶É¼êõ¤ê¤ÇÃÍõ¤·¤ò¤¹¤ë¤³¤È¤Ë¤Ê¤Ã¤Æ¤·¤Þ¤Ã¤Æ¤º¤¤¤Ö¤ó»þ´Ö¤ò¼è¤é¤ì¤Æ¤·¤Þ¤¤¤Þ¤·¤¿¡£~
º£¸å¤Ï£±¤«¤é¼êõ¤ê¤Ë¤Ê¤é¤Ê¤¤¤è¤¦¤ÊÊýË¡¤òõ¤·¤Æ¡¢Å¬ÀÚ¤ÊÃͤòÆþ¤ì¤ÆÀ®¸ùΨ¤ò¾å¤²¤Æ¤¤¤¤¿¤¤¤Ç¤¹¡£
¥Á¡¼¥à¤È¤·¤Æ¤Ï[[dyuma>#zab397a9]]¤¬´Ì¤òÆ°¤«¤¹¥×¥í¥°¥é¥à¤Î¼ÂÀïÅêÆþ¤·¤Æ¤¤¤¿¤Î¤Ç´Ì¤ò¤É¤«¤·¤Æ¥é¥¤¥ó¥È¥ì¡¼¥¹¤¹¤ë²ÝÂê¤ÏãÀ®¤Ï¤Ç¤¤Þ¤·¤¿¡£~
¤¿¤À¡¢¸Ä¿ÍŪ¤Ë¤Ï¡¢PID¤ÎÊý¤ÇKP,KI,KD¤ÎÃÍõ¤·¤Ë»þ´Ö¤ò¤«¤±¤¹¤®¤Æ¼«Ê¬¤Çºî¤Ã¤¿can_move.nxc¤ò¼ÂÀïÅêÆþ¤Ç¤¤Þ¤»¤ó¤Ç¤·¤¿¡£²Äǽ¤Ê¤é¼ÂÀïÅêÆþ¤·¤Æ¤ª¤¤¿¤¤¤Ç¤¹¡£
½ªÎ»¹Ô:
Ìܼ¡
#contents
*²ÝÂê1ÆâÍÆ [#vb71dca6]
>¼«ºî¤·¤¿¥³¡¼¥¹¤Ë±è¤Ã¤Æ°ì¼þ¤¹¤ë¤¹¤ë¥í¥Ü¥Ã¥È¤òºîÀ®¤»¤è¡£
>
>¤¿¤À¤·¡¢¼¡¤ÎÅÀ¤ËÃí°Õ¤¹¤ë¤³¤È
>
>°ì¿Í¤Ï»þ·×²ó¤êÍÑ¡¢¤â¤¦°ì¿Í¤ÏȾ»þ·×²ó¤¦¤êÍÑ¤Î¥×¥í¥°¥é¥à¤òºîÀ®¤¹¤ë¤³¤È¡Ê¥í¥Ü¥Ã¥ÈËÜÂΤÏƱ°ì¤Î¤â¤Î¤ò»È¤¦¡Ë¡£
>¼¡¤Î¥¹¥Æ¥Ã¥×¤Ç¥×¥í¥°¥é¥à¤òºîÀ®¤¹¤ë¤³¤È¡£
>¸òº¹ÅÀ¤ò¤¤Á¤ó¤Èǧ¼±¤·¡¢Àµ¤·¤¯È½ÃǤ·¤ÆÄ̲á¤Ç¤¤ë¤è¤¦¤Ë¤¹¤ë¡£
>³Î¼Â¤Ë°ì¼þ¤Ç¤¤ë¤è¤¦¤Ë¤¹¤ë¡£¥¹¥¿¡¼¥ÈÃÏÅÀ¤«¤é½Ðȯ¤·¤Æ¸µ¤Ë¤â¤É¤ë¤è¤¦¤Ë¤¹¤ë¡£
>¶õ¤´Ì¤ò¸òº¹ÅÀ¤«¤é10cm°Ê¾åÎ¥¤·¤¿Àþ¾å¤Ëµ¯¤¡¢¤½¤ì¤ò°ìö¤É¤±¤ÆºÆ¤Ó¸µ¤Î°ÌÃ֤ˤâ¤É¤¹¤è¤¦¤Ë²þÎɤ¹¤ë¡£
>1¸Ä¤Î¶õ¤´Ì¤Ç¤¦¤Þ¤¯¤¤¤Ã¤¿¤é¡¢2¸Ä¤Î¶õ¤´Ì¤Ç¤ä¤Ã¤Æ¤ß¤ë¡£
°Ê¾å[[2013b/Mission1]]¤è¤ê°úÍÑ
&aname(map);
¥³¡¼¥¹¿Þ
#ref(coi.jpg);
*¥á¥ó¥Ð¡¼ [#zab397a9]
[[dyuma>2013b/Member/dyuma/Mission1]]~
¥í¥Ü¥Ã¥ÈÀ½ºî¡Ê+¥í¥Ü¥Ã¥È´ÉÍý¡Ë¡¦¥×¥í¥°¥é¥ß¥ó¥°¤È¤É¤Á¤é¤â¤³¤Ê¤·¤Æ¤¤¤ëÃæ´Ö´ÉÍý¿¦¡£
[[pepo>2013b/Member/pepo/Mission1]]~
¥í¥Ü¥Ã¥È¤òÁȤޤ»¤¿¤éÈà¤Î±¦¤Ë½Ð¤ë¤â¤Î¤Ï¤¤¤Ê¤¤¡£¡Ê¤¿¤À¤·¾ðÊ󹩳زʡË
michaegon~
¤³¤Î¥Ú¡¼¥¸¤ÎÃø¼Ô~
¥×¥í¥°¥é¥à¤ÎÊý¤ò¤ä¤Ã¤Æ¤Þ¤¹¡£
*¥í¥Ü¥Ã¥È³µÍ× [#t8caacae]
º£²ó¤Î²ÝÂê¤Ë¤¢¤¿¤Ã¤Æ¥Á¡¼¥à¤Ç»È¤Ã¤¿¥í¥Ü¥Ã¥È¤ÎÁ°Ì̤ˤϡ¢¥é¥¤¥ó¥È¥ì¡¼¥¹ÍѤθ÷¥»¥ó¥µ¡¼¤È¡¢´ÌǧÃÎÍѤÎĶ²»ÇÈ¥»¥ó¥µ¡¼¤¬1¤Ä¤º¤Ä¤Ä¤¤¤Æ¤¤¤Þ¤¹¡£
**¥¢¡¼¥à [#caac7b24]
#ref(robot.jpg);
º£²ó»È¤Ã¤¿¥¢¡¼¥à¤Ï¡¢Êª¤ò¤Ä¤«¤à¤è¤ê¤Ï³Î¼ÂÀ¤¬¤¢¤ë¤À¤í¤¦¤È¤¤¤¦¤³¤È¤Ç¡¢Êª¤òÊú¤¨¹þ¤à·Á¤Ë¤·¤Þ¤·¤¿¡£~
ÉáÃʤΥ¢¡¼¥à¤Ï¾å¿Þ¤Î¤è¤¦¤Ë¤ª¤ê¤Æ¤¤¤ë¾õÂ֤ǤϤʤ¯¡¢¾å¤¬¤Ã¤¿¾õÂÖ¤ÇÂÔµ¡¤·¤Æ¤¤¤Þ¤¹¡£Êª¤òÊú¤¨¹þ¤à¤È¤¤Ë¤Ï¾å¿Þ¤Î¤è¤¦¤Ë¤ª¤ê¤Æ¤¤Þ¤¹¡£
**¸÷¥»¥ó¥µ¡¼ [#wd2ba5d2]
#ref(light.jpg);
¥é¥¤¥ó¥È¥ì¡¼¥¹ÍѤθ÷¥»¥ó¥µ¡¼¤ÏÀßÃÖ°ÌÃ֤˵¤¤òÇÛ¤ê¤Þ¤·¤¿¡£~
ÀâÌÀ½ñ¤Ë½ñ¤¤¤Æ¤¢¤ëÄ̤ê¤Ëºî¤Ã¤¿¥í¥Ü¥Ã¥È¤À¤È¡¢¸÷¥»¥ó¥µ¡¼¤Î°ÌÃÖ¤¬¥¿¥¤¥ä¤Î¼´¤«¤é±ó¤¤¤¿¤á¡¢µÞ¥«¡¼¥Ö¤òÁö¹Ô¤¹¤ë¤È¤¤Ê¤É¤Ë¡¢¸÷¥»¥ó¥µ¡¼¼«ÂΤϥ«¡¼¥Ö¾å¤Ë¤¢¤Ã¤Æ¤â¥¿¥¤¥ä¡ÊËÜÂΡˤ¬¥³¡¼¥¹¥¢¥¦¥È¤·¤Æ¤·¤Þ¤¦¤³¤È¤¬¤¢¤ê¤Þ¤·¤¿¡£~
º£²ó¤Ï¥¿¥¤¥ä¤Î¼´¤Ë¶ËÎϸ÷¥»¥ó¥µ¡¼¤ò¶á¤Å¤±¤Æ¡¢¥í¥Ü¥Ã¥È¤Î¹½Â¤¤¬¸¶°ø¤Ç¤Î¥³¡¼¥¹¥¢¥¦¥È¤¬µ¯¤³¤é¤Ê¤¤¤è¤¦¤Ëµ¤¤òÉÕ¤±¤Þ¤·¤¿¡£
°Ê²¼¤Ç¤Ï±¦¥¿¥¤¥ä¤ò¥â¡¼¥¿¡¼B¡¢º¸¥¿¥¤¥ä¤ò¥â¡¼¥¿¡¼C¡¢¥¢¡¼¥à¤ò¥â¡¼¥¿¡¼A¡¢¸÷¥»¥ó¥µ¡¼¤ò¥»¥ó¥µ¡¼1¡¢Ä¶²»ÇÈ¥»¥ó¥µ¡¼¤ò¥»¥ó¥µ¡¼2¤ËÀܳ¤·¤Æ¤¤¤ë¤â¤Î¤È¤·¤ÆÀâÌÀ¤·¤Æ¤¤¤¤Þ¤¹¡£
*¥×¥í¥°¥é¥à³µÍ× [#u38c8111]
¤³¤³¤Ç¤Ï¡¢¤³¤Î¥Ú¡¼¥¸¤ÎÃø¼Ômichaegon¤¬ºîÀ®¤·¤¿¥×¥í¥°¥é¥à¤Î³µÍפò·ÇºÜ¤·¤Þ¤¹¡£
¾¥á¥ó¥Ð¡¼¤¬ºîÀ®¤·¤¿¥×¥í¥°¥é¥à¤Ë¤Ä¤¤¤Æ¤Ï¾¥á¥ó¥Ð¡¼¤Î¥Ú¡¼¥¸¤ò¤´Í÷¤¯¤À¤µ¤¤¡£
**PID_4.1.nxc(¥é¥¤¥ó¥È¥ì¡¼¥¹ÍÑ) [#bea8657d]
#define KP 1.55 // ÈæÎ㥲¥¤¥ó
#define KI 0.00005// ÀÑʬ¥²¥¤¥ó
#define KD 0.85// Èùʬ¥²¥¤¥ó
#define DT 1
#define THRESHOLD 50 // ïçÃÍ
#define SPEED1 40 // ¹â®
#define SPEED2 25 // Äã®
#define GO(s,t) OnFwdSync(OUT_BC,s,t); Wait(DT);
#define JUDGE_T 2500 // ¥®¥¢¥Á¥§¥ó¥¸´ð½à»þ´Ö
float pid;
int result;
int dev[2]; // Êк¹
long time;
int flag;
float integral;
/*¡*/
inline int ope_amount(int sensor)
{
dev[0]=dev[1]; // ²áµî¤Î¥»¥ó¥µ¡¼ÃͤÈïçÃͤκ¹
dev[1]=sensor-THRESHOLD; // ¸½ºß¤Î¥»¥ó¥µ¡¼ÃͤÈïçÃͤκ¹
if (dev[1]==dev[0]) { // ¤â¤·¥»¥ó¥µ¡¼ÃͤÎÊѲ½¤¬¤Ê¤¤¾ì¹ç¤Ïintegral¤ò¥ê¥»¥Ã¥È¤¹¤ë
integral=0;
}
integral+=(dev[1]+dev[0])*DT/2.0; // ÀÑʬ¤ò·×»»¤¹¤ë
pid=KP*dev[1]+KI*integral+KD*(dev[1]-dev[0])/DT;
if (-100>pid) {
result=-100;
} else if (100<pid) {
result=100;
} else {
result=pid;
}
return result; // -100¤«¤é100¤Î´Ö¤Ç¥·¥ó¥¯¥íΨ¤òÊÖ¤¹
}
/*¢*/
task gearchange()
{
time=CurrentTick();
until (SENSOR_1<=THRESHOLD || CurrentTick()-time>=JUDGE_T);
if (CurrentTick()-time>=JUDGE_T) {
flag=1;
} else {
flag=2;
}
}
task main()
{
SetSensorLight(S1);
// Äã®ÈÇ
while (true) { // º¸¥¨¥Ã¥¸ÍÑ
if (flag==1) {
stop gearchange;
flag=0;
PlaySound(SOUND_UP);
break;
} else if (flag==2) {
stop gearchange;
flag=0;
}
if (SENSOR_1>THRESHOLD) {
start gearchange;
}
Low:
GO(SPEED2,ope_amount(SENSOR_1));
}
// ¹â®ÍÑ
while (true) { // º¸¥¨¥Ã¥¸
if (flag==1) {
stop gearchange;
flag=0;
PlaySound(SOUND_DOWN);
goto Low;
} else if (flag==2) {
stop gearchange;
flag=0;
}
if (SENSOR_1>THRESHOLD) {
start gearchange;
}
GO(SPEED1,ope_amount(SENSOR_1));
}
}
¢¨¤³¤Î¥×¥í¥°¥é¥à¤Ï»þ·×²ó¤ê¥é¥¤¥ó¥È¥ì¡¼¥¹ÍѤǤ¹¡£
***·Ð°Þ¡¦´ðËÜÊý¿Ë [#f7c7d47f]
PID_4.1.nxc¤Ï¥á¥ó¥Ð¡¼¤Î[[dyuma>#zab397a9]]¤Î¡¢ÀßÄꤷ¤¿ïçÃͤȸ÷¥»¥ó¥µ¡¼¤Î¼Â¬Ãͤκ¹¤Ë¤è¤Ã¤Æ¥í¥Ü¥Ã¥È¤Î¥«¡¼¥ÖÎ̤ò·èÄꤷ¤Æ¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò¤¹¤ë¤È¤¤¤¦¹Í¤¨¤¬¤â¤È¤Ë¤Ê¤Ã¤Æ¤¤¤Þ¤¹¡£~
/*
ïçÃͤȸ÷¥»¥ó¥µ¡¼¤ÎÃͤκ¹¤Ë¤è¤Ã¤Æ¥«¡¼¥ÖÎ̤ò·è¤á¤ë¥×¥í¥°¥é¥à
»þ·×²ó¤ê¥é¥¤¥ó¥È¥ì¡¼¥¹ÍÑ
*/
#define SPEED 30 // Á°¿Ê¥¹¥Ô¡¼¥É
#define THRESHOLD 50 // ïçÃÍ
#define turn_right(s) OnFwd(OUT_C,SPEED); OnRev(OUT_B,s); // ±¦¤Ë¶Ê¤¬¤ë
#define turn_left(s) OnFwd(OUT_B,SPEED); OnRev(OUT_C,s); // º¸¤Ë¶Ê¤¬¤ë
task main()
{
SetSensorLight(S1);
int back; // µÕ²óž¤¹¤ëÊý¤Î¥¿¥¤¥ä¤Î¥¹¥Ô¡¼¥É
while (true) {
while (SENSOR_1<THRESHOLD) { // ¥í¥Ü¥Ã¥È¤¬¥é¥¤¥ó¾å¤Ë¤¤¤ë¤È¤
back=THRESHOLD-SENSOR_1; // ïçÃͤȥ»¥ó¥µ¡¼¼Â¬Ãͤκ¹¤ò¥¹¥Ô¡¼¥É¤Ë¤¹¤ë
turn_right(back); // ¥é¥¤¥ó¾å¤Ë¤¤¤ë¤È¤¤Ï±¦¤Ë¶Ê¤¬¤ë
Wait(1);
}
while (SENSOR_1>=THRESHOLD) { // ¥í¥Ü¥Ã¥È¤¬¥é¥¤¥ó³°¤Ë¤¤¤ë¤È¤
back=SENSOR_1-THRESHOLD;
turn_left(back); // ¥é¥¤¥ó³°¤Ë¤¤¤ë¤È¤¤Ïº¸¤Ë¶Ê¤¬¤ë
Wait(1);
}
}
}
¤³¤Î¥×¥í¥°¥é¥à¤Ë¤Ï¡¢¤Þ¤Ã¤¹¤°¿Ê¤á¤ºÁö¹Ô¤¬¥¸¥°¥¶¥°¤Ë¤Ê¤ê³ê¤é¤«¤Ç¤Ê¤¤¡¢Á°¿Ê¥¹¥Ô¡¼¥É¤¬ÃÙ¤¤¤Ê¤É¤Î·çÅÀ¤¬¤¢¤ê¤Þ¤·¤¿¡£~
²¿¤«²ò·èºö¤Ï¤Ê¤¤¤«Ä´¤Ù¤Æ¤¤¤¿¤È¤³¤í¡¢¸«¤Ä¤±¤¿¤Î¤¬"PIDÀ©¸æ"¤Ç¤¹¡£~
>PIDÀ©¸æ(¥Ô¡¼¥¢¥¤¥Ç¥£¡¼¤»¤¤¤®¤ç = Proportional Integral Derivative Controller)¤Ï¡¢¥Õ¥£¡¼¥É¥Ð¥Ã¥¯À©¸æ¤Î°ì¼ï¤Ç¤¢¤ê¡¢ÆþÎÏÃͤÎÀ©¸æ¤ò½ÐÎÏÃͤÈÌÜɸÃͤȤÎÊк¹¡¢¤½¤ÎÀÑʬ¡¢¤ª¤è¤ÓÈùʬ¤Î3¤Ä¤ÎÍ×ÁǤˤè¤Ã¤Æ¹Ô¤¦ÊýË¡¤Î¤³¤È¤Ç¤¢¤ë¡£
¡ÖPIDÀ©¸æ¡×¡Ø¥Õ¥ê¡¼É´²Ê»öŵ¡¡¥¦¥£¥¥Ú¥Ç¥£¥¢ÆüËܸìÈÇ¡Ù(http://ja.wikipedia.org/wiki/PID%E5%88%B6%E5%BE%A1 )2013ǯ6·î18Æü (²Ð) 17:14 UTC ¤è¤ê°úÍÑ~
º£²ó¤Ï¡¢ÁàºîÎ̤òope¡¢ÊÑ¿ôx,y,z¡¢Äê¿ôKp¡ÊÈæÎ㥲¥¤¥ó¤È¤¤¤¦¡Ë¡¢Ki¡ÊÀÑʬ¥²¥¤¥ó¡Ë¡¢Kd¡ÊÈùʬ¥²¥¤¥ó¡Ë¤òÍѤ¤¤Æ°Ê²¼¤Î¼°¤ò¤¿¤Æ¤ÆÍøÍѤ·¤Þ¤¹¡£&aname(pid);~
#ref(tex1.jpg)
PID_4.1.nxc¤Ç¤Ï¡¢OnFwdSync¤ËÍѤ¤¤ë¥â¡¼¥¿¡¼¤ÎƱ´ü¡Ê¥·¥ó¥¯¥í¡ËΨ¤Ëope¤òÍѤ¤¤Þ¤¹¡£
&ruby(Proportional){ÈæÎã};(P)À©¸æ¤Ç¤Ï¡¢ÈæÎ㥲¥¤¥óKp¤È¡¢ïçÃͤȸ÷¥»¥ó¥µ¡¼¤Î¼Â¬Ãͤκ¹¡ÊÊк¹¡Ëx¤òÍѤ¤¤ÆÁàºîÎ̤ò·èÄꤷ¤Þ¤¹¡£~
¥í¥Ü¥Ã¥È¤Î¸½ºß°ÌÃÖ¤¬ïçÃ͡ʺ£²ó¤Ï¥é¥¤¥ó¤Î¶³¦¾å¡Ë¤«¤éÎ¥¤ì¤ì¤ÐÎ¥¤ì¤ë¤Û¤ÉÈæÎãŪ¤ËÁàºîÎ̤¬Â礤¯¤Ê¤ê¤Þ¤¹¡£
&ruby(Integral){ÀÑʬ};(I)À©¸æ¤Ç¤Ï¡¢ÀÑʬ¥²¥¤¥óKi¤È¡¢²áµî¤Î¸÷¥»¥ó¥µ¡¼¼Â¬ÃͤÎÎßÀÑy¤òÍѤ¤¤Æ¥«¡¼¥ÖÃæ¤Ê¤É¤Î¥í¥Ü¥Ã¥È¤Î°ÌÃÖ¤òÊäÀµ¤·¤Þ¤¹¡£~
ÈæÎã(P)À©¸æ¤À¤±¤Ç¤Ï¡¢¥í¥Ü¥Ã¥È¤¬ïçÃͶáÊÕ¤ò¿Ê¤ó¤Ç¤¤¤ë»þ¤ÏÁàºîÎ̤¬¾®¤µ¤¯¤Ê¤ê¡¢¥é¥¤¥ó¤¬¥«¡¼¥Ö¤òÉÁ¤¤¤Æ¤¤¤¿¤ê¤¹¤ë¤È¡¢¥«¡¼¥Ö¤ËÂбþ¤Ç¤¤º¤Ë¥é¥¤¥ó¡ÊïçÃ͡ˤ«¤é¼ã´³Î¥¤ì¤¿°ÌÃ֤ǰÂÄꤷ¤Æ¤·¤Þ¤¦¤³¤È¤¬¤¢¤ê¤Þ¤¹¡£¤½¤Î¤è¤¦¤Ê¤È¤¤Ë¡¢ÈæÎã(P)À©¸æ¤Ç¤ÏÂбþ¤Ç¤¤Ê¤¤¤è¤¦¤Ê¤ï¤º¤«¤ÊÊк¹¤ò¤âÃßÀѤ·¤Æ¤¤¤¡¢¤½¤ÎÎ̤˱þ¤¸¤ÆÁàºîÎ̤òÁý¤ä¤¹ÀÑʬ(I)À©¸æ¤ò»È¤Ã¤Æ¥í¥Ü¥Ã¥È¤ò¤è¤êïçÃͤ˶ᤤ°ÌÃÖ¤ËÊäÀµ¤·¤Þ¤¹¡£
&ruby(Derivative){ÀÑʬ};(D)À©¸æ¤Ç¤Ï¡¢Èùʬ¥²¥¤¥óKd¤È¡¢¸÷¥»¥ó¥µ¡¼¼Â¬ÃͤÎÊѲ½¡Ê·¹¤¡Ëz¤òÍѤ¤¤Æ¡¢Ì¤Íè¤Ë¥«¡¼¥Ö¤¬¤¢¤ë¤«¤Ê¤É¤òͽ¬¤·¤ÆÁàºîÎ̤ò·èÄꤷ¤Þ¤¹¡£¤³¤ì¤Ë¤è¤Ã¤Æ¥«¡¼¥Ö¤Î»Ï¤Þ¤ê¤Ç¥é¥¤¥ó¤«¤éÂ礤¯³°¤ì¤Æ¤·¤Þ¤¦¤È¤¤¤¦»öÂÖ¤òËɤ®¤Þ¤¹¡£
***KP,KI,KD¤Î·èÄêÊýË¡ [#h5ec5df1]
ÈæÎ㥲¥¤¥óKP,ÀÑʬ¥²¥¤¥óKI,Èùʬ¥²¥¤¥óKD¤ÎºÇŬ¤ÊÃͤò¸«¤Ä¤±¤ëÊýË¡¤Ë¤Ï¡¢Í̾¤Ê¤È¤³¤í¤Ç¤Ï¸Â³¦´¶ÅÙË¡¤Ê¤É¤¬¤¢¤ê¤Þ¤¹¡£~
º£²ó¤Ï¸Â³¦´¶ÅÙË¡¤ò»î¤·¤Æ¤ß¤Þ¤·¤¿¡£
¸Â³¦´¶ÅÙË¡¤Ç¤Ï¡¢¤Þ¤º¤Ï¤¸¤á¤ËKI=KD=0¤È¤·¤ÆPÀ©¸æ¤À¤±¤ÇÁö¹Ô¤µ¤»¤Æ¤ß¤Þ¤¹¡£~
¤³¤Î¤È¤KP¤ÎÃͤò½ù¡¹¤ËÁý¤ä¤·¤Æ¤¤¤Ã¤Æ¡¢¥í¥Ü¥Ã¥È¤¬»ý³Ū¤Ë¿¶Æ°¤¹¤ë¡Êsin¥«¡¼¥Ö¤Î¤è¤¦¤Êµ°Æ»¤òÉÁ¤¤Ê¤¬¤éÁö¹Ô¤·Â³¤±¤ë¡Ë¾õÂ֤ˤʤëKP¤òõ¤·¤Þ¤¹¡Ê¤³¤ì¤òKu¤È¤ª¤¯)¡£~
¤³¤Î¤È¤¤Î¥í¥Ü¥Ã¥È¤Î¿¶Æ°¼þ´ü¤òTu¤È¤ª¤¤¤Æ¡¢Ku,Tu¤«¤éPIDÀ©¸æ¤Ë¤·¤¿¤È¤¤ÎŬÀÚ¤ÊKP,KI,KD¤ò»»½Ð¤·¤Þ¤¹¡£
¤³¤Î¤è¤¦¤Ë¤¦¤Þ¤¯¸Â³¦´¶ÅÙË¡¤¬»È¤¨¤ì¤Ð¤è¤«¤Ã¤¿¤Î¤Ç¤¹¤¬¡¢¼ÂºÝ¤ä¤Ã¤Æ¤ß¤ë¤È¤¦¤Þ¤¯¤¤¤¯¤¤¤Þ¤»¤ó¤Ç¤·¤¿¡£~
¤½¤â¤½¤âKP¤ÎÃͤò¤¤¤¯¤éÂ礤¯¤·¤Æ¤â¿¶Æ°¤¬»ý³¤»¤º¤Ë¤¹¤°¤Ë¿¶Æ°¤¬¸º¿ê¤·¤Æ¤·¤Þ¤¤Ku,Tu¤¬¤¦¤Þ¤¯½Ð¤»¤Ê¤«¤Ã¤¿¤Î¤Ç¡¢º£²ó¤Ï¸Â³¦´¶ÅÙË¡»È¤¦¤Î¤òÃÇÇ°¤·¤Þ¤·¤¿¡£
¤È¤¤¤¦¤³¤È¤Çº£²ó¤Ï½é¤á¤«¤é¼êºî¶È¤ÇÃͤò·è¤á¤Þ¤·¤¿¡£~
PIDÀ©¸æ¤ÎÃæ¤Ç¥á¥¤¥ó¤Ë¤Ê¤ë¤Î¤ÏPÀ©¸æ¤Ç¡¢IDÀ©¸æ¤ÏPÀ©¸æ¤ÎÊä½õ¤Î¤è¤¦¤Ê¤â¤Î¤Ê¤Î¤Ç¡¢¤Þ¤º¤ÏPÀ©¸æ¤Î¤ß¤ÇÁö¹Ô¤µ¤»¤Þ¤¹¡£¤³¤Î»þ¡¢°ìÈÖ³ê¤é¤«¤Ë°ÂÄꤷ¤ÆÁö¹Ô¤·¤¿KP¤ò¸«¤Ä¤±¤Æ»ÈÍѤ·¤Þ¤·¤¿¡£~
¼¡¤Ë¡¢IÀ©¸æ¤ò²Ã¤¨¤Æ¥«¡¼¥Ö¤òÁö¹Ô¤µ¤»¤Þ¤¹¡£IÀ©¸æ¤Ç¤Ï¼ã´³¤Î°ÌÃÖÊäÀµ¤¬¤Ç¤¤ì¤Ð¤¤¤¤¤Î¤Ç¡¢KI¤ÎÃͤϡ¢¥í¥Ü¥Ã¥È¤¬PÀ©¸æ¤Î¤ß¤Î»þ¤è¤ê¤â¥«¡¼¥Ö¤ËµÛ¤¤ÉÕ¤¯¤è¤¦¤ËÁö¹Ô¤·¤Æ¡¢¤¢¤Þ¤êÂ礤¯¤Ê¤¤ÊÕ¤ê¤ËÀßÄꤷ¤Æ¤ª¤¤Þ¤¹¡£~
ºÇ¸å¤ÎDÀ©¸æ¤Ï¡¢µÞ·ã¤Ê¥³¡¼¥¹ÊѲ½¤ËÂбþ¤µ¤»¤ë¤¿¤á¤Î¤â¤Î¤Ç¡¢¤¢¤Þ¤êKD¤ÎÃͤòÂ礤¯¤·¤¹¤®¤ë¤È¡¢¥³¡¼¥¹¤Î¤ï¤º¤«¤ÊÊѲ½¤Ë¤â²á¾ê¤ËÈ¿±þ¤·¤Æ¡¢Áö¹ÔÃæ¤Ë¥í¥Ü¥Ã¥È¤¬ÌµÂ̤ʿ¶Æ°¤ò¤·¤ÆÉÔ°ÂÄê¤ÊÁö¹Ô¤Ë¤Ê¤Ã¤Æ¤·¤Þ¤¤¤Þ¤¹¡£¤è¤Ã¤Æ¡¢KD¤ÎÃͤâ¤Þ¤¢¤êÂ礤¹¤®¤Ê¤¤ÊÕ¤ê¤ËÀßÄꤷ¤Æ¤ª¤¤Þ¤¹¡£
***¡ inline´Ø¿ôope_amount [#k44c6886]
/*PID_4.1.nxc¤Î°ìÉô*/
#define KP 1.55 // ÈæÎ㥲¥¤¥ó
#define KI 0.00005// ÀÑʬ¥²¥¤¥ó
#define KD 0.85// Èùʬ¥²¥¤¥ó
#define DT 1
#define THRESHOLD 50 // ïçÃÍ
float pid;
int result;
int dev[2];
float integral;
inline int ope_amount(int sensor)
{
dev[0]=dev[1]; // ²áµî¤ÎÊк¹
dev[1]=sensor-THRESHOLD; // ¸½ºß¤ÎÊк¹
if (dev[1]==dev[0]) { // ¤â¤·Êк¹ÊѲ½¤¬¤Ê¤¤¾ì¹ç¤Ïintegral¤ò¥ê¥»¥Ã¥È¤¹¤ë
integral=0;
}
integral+=(dev[1]+dev[0])*DT/2.0; // ÀÑʬ¤ò·×»»¤¹¤ë
pid=KP*dev[1]+KI*integral+KD*(dev[1]-dev[0])/DT;
if (-100>pid) {
result=-100;
} else if (100<pid) {
result=100;
} else {
result=pid; // int·¿¤Ë¥¥ã¥¹¥È
}
return result;
}
KP¤ÏÈæÎ㥲¥¤¥ó¡¢KI¤ÏÀÑʬ¥²¥¤¥ó¡¢KP¤ÏÈùʬ¥²¥¤¥ó¡¢DT¤Ï¸÷¥»¥ó¥µ¡¼¤ÎÃͤòµÏ¿¤¹¤ë´Ö³Ö¡Êº£²ó¤Ï1¥ß¥êÉáˤǤ¹¡£~
dev[0]¤Ë¤Ï1¥ß¥êÉÃÁ°¤Î¸÷¥»¥ó¥µ¡¼¤ÎÃͤÈïçÃͤκ¹¡ÊÊк¹¡Ë¡¢dev[1]¤Ë¤Ï¸½ºß¤ÎÊк¹¤òÆþ¤ì¤Þ¤¹¡£~
¼¡¤ÏIÀ©¸æ¤Î¤¿¤á¤Î½àÈ÷¤Ç¤¹¡£integral¤Ë¤Ï¡¢1¥ß¥êÉ䴤ȤË(dev[1]+dev[0])*DT/2¡Ê²¼¿Þ¤Î²«¿§¤¤Éôʬ¡Ë¤ò¤·¤Æ¤¤¤¤Þ¤¹¡ÊÀÑʬ¡Ë¡£~
#ref(integral.jpg)
¤³¤Î¤È¤¡¢integral¤ËÊк¹¤¬ÃßÀѤ·Â³¤±¤ë¤È¡¢IÀ©¸æ¤Ç¤ÎÊäÀµ¤¬É¬Íפʤ¯¤Ê¤Ã¤Æ¤âÁàºîÎ̤¬ÌµÂ̤ËÁý¤¨¤¿¤Þ¤Þ¤Ë¤Ê¤Ã¤Æ¤·¤Þ¤¦¤Î¤Ç¡¢Êк¹ÊѲ½¤¬¤Ê¤¯¤Ê¤Ã¤¿¤È¤¡ÊľÀþ¾å¤Ê¤É¡Ë¤Ë¤Ïintegral¤ò¥ê¥»¥Ã¥È¤¹¤ëµ¡Ç½¤òÉÕ¤±¤Æ¤¢¤ê¤Þ¤¹¡£
ÉâÆ°¾®¿ôÅÀpid¤Ë¤Ï¡¢PIDÀ©¸æ¤Ë¤è¤Ã¤Æ·èÄꤷ¤¿ÁàºîÎ̤òÆþ¤ì¤Þ¤¹¡Ê[[¼°(1)>#pid]]¡Ë¡£~
¤³¤³¤Ç¡¢~
Kp*dev[1]¤ÏPÀ©¸æ¡¢~
Ki*integral¤ÏIÀ©¸æ¡¢~
¤Þ¤¿¡¢DT¤òÈù¾®»þ´Ö¤È¤ß¤Ê¤·¤Æ¸÷¥»¥ó¥µ¡¼ÃÍ¿ä°Ü¤Î¥°¥é¥Õ¤Î·¹¤¤ò(dev[1]-dev[0])/DT¤È¤·¤¿¤È¤¡ÊÈùʬ¡Ë¡¢~
Kd*(dev[1]-dev[0])/DT¤ÏDÀ©¸æ¤Ë¤è¤ëÁàºîÎ̤òɽ¤·¤Æ¤¤¤Þ¤¹¡£
#ref(d.jpg),left
¾å¤Çµá¤á¤¿pid¤òOnFwdSync¤Î¥·¥ó¥¯¥íΨ¡ÊÈϰϤÏ-100¤«¤é100¤Þ¤Ç¤ÎÀ°¿ô¡Ë¤È¤·¤Æ»È¤¤¤¿¤¤¤Î¤Ç¡¢¤³¤Î´Ø¿ô¤ÎºÇ¸å¤Îifʸ¤Ç¡¢int·¿¤Îresult¤Ëpid¤òÂåÆþ¤·¤ÆÀ°¿ôÃͤËÊÑ´¹¤·¤¿¤¢¤È¡¢-100¤«¤é100¤Þ¤Ç¤Ë¼ý¤Þ¤ë¤è¤¦¤ËÃͤòÄ´À°¤·¤Þ¤¹¡£
***¢ ¥á¥¤¥ó¥¿¥¹¥¯¡¦¥µ¥Ö¥¿¥¹¥¯gearchange [#f48902aa]
/*PID_4.1.nxc¤Î°ìÉô*/
#define THRESHOLD 50 // ïçÃÍ
#define SPEED1 40 // ¹â®
#define SPEED2 25 // Äã®
#define GO(s,t) OnFwdSync(OUT_BC,s,t); Wait(DT);
#define JUDGE_T 2500 // ¥®¥¢¥Á¥§¥ó¥¸´ð½à»þ´Ö
int flag;
task gearchange()
{
time=CurrentTick();
until (SENSOR_1<=THRESHOLD || CurrentTick()-time>=JUDGE_T);
if (CurrentTick()-time>=JUDGE_T) {
flag=1;
} else {
flag=2;
}
}
task main()
{
SetSensorLight(S1);
// Äã®Áö¹Ô¥ë¡¼¥×
while (true) { // º¸¥¨¥Ã¥¸ÍÑ
if (flag==1) {
stop gearchange;
flag=0;
PlaySound(SOUND_UP);
break;
} else if (flag==2) {
stop gearchange;
flag=0;
}
if (SENSOR_1>THRESHOLD) {
start gearchange;
}
Low:
GO(SPEED2,ope_amount(SENSOR_1));
}
// ¹â®Áö¹Ô¥ë¡¼¥×
while (true) { // º¸¥¨¥Ã¥¸
if (flag==1) {
stop gearchange;
flag=0;
PlaySound(SOUND_DOWN);
goto Low;
} else if (flag==2) {
stop gearchange;
flag=0;
}
if (SENSOR_1>THRESHOLD) {
start gearchange;
}
GO(SPEED1,ope_amount(SENSOR_1));
}
}
¤â¤È¤â¤È¤Î¥á¥¤¥ó¥¿¥¹¥¯¤Ï¡¢~
task main()
{
while (true) {
GO(SPEED1,ope_amount(SENSOR_1));
}
}
¤À¤±¤Ç¤·¤¿¡£¤³¤ì¤Ç¤â¥é¥¤¥ó¥È¥ì¡¼¥¹¤Ï¤Ç¤¤ë¤Î¤Ç¤¹¤¬¡¢¸òº¹ÅÀ¤äT»úÏ©¤òľ¿Ê¤·¤Æ¤·¤Þ¤¤¡¢¶Ê¤¬¤Ã¤Æ¤¯¤ì¤Þ¤»¤ó¤Ç¤·¤¿¡£¤³¤ì¤Ï¡¢¸òº¹ÅÀ¤äT»úÏ©¤Îľ³ÑÉôʬ¤ËÁàºîÎ̤ÎÊѲ½¤¬Äɤ¤¤Ä¤±¤º¡¢¤Þ¤ÀľÀþ¤À¤Èǧ¼±¤·¤Æ¤¤¤ë¤¦¤Á¤ËÄ̲ᤷ¤Æ¤·¤Þ¤¦¤³¤È¤¬¸¶°ø¤Î¤è¤¦¤Ç¤·¤¿¡£~
¸òº¹ÅÀ¤òľÀþ¤À¤Èǧ¼±¤·¤ÆÄ̲ᤷ¤Æ¤¯¤ì¤ë¤Î¤Ï¤¢¤ê¤¬¤¿¤¤¤Î¤Ç¤¹¤¬¡¢T»úÏ©¤âľ¿Ê¤·¤Æ¤·¤Þ¤¦¤È¥¹¥¿¡¼¥ÈÃÏÅÀ¤«¤é½Ð¤é¤ì¤Ê¤¤¤Î¤Ç¡¢T»úÏ©¤¬¸½¤ì¤ëÉôʬ¤À¤±Â®ÅÙ¤òÍî¤È¤·¤Æ¥«¡¼¥Ö¤È¤·¤Æǧ¼±¤¹¤ë¤è¤¦¤Ë²þÎɤ·¤¿¤Î¤¬º£²ó¤Î¥á¥¤¥ó¥¿¥¹¥¯¤Ç¤¹¡£
¤³¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤µ¤»¤ë¤È¡¢¤Þ¤º¤ÏÄ㮤ǥ饤¥ó¥È¥ì¡¼¥¹¤ò¤¹¤ë̵¸Â¥ë¡¼¥×¤ËÆþ¤ê¤Þ¤¹¡£~
¤³¤Î¥ë¡¼¥×¤Ë¤¤¤ë¸Â¤ê¡¢¥í¥Ü¥Ã¥È¤Ï¸òº¹ÅÀ¤âT»úÏ©¤â¥«¡¼¥Ö¤ÈȽÃǤ·¤Æ¶Ê¤¬¤Ã¤Æ¤¤¤¤Þ¤¹¡£~
¤è¤Ã¤Æ¡¢½é¤á¤ÎT»úÏ©¤Ï̵»ö¤Ë¶Ê¤¬¤ë¤³¤È¤¬¤Ç¤¤Þ¤¹¤¬¡¢¤³¤Î¤Þ¤Þ¤Ç¤Ï¤½¤Î¤¢¤È¤Î¸òº¹ÅÀ¤Ê¤É¤âÄ̲᤻¤º¤Ë¶Ê¤¬¤Ã¤Æ¤·¤Þ¤¤¤Þ¤¹¡£
¤µ¤Æ¡¢½é¤á¤ÎT»úÏ©¤ò¡¢º¸¤Ë¶Ê¤¬¤Ã¤¿¾ì¹ç¤Ï¼¡¤Ëľ³Ñ¥«¡¼¥Ö¤Î³°Â¦¤ò¡¢±¦¤Ë¶Ê¤¬¤Ã¤¿¾ì¹ç¤Ï¼¡¤Ë¥Ø¥¢¥Ô¥ó¥«¡¼¥Ö¤Î³°Â¦¤ò²ó¤ë¤³¤È¤Ë¤Ê¤ê¤Þ¤¹¡£¡Ê[[¥³¡¼¥¹¿Þ>#map]]¡Ë~
¤³¤Î¤È¤¡¢¤É¤¦¤·¤Æ¤â¥í¥Ü¥Ã¥È¤Ï¥é¥¤¥ó¤è¤ê¤â¼ã´³Âç²ó¤ê¤Ë¤Ê¤ë¤¿¤á¡¢£²¡Á£³Éäδ֤º¤Ã¤È¡¢¸÷¥»¥ó¥µ¡¼¤ÎÃͤ¬ïçÃͤè¤ê¤âÂ礤¯¤Ê¤ê¤Þ¤¹¡£~
¤³¤Î¸½¾Ý¤Ï¡¢ÈæÎ㥲¥¤¥ó¤Ê¤É¤òŬÀÚ¤ÊÃͤˤ·¤Æ¤ª¤±¤Ð¡¢º£²ó¤Î¥³¡¼¥¹¤Î¾¤Î¾ì½ê¤Ç¤Ïµ¯¤³¤ê¤Þ¤»¤ó¡£¤³¤Î¤³¤È¤òÍøÍѤ·¤Æ¡¢Â®ÅÙÊѹ¹¤ò¹Ô¤¤¤Þ¤¹¡£
Ä㮥饤¥ó¥È¥ì¡¼¥¹¤Î¥ë¡¼¥×Ãæ¤Ç¡¢¸÷¥»¥ó¥µ¡¼¤ÎÃͤ¬ïçÃͤè¤êÂ礤¯¤Ê¤ë¡Ê¥í¥Ü¥Ã¥È¤¬¥é¥¤¥ó³°¤Ë½Ð¤ë¡Ë¤È¡¢¥µ¥Ö¥¿¥¹¥¯gearchange¤¬ºîÆ°¤·¤Þ¤¹¡£~
¥µ¥Ö¥¿¥¹¥¯gearchange¤Ï¡¢°Ê²¼¤ÎÆ°ºî¤ò¤·¤Þ¤¹¡£
+¸÷¥»¥ó¥µ¡¼¤ÎÃͤ¬ïçÃͤè¤êÂ礤¯¤Ê¤Ã¤Æ¤¤¤ë¡Ê¥í¥Ü¥Ã¥È¤¬¥é¥¤¥ó³°¤Ë¤¤¤ë¡Ë»þ´Ö¤ò·×¬
+¤â¤·¸÷¥»¥ó¥µ¡¼¤ÎÃͤ¬ïçÃͤè¤êÂ礤«¤Ã¤¿»þ´Ö¤¬¡¢~
JUDGE_T°Ê¾å¤À¤Ã¤¿¾ì¹ç¤Ï¡¢flag¤Ë1¤òÂåÆþ~
JUDGE_T°Ê²¼¤À¤Ã¤¿¾ì¹ç¤Ïflag¤Ë2¤òÂåÆþ
¤¹¤ë¤È¡¢¥á¥¤¥ó¥¿¥¹¥¯Æâ¤Îifʸ¤¬¼Â¹Ô¤µ¤ì¤Þ¤¹¡£¤³¤Îifʸ¤Ç¤Ï¡¢
+¥µ¥Ö¥¿¥¹¥¯gearchange¤òÄä»ß¤µ¤»¤ë
+flag¤ò0¤ËÌ᤹
+gearchange¤Ç~
flag¤Ë1¤¬ÂåÆþ¤µ¤ì¤Æ¤¤¤¿¡Ê¥í¥Ü¥Ã¥È¤¬JUDGE_T°Ê¾å¥é¥¤¥ó³°¤Ë¤¤¤¿¡Ë¾ì¹ç¤ÏÄ㮥饤¥ó¥È¥ì¡¼¥¹¤Î¥ë¡¼¥×¤òÈ´¤±¤Æ¹â®¥é¥¤¥ó¥È¥ì¡¼¥¹¤Î¥ë¡¼¥×¤Ø°ÜÆ°~
flag¤Ë2¤¬ÂåÆþ¤µ¤ì¤Æ¤¤¤¿¡Ê¥í¥Ü¥Ã¥È¤¬JUDGE_T¤Ë㤹¤ëÁ°¤Ë¥é¥¤¥ó¾å¤ËÌá¤Ã¤¿¡Ë¾ì¹ç¤Ï¤³¤Î¤Þ¤ÞÄ㮥饤¥ó¥È¥ì¡¼¥¹¤Î¥ë¡¼¥×¤ò·Ñ³
¤³¤Îµ¡Ç½¤Ï¹â®¥é¥¤¥ó¥È¥ì¡¼¥¹¤Î¥ë¡¼¥×¤Ë¤âÅëºÜ¤µ¤ì¤Æ¤¤¤Þ¤¹¡£
¤³¤³¤Ç¡¢¥í¥Ü¥Ã¥È¤¬¥³¡¼¥¹¤ò»þ·×²ó¤ê¤Ë¥é¥¤¥ó¥È¥ì¡¼¥¹¤µ¤»¤Æ¤ß¤Þ¤·¤¿¡£¥í¥Ü¥Ã¥È¤Ï¡¢
+Äã®Áö¹Ô¤Ç»Ï¤Þ¤ê¡¢½é¤á¤ÎT»úÏ©¤òº¸¤Ë¶Ê¤¬¤ë
+¼¡¤Îľ³Ñ¥«¡¼¥Ö¤Î³°Â¦¤ò²ó¤ë~
¤³¤Î¤È¤¡¢JUDGE_T°Ê¾å¥é¥¤¥ó¤«¤é³°¤ì¤Æ¥í¥Ü¥Ã¥È¤¬²ó¤ë¤Î¤Ç¡¢¤³¤³¤ÇÄã®Áö¹Ô¥ë¡¼¥×¤«¤é¹â®Áö¹Ô¥ë¡¼¥×¤ËÀÚ¤êÂؤï¤ë
+¹â®Áö¹ÔÃæ¤Ç¤Ï¡¢¸òº¹ÅÀ¤äT»úÏ©¤òľÀþ¤ÈȽÃǤ·¤ÆÄ̲ᤷ¤Æ¤¤¤¯¤Î¤Ç¡¢¤³¤Î¸å¤Ë¤¢¤ë¸òº¹ÅÀ¤ÏÁ´¤ÆÄ̲á
+£²¤Ä¤á¤Î¸òº¹ÅÀ¤òÈ´¤±¤Æ¥Ø¥¢¥Ô¥ó¥«¡¼¥Ö¤Î³°Â¦¤ò²ó¤ëÉôʬ¤Ëº¹¤·³Ý¤«¤ë~
¥«¡¼¥Ö¤Î¶Ê¤¬¤ê¶ñ¹ç¤ËÄɤ¤¤Ä¤±¤ºJUDGE_T°Ê¾å¥í¥Ü¥Ã¥È¤¬¥é¥¤¥ó¤«¤é³°¤ì¤ë¤Î¤Ç¡¢¹â®Áö¹Ô¤«¤éÄã®Áö¹Ô¤ËÀÚ¤êÂؤï¤ë
+¤½¤Î¸åº¹¤·³Ý¤«¤ëT»úÏ©¤ÏÄã®Áö¹ÔÃæ¤Ê¤Î¤Ç¥«¡¼¥ÖȽÄꤷ¤Æ¶Ê¤¬¤ê¡¢Ìµ»ö¤Ë¥´¡¼¥ë¤ØÅþã
¤³¤Î¤è¤¦¤Ë¥³¡¼¥¹¤ò°ì¼þ¤Ç¤¤Þ¤·¤¿¡£
¤Á¤Ê¤ß¤Ë¡¢
GO(SPEED2,ope_amount(SENSOR_1));
GO(SPEED1,ope_amount(SENSOR_1));
¤ÎÉôʬ¤ò¡¢
GO(SPEED2,-ope_amount(SENSOR_1));
GO(SPEED1,-ope_aount(SENSOR_1));
¤ËÊѤ¨¤ë¤À¤±¤ÇÈ¿»þ·×¼þ¤ê¤Î¥é¥¤¥ó¥È¥ì¡¼¥¹¤â¤Ç¤¤Þ¤·¤¿¡£
**can_move.nxc(´Ì°ÜÆ°´ØÏ¢) [#p5edb615]
/*
ÄêµÁÉô
*/
#define SPEED1 40
#define SPIN RotateMotorEx(OUT_BC,100,180,100,true,true); Wait(2000);
#define CATCH(s) RotateMotor(OUT_A,s,180); Wait(2000); Off(OUT_A);
/*
´Ø¿ô
*/
inline void can_move()
{
Float(OUT_BC);
Wait(100);
Off(OUT_BC);
CATCH(100); // ´Ì¤ò¤Ä¤«¤ó¤À¾õÂÖ
RotateMotorEx(OUT_BC,SPEED1,1080,0,true,true); // ¼ã´³Á°¿Ê,²óž³Ñ¤ÏŬµ¹Êѹ¹¤ò
Wait(100);
Off(OUT_BC);
/*
Àû²ó
²óž³Ñ¤ÏŬµ¹Êѹ¹¤·¤Æ¤¯¤À¤µ¤¤
*/
SPIN;
CATCH(-100);
/*
Àû²ó
²óž³Ñ¤ÏŬµ¹Êѹ¹¤ò
*/
SPIN;
}
/*
task mainÆâÁȤ߹þ¤ßÍÑ
*/
task main()
{
SetSensorLowspeed(S2);
if (SensorUS(S2)<=10) {
can_move();
}
}
¢¨¼ÂºÝ¤Ë¤Ï¤³¤ì¤òPID_4.1.nxc¤ËÁȤ߹þ¤ó¤Ç»ÈÍѤ·¤Þ¤¹¡£
***´ðËÜÊý¿Ë [#naba760e]
¥é¥¤¥ó¥È¥ì¡¼¥¹Ãæ¤ËÁ°Êý¤Ë´Ì¤¬¤¢¤ë¤È¤¡¢Ä¶²»ÇÈ¥»¥ó¥µ¡¼¤Ç¬¤Ã¤Æµ÷Î¥¤¬10cm¤Ë¤Ê¤Ã¤¿¤È¤¤Ë´Ì¤òÆ°¤«¤¹inline´Ø¿ôcan_move¤¬ºîÆ°¤·¤Þ¤¹¡£
***inline´Ø¿ôcan_move [#mdaee0c0]
´Ì¤È¤Îµ÷Î¥¤¬10cm¤Ë¤Ê¤Ã¤¿¤éFloat¤Ç»ß¤Þ¤é¤»¤Þ¤¹¡£SPEED1¤¬40ÄøÅ٤ʤ顢¤³¤ì¤Ç´Ì¤Ë¤Ö¤Ä¤«¤é¤Ê¤¤ÄøÅ٤Ƕá¤Å¤¯¤³¤È¤¬¤Ç¤¤Þ¤¹¡£~
´°Á´¤Ë»ß¤Þ¤Ã¤¿¤é¡¢¥¢¡¼¥à¤ò¤ª¤í¤·¤Æ´Ì¤òÊú¤¨¹þ¤ß¤Þ¤¹¡£¤½¤Î¤Þ¤Þ¼ã´³Á°¿Ê¤·¤¿¤¢¤È¤½¤Î¾ì¤ÇÀû²ó¤·¤Æ´Ì¤¬¸µ¤¢¤Ã¤¿¾ì½ê¤Ë´Ì¤ò¤ª¤¤¤Æ¥¢¡¼¥à¤ò¤¢¤²¤Æ´Ì¤òÊü¤·¤Þ¤¹¡£~
¤½¤Î¸å¤Ë¤â¤¦£±ÅÙ¤½¤Î¾ì¤ÇÀû²ó¤·¤ÆÁ°¤ò¸þ¤¤¤Æ¡¢ËÜÍè¤Î¥é¥¤¥ó¥È¥ì¡¼¥¹¥×¥í¥°¥é¥à¤ØÌá¤ê¡¢¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò³¹Ô¤·¤Þ¤¹¡£
*È¿¾ÊÅÀ¡¦²þÁ±ÅÀ [#p4525d91]
PIDÀ©¸æ¤Ë»È¤¦KP,KI,KD¤ÎºÇŬ¤ÊÃͤòµá¤á¤ë¤Î¤¬Æñ¤·¤«¤Ã¤¿¤Ç¤¹¡£¤³¤ÎÃͤ¬¤ª¤«¤·¤¤¤È¥é¥¤¥ó¥È¥ì¡¼¥¹¤ÎÀ®¸ùΨ¤¬³ÊÃʤËÍî¤Á¤Æ¤·¤Þ¤¦¤Î¤â¶ìÏ«¤·¤Þ¤·¤¿¡£~
Ä´¤Ù¤Æ¤¤¤ë¤Èµá¤áÊý¤¬¤¢¤Ã¤¿¤Î¤Ç¤¹¤¬¡¢¤Ê¤«¤Ê¤«¤½¤ÎÊýË¡¤ò»È¤Ã¤Æ¤â¤¦¤Þ¤¯¤¤¤¤Þ¤»¤ó¤Ç¤·¤¿¡£~
·ë¶É¼êõ¤ê¤ÇÃÍõ¤·¤ò¤¹¤ë¤³¤È¤Ë¤Ê¤Ã¤Æ¤·¤Þ¤Ã¤Æ¤º¤¤¤Ö¤ó»þ´Ö¤ò¼è¤é¤ì¤Æ¤·¤Þ¤¤¤Þ¤·¤¿¡£~
º£¸å¤Ï£±¤«¤é¼êõ¤ê¤Ë¤Ê¤é¤Ê¤¤¤è¤¦¤ÊÊýË¡¤òõ¤·¤Æ¡¢Å¬ÀÚ¤ÊÃͤòÆþ¤ì¤ÆÀ®¸ùΨ¤ò¾å¤²¤Æ¤¤¤¤¿¤¤¤Ç¤¹¡£
¥Á¡¼¥à¤È¤·¤Æ¤Ï[[dyuma>#zab397a9]]¤¬´Ì¤òÆ°¤«¤¹¥×¥í¥°¥é¥à¤Î¼ÂÀïÅêÆþ¤·¤Æ¤¤¤¿¤Î¤Ç´Ì¤ò¤É¤«¤·¤Æ¥é¥¤¥ó¥È¥ì¡¼¥¹¤¹¤ë²ÝÂê¤ÏãÀ®¤Ï¤Ç¤¤Þ¤·¤¿¡£~
¤¿¤À¡¢¸Ä¿ÍŪ¤Ë¤Ï¡¢PID¤ÎÊý¤ÇKP,KI,KD¤ÎÃÍõ¤·¤Ë»þ´Ö¤ò¤«¤±¤¹¤®¤Æ¼«Ê¬¤Çºî¤Ã¤¿can_move.nxc¤ò¼ÂÀïÅêÆþ¤Ç¤¤Þ¤»¤ó¤Ç¤·¤¿¡£²Äǽ¤Ê¤é¼ÂÀïÅêÆþ¤·¤Æ¤ª¤¤¿¤¤¤Ç¤¹¡£
¥Ú¡¼¥¸Ì¾: