Ìܼ¡ #contents *²ÝÂê1ÆâÍÆ [#vb71dca6] >¼«ºî¤·¤¿¥³¡¼¥¹¤Ë±è¤Ã¤Æ°ì¼þ¤¹¤ë¤¹¤ë¥í¥Ü¥Ã¥È¤òºîÀ®¤»¤è¡£ > >¤¿¤À¤·¡¢¼¡¤ÎÅÀ¤ËÃí°Õ¤¹¤ë¤³¤È > >°ì¿Í¤Ï»þ·×²ó¤êÍÑ¡¢¤â¤¦°ì¿Í¤ÏȾ»þ·×²ó¤¦¤êÍÑ¤Î¥×¥í¥°¥é¥à¤òºîÀ®¤¹¤ë¤³¤È¡Ê¥í¥Ü¥Ã¥ÈËÜÂΤÏƱ°ì¤Î¤â¤Î¤ò»È¤¦¡Ë¡£ >¼¡¤Î¥¹¥Æ¥Ã¥×¤Ç¥×¥í¥°¥é¥à¤òºîÀ®¤¹¤ë¤³¤È¡£ >¸òº¹ÅÀ¤ò¤¤Á¤ó¤Èǧ¼±¤·¡¢Àµ¤·¤¯È½ÃǤ·¤ÆÄ̲á¤Ç¤¤ë¤è¤¦¤Ë¤¹¤ë¡£ >³Î¼Â¤Ë°ì¼þ¤Ç¤¤ë¤è¤¦¤Ë¤¹¤ë¡£¥¹¥¿¡¼¥ÈÃÏÅÀ¤«¤é½Ðȯ¤·¤Æ¸µ¤Ë¤â¤É¤ë¤è¤¦¤Ë¤¹¤ë¡£ >¶õ¤´Ì¤ò¸òº¹ÅÀ¤«¤é10cm°Ê¾åÎ¥¤·¤¿Àþ¾å¤Ëµ¯¤¡¢¤½¤ì¤ò°ìö¤É¤±¤ÆºÆ¤Ó¸µ¤Î°ÌÃ֤ˤâ¤É¤¹¤è¤¦¤Ë²þÎɤ¹¤ë¡£ >1¸Ä¤Î¶õ¤´Ì¤Ç¤¦¤Þ¤¯¤¤¤Ã¤¿¤é¡¢2¸Ä¤Î¶õ¤´Ì¤Ç¤ä¤Ã¤Æ¤ß¤ë¡£ °Ê¾å[[2013b/Mission1]]¤è¤ê°úÍÑ ¥³¡¼¥¹¿Þ *¥á¥ó¥Ð¡¼ [#zab397a9] [[dyuma>2013b/Member/dyuma/Mission1]]~ ¥í¥Ü¥Ã¥ÈÀ½ºî¡Ê+¥í¥Ü¥Ã¥È´ÉÍý¡Ë¡¦¥×¥í¥°¥é¥ß¥ó¥°¤È¤É¤Á¤é¤â¤³¤Ê¤·¤Æ¤¤¤ëÃæ´Ö´ÉÍý¿¦¡£ [[pepo>2013b/Member/pepo/Mission1]]~ ¥í¥Ü¥Ã¥È¤òÁȤޤ»¤¿¤éÈà¤Î±¦¤Ë½Ð¤ë¤â¤Î¤Ï¤¤¤Ê¤¤¡£¡Ê¤¿¤À¤·¾ðÊó¹©³Ø²Ê¡Ë michaegon~ ¤³¤Î¥Ú¡¼¥¸¤ÎÃø¼Ô~ ¥×¥í¥°¥é¥à¤ÎÊý¤ò¤ä¤Ã¤Æ¤Þ¤¹¡£ *¥í¥Ü¥Ã¥È³µÍ× [#t8caacae] °Ê²¼¤Ç¤Ï±¦¥¿¥¤¥ä¤ò¥â¡¼¥¿¡¼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 BENCHMARK 2500 // ¥®¥¢¥Á¥§¥ó¥¸´ð½à»þ´Ö float p; float i; float d; int diff[2]; // Êк¹ long time; int flag; float integral; /*¢*/ inline int ope_limit(float x, int y, int z) { int result=x; // int·¿¤Ë¥¥ã¥¹¥È if (result<y) { result=y; } else if (result>z) { result=z; } return result; } /*£*/ inline int ope_amount(int sensor) { diff[0]=diff[1]; // ²áµî¤Î¥»¥ó¥µ¡¼ÃͤÈïçÃͤκ¹ diff[1]=sensor-THRESHOLD; // ¸½ºß¤Î¥»¥ó¥µ¡¼ÃͤÈïçÃͤκ¹ if (diff[1]==diff[0]) { // ¤â¤·¥»¥ó¥µ¡¼ÃͤÎÊѲ½¤¬¤Ê¤¤¾ì¹ç¤Ïintegral¤ò¥ê¥»¥Ã¥È¤¹¤ë integral=0; } integral+=(diff[1]+diff[0])*DT/2.0; // ÀÑʬ¤ò·×»»¤¹¤ë p=KP*diff[1]; // ÈæÎã·×»» i=KI*integral; // ÀÑʬ·×»» d=KD*(diff[1]-diff[0])/DT; // Èùʬ·×»» return ope_limit(p+i+d,-100,100); // -100¤«¤é100¤Î´Ö¤Ç¥·¥ó¥¯¥íΨ¤òÊÖ¤¹ } /*¤*/ task gearchange() { time=CurrentTick(); until (SENSOR_1<=THRESHOLD || CurrentTick()-time>=BENCHMARK); if (CurrentTick()-time>=BENCHMARK) { 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: while (SENSOR_1>THRESHOLD) { GO(SPEED2,ope_amount(SENSOR_1)); } while (SENSOR_1<THRESHOLD) { GO(SPEED2,ope_amount(SENSOR_1)); } /* // ±¦¥¨¥Ã¥¸ÍÑ¡£Å¬µ¹/ * ¡Á* /¤Î°ÌÃÖ¤òÆþ¤ìÂؤ¨¤Æ¤¯¤À¤µ¤¤¡£ while (SENSOR_1<THRESHOLD) { GO(SPEED2,-ope_amount(SENSOR_1)); } if (SENSOR_1>THRESHOLD) { start gearchange; } while (SENSOR_1>THRESHOLD) { 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; } while (SENSOR_1>THRESHOLD) { GO(SPEED1,ope_amount(SENSOR_1)); } while (SENSOR_1<THRESHOLD) { GO(SPEED1,ope_amount(SENSOR_1)); } /* // ±¦¥¨¥Ã¥¸ÍÑ¡£Å¬µ¹/ * ¡Á* /¤Î°ÌÃÖ¤òÆþ¤ìÂؤ¨¤Æ¤¯¤À¤µ¤¤¡£ while (SENSOR_1<THRESHOLD) { GO(SPEED1,-ope_amount(SENSOR_1)); } if (SENSOR_1>THRESHOLD) { start gearchange; } while (SENSOR_1>THRESHOLD) { 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 ¤è¤ê°úÍÑ #ref(tex1.jpg) ***¡ ¥Þ¥¯¥í¡¦ÊÑ¿ô [#yc4b16c3] ***¢ inline´Ø¿ôope_limit [#u063292a] ***£ inline´Ø¿ôope_amount [#k44c6886] ***¤ ¥µ¥Ö¥¿¥¹¥¯gearchange [#c893e581] ***¥ ¥á¥¤¥ó¥¿¥¹¥¯ [#zb42dde9] **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,90); Wait(2000); Off(OUT_A); /* ´Ø¿ô */ inline void can_move() { ResetTachoCount(OUT_BC); Float(OUT_BC); Wait(100); long motor=MotorTachoCount(OUT_B); Off(OUT_BC); /* ¥â¡¼¥¿¡¼¤Î²óž³Ñ¤ÏŬµ¹Êѹ¹¤·¤Æ¤¯¤À¤µ¤¤ ¥¢¡¼¥à¤Ï¥â¡¼¥¿¡¼A¤ËÀܳ¤µ¤ì¤Æ¤¤¤ë¤â¤Î¤È¤¹¤ë */ CATCH(100); // ´Ì¤ò¤Ä¤«¤ó¤À¾õÂÖ RotateMotorEx(OUT_BC,SPEED1,motor,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]