Ìܼ¡
¥Ô¥ó¥Ý¥ó¶Ì¤È»æ¥³¥Ã¥×¤òÊÌ¡¹¤Ë²ó¼ý¤·¤Æ¥Ô¥ó¥Ý¥ó¶Ì¤ÏXÃÏÅÀ¡¢»æ¥³¥Ã¥×¤ÏYÃÏÅÀ¤Ë±¿¤Ö¡£¥ë¡¼¥ë¤ä¥Õ¥£¡¼¥ë¥É¤Î¾ÜºÙ¤Ë¤Ä¤¤¤Æ¤Ï¤³¤Á¤é¤ò»²¾È¤·¤Æ¤¯¤À¤µ¤¤¡£
º£²óƱ¤¸¤è¤¦¤Ê¤Ä¤¯¤ê¤Îµ¡ÂΤò£²µ¡ºîÀ®¤·¤¿¡£¤½¤ì¤¾¤ìÊ̤ǺîÀ®¤·¤¿¤Î¤ÇºÙ¤«¤¤¤È¤³¤í¤Ê¤É¤Î¿¾¯¤Î°ã¤¤¤Ï¤¢¤ë¡£¤¿¤À¥×¥í¥°¥é¥à¤Ë´Ø¤·¤Æ¤ÏÁ´¤¯Ê̤Τâ¤Î¤ÇÆ°¤«¤·¤Æ¤¤¤ë¡£»æ¥³¥Ã¥×¤òÄϤó¤Ç»ý¤Á¾å¤²¤ëµ¡¹½¤Ï£²µ¡¤È¤âƱ¤¸µ¡¹½¤Ç¤¢¤ë¡£¼«Ê¬¤¬Ã´Åö¤·¤¿¤Î¤Ïµ¡ÂΣÁ¤Ê¤Î¤Ç¡¢µ¡ÂΣÁ¤Ë¤Ä¤¤¤Æ¾Ü¤·¤¯ÀâÌÀ¤·¤Æ¤¤¤¯¡£
µ¡ÂΤÎÁ°¤ËĶ²»ÇÈ¥»¥ó¥µ¡¼¡¢¸å¤í¤Ë»æ¥³¥Ã¥×¤òÄϤó¤Ç»ý¤Á¾å¤²¤ë¤¿¤á¤Î¥¢¡¼¥à¤ò¤Ä¤±¤¿¡£¤³¤Îµ¡¹½¤Î¥¢¡¼¥à¤òÁ°¤Ë¤Ä¤±¤ë¤ÈĶ²»ÇÈ¥»¥ó¥µ¡¼¤Î¼ÙËâ¤Ë¤Ê¤Ã¤Æ¤·¤Þ¤¦¤Î¤Ç¸å¤í¤Ë¤Ä¤±¤¿¡£Ä¶²»ÇÈ¥»¥ó¥µ¡¼¤Ï½Ä¤Ë¤Ä¤±¤¿Êý¤¬Àµ³Î¤Ë¬¤ì¤ë¤È¤¤¤¦¤³¤È¤Ç½Ä¤Ë¼è¤êÉÕ¤±¤¿¡£
´ðËÜŪ¤Ë¤Ï£Á¤ÈƱ¤¸¤è¤¦¤Êµ¡¹½¤Ç¤¢¤ë¡££Â¤Ë¤Ï¥é¥¤¥È¥»¥ó¥µ¡¼¤È¥¹¥¿¡¼¥È¤Î»þ¤Ë»È¤¦¥¿¥Ã¥Á¥»¥ó¥µ¡¼¤¬¤Ä¤¤¤Æ¤¤¤ë¡£¾Ü¤·¤¯¤Ï²¼¤Î¥Ú¡¼¥¸¤ò¤´Í÷¤¯¤À¤µ¤¤¡£
µ¡ÂÎËÜÂΤ˥⡼¥¿¡¼¤ò£²¤Ä»È¤Ã¤Æ¤·¤Þ¤Ã¤Æ¤¤¤ë¤Î¤Ç¡¢£±¤Ä¤Î¥â¡¼¥¿¡¼¤Ç»æ¥³¥Ã¥×¤òÄϤࡦ»ý¤Á¾å¤²¤ë¤òξÊý¹Ô¤¦µ¡¹½¤òºîÀ®¤·¤¿¡£¤Þ¤ºÄϤॢ¡¼¥à¤ÎÉôʬ¤¬ÊĤޤꡢ¤½¤ì°Ê¾åÊĤޤé¤Ê¤¤¤È¤Ê¤ë¤È¥¢¡¼¥àÁ´ÂΤ¬»ý¤Á¾å¤¬¤ë¤È¤¤¤¦»ÅÁȤߤÀ¡£¼Ì¿¿¤Ë¤¢¤ë¤è¤¦¤Ë¡¢£³¤Ä¤Î»õ¼Ö¤È¼Ì¿¿¤Ç¤Ï³Îǧ¤·¤Å¤é¤¤¤Î¤À¤¬¾®¤µ¤¤»õ¼Ö¤Ë¤è¤Ã¤Æ¤³¤Î»ÅÁȤߤ¬½ÐÍè¤Æ¤¤¤ë¡£
»æ¥³¥Ã¥×¤òÄϤࡦ»ý¤Á¾å¤²¤ë¡¦½Å¤Í¤ëή¤ì¤Ï¼¡¤ÎÄ̤ê¤À¡£
ÄϤॢ¡¼¥à¤ò³«¤¤¤¿¾õÂ֤ǻ楳¥Ã¥×¤ÎÁ°¤Ë¹Ô¤¡¢»æ¥³¥Ã¥×¤¬¥¢¡¼¥àÆâ¤ËÆþ¤ë¤è¤¦¤ËÁ°¿Ê¤¹¤ë¡£¤½¤·¤Æ»æ¥³¥Ã¥×¤òÄϤࡣ
ÄϤॢ¡¼¥à¤¬¤³¤ì°Ê¾åÊĤޤé¤Ê¤¤¤È¤Ê¤ë¤È¡¢¥¢¡¼¥àÁ´ÂΤ¬¾å¤¬¤ê»Ï¤á¤ë¡£¥¢¡¼¥àÁ´ÂΤϾ¯¤·¤À¤±¤¢¤²¤Æ»ß¤á¤ë¡£¥Ô¥ó¥Ý¥ó¶Ì¤¬¾¯¤·¸«¤¨¤ë¤¯¤é¤¤¤Ë»æ¥³¥Ã¥×¤ò¾å¤²¤ë¤³¤È¤Ç¡¢¥Ô¥ó¥Ý¥ó¶Ì¤ò³ä¤êȤ¤ÎÏȤËÆþ¤ì¤ë¤³¤È¤¬»æ¥³¥Ã¥×¤ÏÏȤ˰ú¤Ã¤«¤«¤é¤º¡¢¥Ô¥ó¥Ý¥ó¶Ì¤ÏÏȤò¾è¤ê±Û¤¨¤Æ¤¯¤ì¤ë¤Î¤Ç¥¹¥à¡¼¥º¤Ë¹Ô¤¨¤ë¡£¤½¤·¤Æ¥¢¡¼¥àÁ´ÂΤò¾å¤²¤ë¤³¤È¤Ç¥Ô¥ó¥Ý¥ó¶Ì¤Ï»æ¥³¥Ã¥×¤«¤é½Ð¤Æ¤¯¤ë¡£
»æ¥³¥Ã¥×¤òÄϤó¤Ç»ý¤Á¾å¤²¤¿¾õÂÖ¤ÇÆ°¤¡¢¤¹¤Ç¤ËÃÖ¤«¤ì¤Æ¤¤¤ë»æ¥³¥Ã¥×¤ÎÁ°¤Ë¹Ô¤¯¡£¼Ì¿¿¤Ç¤âʬ¤«¤ë¤è¤¦¤Ë¡¢»æ¥³¥Ã¥×¤ò»ý¤Ã¤¿¾õÂ֤Ǥ⤽¤Î²¼¤Ë»æ¥³¥Ã¥×¤¬Æþ¤ì¤ë¤è¤¦¤Ê¹â¤µ¤Ï³ÎÊݽÐÍè¤ë¤è¤¦¤ËºîÀ®¤·¤¿¡£¤½¤·¤ÆÄϤॢ¡¼¥à¤ò¾¯¤·³«¤¯¤È»ý¤Ã¤Æ¤¤¤¿»æ¥³¥Ã¥×¤ÏÍî¤Á¤ë¡£¥¢¡¼¥àÉôʬ¤ò·Ú¤¯¤·¤Æ¤¤¤ë¤¿¤á¡¢¥â¡¼¥¿¡¼¤òµÕ¤Ë²óž¤µ¤»¤¿¤È¤¤Ë¡¢ÄϤॢ¡¼¥à¤¬Àè¤Ë³«¤¯¤è¤¦¤Ë¤Ê¤Ã¤Æ¤¤¤ë¡£
µ¡ÂΣÁ¤Ïº¸Â¦¡¢µ¡ÂΣ¤ϱ¦Â¦¤«¤é¥¹¥¿¡¼¥È¤¹¤ë¡£»æ¥³¥Ã¥×¤Ïº¸¾å¤Î¤â¤Î¤ò»æ¥³¥Ã¥×£±¡¢ºÇ¤âXÃÏÅÀ¤Ë¶á¤¤¤â¤Î¤ò»æ¥³¥Ã¥×£²¡¢²¼¤ÎÊý¤Ë¤¢¤ë¤â¤Î¤ò»æ¥³¥Ã¥×£³¤È¤·¤ÆÀâÌÀ¤·¤Æ¤¤¤¯¡£¡ß¤Î¤Ä¤¤¤Æ¤¤¤ë»æ¥³¥Ã¥×¤Ï¾ã³²Êª¤Ç¤¢¤ë¡£¡Ê¼Ì¿¿»²¾È¡Ë
»æ¥³¥Ã¥×¤ò¼è¤ê¡¢¥Ô¥ó¥Ý¥ó¶Ì¤òXÃÏÅÀ¤ØÃÖ¤¯½çÈ֤ϡ¢£±¢ª£²¢ª£³¤Î½ç¤Ç¤¢¤ë¡£ YÃÏÅÀ¤Ë»æ¥³¥Ã¥×¤òÃÖ¤¯½çÈ֤ϡ¢£²¢ª£±¢ª£³¤Î½ç¤Ç¤¢¤ë¡£ »æ¥³¥Ã¥×£±¤È£³¤Ïµ¡ÂΣÁ¡¢»æ¥³¥Ã¥×£²¤Ïµ¡ÂΣ¤¬Ã´Åö¤¹¤ë¡£
Àµ³Î¤µ¤äÄ´À°¤Î¤·¤ä¤¹¤µ¤ò¹Í¤¨¤ÆÃÙ¤á¤Î¥¹¥Ô¡¼¥É¤ËÄêµÁ¤·¤¿¡£
#define SPEED_SLOW 25
»æ¥³¥Ã¥×¤òõ¤·¤Æ¡¢¤½¤Î»æ¥³¥Ã¥×¤Î¼êÁ°¤Þ¤Ç¿Ê¤à¥×¥í¥°¥é¥à¤Ç¤¢¤ë¡£
const float diameter = 5.45; //¥¿¥¤¥ä¤Îľ·Â(cm) const float track = 10.91; //¥¿¥¤¥ä¤Î¥È¥ì¥Ã¥ÉÉý(cm) const float pi = 3.1415; //±ß¼þΨ void fwdDist(float d) //µ÷Î¥ d cm Á°¿Ê { long angle = d/(diameter*pi)*360.0; //ɬÍפʥ¿¥¤¥ä¤Î²óž³ÑÅÙ RotateMotorEx(OUT_AB,SPEED_SLOW, angle,0,true,true); } void baDist(float d) //µ÷Î¥ d cm ¸å¿Ê { long angle = d/(diameter*pi)*360.0; //ɬÍפʥ¿¥¤¥ä¤Î²óž³ÑÅÙ RotateMotorEx(OUT_AB,SPEED_SLOW, -angle,0,true,true); } void turnAng(long ang) //³ÑÅÙangÅ٤λþ·×²ó¤ê¤ÎÀû²ó { long angle = track/diameter * ang; //ɬÍפʥ¿¥¤¥ä¤Î²óž³ÑÅÙ RotateMotorEx(OUT_AB,SPEED_SLOW, angle,100,true,true); } void nturnAng(long ang) //³ÑÅÙangÅ٤λþ·×²ó¤ê¤ÎÀû²ó { long angle = track/diameter * ang; //ɬÍפʥ¿¥¤¥ä¤Î²óž³ÑÅÙ RotateMotorEx(OUT_AB,SPEED_SLOW, angle,0,true,true); } int searchDirection(long ang) //¸½ºß¤ÎÊý¸þ¤òÃæ¿´¤ËangÅÙ¤ÎÈϰϤÇõ¤· //¾ã³²Êª¤Þ¤Ç¤Îµ÷Î¥¤òÊÖ¤¹ { long tacho_min ; //¤â¤Ã¤È¤â¶á¤¤µ÷Î¥¤ò¼Â¸½¤¹¤ë¥¿¥¤¥ä¤Î²óž¿ô int d_min = 300 ; //¤â¤Ã¤È¤â¶á¤¤µ÷Î¥¤Î²¾¤ÎºÇ¾®ÃÍ long angle = (track/diameter)*ang; //Àû²ó³ÑÅÙ¤«¤é¥¿¥¤¥ä¤Î²óž¤ò·×»» turnAng(ang/2); //»ØÄꤵ¤ì¤¿³ÑÅÙ¤ÎȾʬ¤òÀû²ó ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È OnFwdSync(OUT_AB,SPEED_SLOW,-100); //È¿»þ·×²ó¤ê¤ËÀû²ó while(MotorTachoCount(OUT_A)<=angle){ if (SensorUS(S1)<d_min){ //¸½ºß¤Îµ÷Î¥¤¬²¾¤ÎºÇ¾®Ãͤè¤ê¾®¤µ¤¤¾ì¹ç d_min = SensorUS(S1); //²¾¤ÎºÇ¾®Ãͤò¹¹¿· tacho_min = MotorTachoCount(OUT_A); //¤³¤Î»þ¤Î¥¿¥¤¥ä¤Î²óž¿ô¤òµÏ¿ } } OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); until(MotorTachoCount(OUT_A)<=tacho_min || SensorUS(S1)<=d_min); Wait(14); //ÈùÄ´À° Off(OUT_AB);Wait(500); return d_min; }
ËÜʸ¤Î°ìÈÖ½é¤á¤Î¥×¥í¥°¥é¥à¤òÎã¤Ë¤¹¤ë¤È¡¢int d = searchDirection(30)¤Ç¸½ºß¸þ¤¤¤Æ¤¤¤ëÊý¸þ¤òÃæ¿´¤Ë15ÅÙ¤º¤Ä¤Î·×30ÅÙ¤ÎÈϰϤǻ楳¥Ã¥×¤òĶ²»ÇÈ¥»¥ó¥µ¡¼¤Ë¤è¤Ã¤Æõ¤·¡¢fwdDist(d-25.0)¤Ç»æ¥³¥Ã¥×¤Î¼êÁ°25cm¤Þ¤Ç¹Ô¤¯¤È¤¤¤¦¤è¤¦¤Ê¥×¥í¥°¥é¥à¤Ç¤¢¤ë¡£
»æ¥³¥Ã¥×¤òÄϤó¤Ç»ý¤Á¾å¤²¤ë¡¦¥¢¡¼¥à¤ò³«¤¤¤Æ²¼¤²¤ë¥×¥í¥°¥é¥à¤Ç¤¢¤ë¡£³ÑÅÙ»ØÄê¤Ç£äÅ٥⡼¥¿¡¼¤ò²ó¤·¤ÆÆ°¤«¤·¤Æ¤¤¤ë¡£¥¢¡¼¥à¤òÊĤ¸¤Æ¾å¤²¤ë°ìÏ¢¤ÎÆ°¤¤¬¥â¡¼¥¿¡¼¤Î²óž³Ñ¤¬¤ª¤è¤½645ÅÙ¤À¤Ã¤¿¤Î¤Ç¤³¤ì¤òºÇÂç¤È¤·¤Æ¿Ê¤á¤¿¡£
sub c_up(float d) //645¤ÇºÇÂç { RotateMotor(OUT_C,-30,d); Off(OUT_C); } sub c_down(float d) { RotateMotor(OUT_C,30,d); Off(OUT_C); }
¥×¥í¥°¥é¥à¤ÎËÜʸ¤Ç¤¢¤ë¡£
»æ¥³¥Ã¥×¤ò¼è¤ëή¤ì¤È¤·¤Æ¤Ï¡¢Ãµ¤·¤Æ¶á¤Å¤È¿Å¾¤·¤Æ¥¢¡¼¥à¤òÆ°¤«¤·¤ÆÄϤࡣ´ðËÜŪ¤ËÆ°¤¤Ï³ÑÅÙ»ØÄê¤Ê¤Î¤ÇÅÅÃÓ»ÄÎ̤˽ÐÍè¤ë¤À¤±º¸±¦¤µ¤ì¤Ê¤¤¤è¤¦¤Ë¤·¤Æ¤¤¤ë¡£
task main() { SetSensorLowspeed(S1); int d = searchDirection(30);¡¡¡¡//»æ¥³¥Ã¥×£±¤ÎÊý¸þ¤òõ¤·¤¿¸å¡¢¶á¤Å¤¤¤Æ25cm¼êÁ°¤ÇÄä»ß if (d > 10){ fwdDist(d-25.0); }
¤³¤³¤Î¸¡ÃΤÏĶ²»ÇÈ¥»¥ó¥µ¡¼¤Î¼è¤êÉÕ¤±³ÑÅ٤ζñ¹ç¤Ç¤¦¤Þ¤¯¤¤¤«¤Ê¤«¤Ã¤¿¤ê¤·¤¿¤Î¤ÇÄ´À°¤¬É¬ÍפÀ¤Ã¤¿¡£
turnAng(200);¡¡¡¡//200Å٤λþ·×²ó¤ê¤ÎÀû²ó c_down(640);¡¡¡¡//¥¢¡¼¥à¤ò³«¤¤¤Æ²¼¤²¤ë OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¤³¤Î2¤Ä¤òÆþ¤ì¤Ê¤¤¤È¼¡¤Î¹ÔÆ°¤¬¤¦¤Þ¤¯Æ°¤«¤Ê¤«¤Ã¤¿¡Ê°Ê¹ß¡¢ÈùÄ´À°¤È¤·¤Æµ¤·¤Æ¤¤¤¯¡Ë baDist(12.0);¡¡¡¡//12cm¸åÂà c_up(260); //»æ¥³¥Ã¥×£±¤ò¤Ä¤«¤ó¤Ç¾¯¤·¾å¤²¤ë ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È turnAng(110);¡¡¡¡//110Å٤λþ·×²ó¤ê¤ÎÀû²ó OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À° baDist(11.0);¡¡¡¡//11cm¸åÂà c_up(330); //»æ¥³¥Ã¥×£±¤ò¾å¤²¡¢¥Ô¥ó¥Ý¥ó¶Ì¤ò¤Ï¤Ê¤¹ ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È turnAng(90);¡¡¡¡//90Å٤λþ·×²ó¤ê¤ÎÀû²ó OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À° fwdDist(17.0);¡¡¡¡//17cmÁ°¿Ê ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È turnAng(80);¡¡¡¡//80Å٤λþ·×²ó¤ê¤ÎÀû²ó ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È int e = searchDirection(40);¡¡¡¡//¾ã³²¤Î»æ¥³¥Ã¥×¤ÎÊý¸þ¤òõ¤·¤¿¸å¡¢¤½¤ÎÊý¸þ¤ò¸þ¤¤¤ÆÄä»ß int msg; // ¼õ¤±¼è¤Ã¤¿Ãͤò³ÊǼ¤¹¤ëÊÑ¿ô while (true) {¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥á¥Ã¥»¡¼¥¸¤ò¼õ¤±¼è¤ë¤Þ¤ÇÄä»ß¤·¤ÆÂÔµ¡ ReceiveRemoteNumber(MAILBOX1,true,msg); if (msg == 1){ break;¡¡¡¡¡¡¡¡//whileʸ¤òÈ´¤±½Ð¤¹ } else { Off(OUT_AB); } Wait(100); } Off(OUT_AB);Wait(3000);¡¡¡¡//3ÉÃÄä»ß
whileʸ¤Çµ¡ÂΣ¤«¤é¤Î¿®¹æ¤¬Íè¤ë¤Þ¤ÇÄä»ß¤µ¤»¡¢¿®¹æ¤¬Í褿¤ébreakʸ¤Çwhileʸ¤«¤éÈ´¤±½Ð¤¹¤è¤¦¤Ë¤Ê¤Ã¤Æ¤¤¤ë¡££³ÉäÎÄä»ß¤Ï¿®¹æ¤ò»È¤Ã¤Æ¤¤¤Ê¤¤¤È¤¤Ë¾¯¤·´Ö¤ò¼è¤ë¤¿¤á¤Ë»È¤Ã¤Æ¤¤¤¿¤â¤Î¤Ç¤¢¤ë¡£
ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È turnAng(30);¡¡¡¡//30Å٤λþ·×²ó¤ê¤ÎÀû²ó OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À° fwdDist(15.0);¡¡¡¡//15cmÁ°¿Ê ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È int f = searchDirection(30);¡¡¡¡//»æ¥³¥Ã¥×£²¤ÎÊý¸þ¤òõ¤·¤¿¸å¡¢¶á¤Å¤¤¤Æ25cm¼êÁ°¤ÇÄä»ß if (f > 10){ fwdDist(f-25.0); } ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È turnAng(156);¡¡¡¡//156Å٤λþ·×²ó¤ê¤ÎÀû²ó OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À° baDist(8.0);¡¡¡¡//8cm¸åÂà c_down(100);¡¡¡¡//¥¢¡¼¥à¤ò³«¤¡¢»æ¥³¥Ã¥×£±¤ò¤Ï¤Ê¤¹ OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À° baDist(2.0);¡¡¡¡//2cm¸åÂà
»æ¥³¥Ã¥×¤ò¤Ï¤Ê¤¹¤À¤±¤Ç¤Ï»æ¥³¥Ã¥×¤¬´°Á´¤ËÆþ¤é¤Ê¤«¤Ã¤¿¤ê°ú¤Ã¤«¤«¤Ã¤¿¤ê¤·¤¿¤Î¤Ç¡¢¼¡¤Î¸åÂà¤Ç¾¯¤·Íɤ餷¤ÆÆþ¤ë¤è¤¦¤Ë¤·¤Æ¤¤¤ë¡£
OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À° fwdDist(12.0);¡¡¡¡//12cmÁ°¿Ê c_up(100);¡¡¡¡//¥¢¡¼¥à¤òºÇÂç¤Þ¤Ç¾å¤²¤ë
¥¢¡¼¥à¤òºÇÂç¤Þ¤Ç¾å¤²¤ë¤³¤È¤Ç¡¢»æ¥³¥Ã¥×£³¤â£±¤Î»þ¤ÈƱÍͤοôÃͤÈή¤ì¤ÇÆ°¤«¤»¤ë¤è¤¦¤Ë¤·¤Æ¤¤¤ë¡£
ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È turnAng(230);¡¡¡¡//230Å٤λþ·×²ó¤ê¤ÎÀû²ó ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È int g = searchDirection(20);¡¡¡¡//»æ¥³¥Ã¥×£³¤ÎÊý¸þ¤òõ¤·¤¿¸å¡¢¶á¤Å¤¤¤Æ25cm¼êÁ°¤ÇÄä»ß if (g > 10){ fwdDist(g-25.0); } turnAng(190);¡¡¡¡//190Å٤λþ·×²ó¤ê¤ÎÀû²ó c_down(630);¡¡¡¡//¥¢¡¼¥à¤ò³«¤¤¤Æ²¼¤²¤ë OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À° baDist(16.0);¡¡¡¡//16cm¸åÂà c_up(270);¡¡¡¡//»æ¥³¥Ã¥×£³¤ò¤Ä¤«¤ó¤Ç¾¯¤·¾å¤²¤ë ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È turnAng(140);¡¡¡¡//140Å٤λþ·×²ó¤ê¤ÎÀû²ó c_up(340);¡¡¡¡//»æ¥³¥Ã¥×£³¤ò¾å¤²¡¢¥Ô¥ó¥Ý¥ó¶Ì¤ò¤Ï¤Ê¤¹ ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È turnAng(290);¡¡¡¡//290Å٤λþ·×²ó¤ê¤ÎÀû²ó OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À° baDist(12.0);¡¡¡¡//12cm¸åÂà ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È int h = searchDirection(20);¡¡¡¡//»æ¥³¥Ã¥×£±¤ÎÊý¸þ¤òõ¤·¤¿¸å¡¢¶á¤Å¤¤¤Æ25cm¼êÁ°¤ÇÄä»ß if (h > 10){ fwdDist(h-25.0); } ResetTachoCount(OUT_AB); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È turnAng(156);¡¡¡¡//156Å٤λþ·×²ó¤ê¤ÎÀû²ó OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À° baDist(8.0);¡¡¡¡//8cm¸åÂà c_down(100);¡¡¡¡//¥¢¡¼¥à¤ò³«¤¡¢»æ¥³¥Ã¥×£³¤ò¤Ï¤Ê¤¹ OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À° baDist(2.0);¡¡¡¡//2cm¸åÂà OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À° fwdDist(12.0);¡¡¡¡//12cmÁ°¿Ê }
»æ¥³¥Ã¥×£³¤â£±¤ÈƱÍͤˤ¦¤Þ¤¯Æþ¤ë¤è¤¦¤Ë¾¯¤·Íɤ餹ư¤¤òÆþ¤ì¤Æ¤¤¤ë¡£
¥¹¥Ô¡¼¥É¤ä»þ´Ö¤Ê¤É¤òÄêµÁ¤·¤Æ¤¤¤ë¡£
#define Apower 37 #define Bpower 34 //ľ¿Ê¤¹¤ë¤Î¤ËɬÍפʽÐÎÏ #define ttime 695 //90Åٶʤ¬¤ë¤Î¤ËɬÍפʻþ´Ö #define SPEED 50 #define SPEED_SLOW 30
¥µ¥Ö¥ë¡¼¥Á¥ó¡£
sub go_for()¡¡¡¡//Á°¿Ê { OnFwd(OUT_A,Apower); OnFwd(OUT_B,Bpower); } sub go_back() ¡¡¡¡//¸åÂà { OnFwd(OUT_A,-Apower); OnFwd(OUT_B,-Bpower); } sub turnL(float c)¡¡¡¡//º¸¤Ë¶Ê¤¬¤ë { OnFwd(OUT_A,Apower);OnFwd(OUT_B,-Bpower); Wait(c*ttime); Float(OUT_AB); //º¸¤Ë¶Ê¤¬¤ë¡Êc¡ß90¡ËÅÙ } sub turnR(float c)¡¡¡¡//±¦¤Ë¶Ê¤¬¤ë { OnFwd(OUT_A,-Apower);OnFwd(OUT_B,Bpower); Wait(c*ttime); Float(OUT_AB); //±¦¤Ë¶Ê¤¬¤ë¡Êc¡ß90¡ËÅÙ } int cyari(int lsw)¡¡¡¡//¥¹¥¿¡¼¥È»þ¤Î°ÌÃÖÄ´À° { ResetTachoCount(OUT_C); SetSensorLight(S1); SetSensorMode(S1,SENSOR_MODE_RAW); SetSensorTouch(S4); int i=1; int lsb=SENSOR_1; int lsb2=SENSOR_1-1; while (i==1){ go_for(); lsb2=SENSOR_1; NumOut(80,LCD_LINE4,lsb2); if (lsb>lsb2) { lsb=lsb2; } if (lsb+10<lsb2) { i=10; } if (lsb>lsw-150) { i=1; } } Off(OUT_AB); NumOut(80,LCD_LINE1,lsw); NumOut(80,LCD_LINE2,lsb); return lsb; }
const float diameter=5.45;¡¡¡¡//¥¿¥¤¥ä¤Îľ·Â const float track=11.35;¡¡¡¡//¥¿¥¤¥ä¤Î¥È¥ì¥Ã¥ÉÉý const float pi=3.1415;¡¡¡¡//±ß¼þΨ void fwdDist(float d)¡¡¡¡//µ÷Î¥£äcmÁ°¿Ê { long angle=d/(diameter*pi)*360.0; RotateMotorEx(OUT_AB,SPEED_SLOW,angle,0,true,true); } void turnAng(long ang)¡¡¡¡//³ÑÅÙangÅ٤λþ·×²ó¤ê¤ÎÀû²ó { long angle=track/diameter*ang; RotateMotorEx(OUT_AB,SPEED_SLOW,angle,100,true,true); }
sub arm(int c)¡¡¡¡//¥¢¡¼¥à¤òÆ°¤«¤¹ { RotateMotor(OUT_C, 40,c); Off(OUT_C); }
¥×¥í¥°¥é¥à¤ÎËÜʸ¤Ç¤¢¤ë¡£
task main () { ResetTachoCount(OUT_C); SetSensorLight(S1); SetSensorMode(S1,SENSOR_MODE_RAW); SetSensorTouch(S4); int lsw=SENSOR_1; int lsb=cyari(lsw); while(SENSOR_4==0) {} fwdDist(15); Wait(15000); turnL(1.1);//1 PlaySound(SOUND_UP); Wait(1000); fwdDist(30); Wait(1000); turnAng(40); PlaySound(SOUND_UP); turnAng(180); Wait(1000); fwdDist(15); Wait(1000); arm(550); Wait(1000); go_back(); Wait(2000); Off(OUT_AB); arm(-180); Wait(1000); arm(-45); Wait(1000); turnL(1.05);//2 Wait(1000); arm(-200); fwdDist(35); turnL(0.4); Wait(1000); arm(100); PlaySound(SOUND_UP); SendRemoteNumber(1,MAILBOX1,1); fwdDist(20); while(SENSOR_4==0) {} }
µ¡ÂΣ¤ÎÂç¤Þ¤«¤ÊÆ°¤¤Îή¤ì¤È¤·¤Æ¤Ï¡¢µ¡ÂΣÁ¤¬»æ¥³¥Ã¥×£±¤Î¥Ô¥ó¥Ý¥ó¶Ì¤òXÃÏÅÀ¤Ø¤Ï¤Ê¤¹¤Þ¤Ç¤Ï¡¢¾¯¤·Á°¿Ê¤·¤ÆÂÔµ¡¤·¤Æ¤¤¤ë¡£¤½¤Î¸å¡¢»æ¥³¥Ã¥×£²¤ò¼è¤ê¤Ë¹Ô¤¡¢¥Ô¥ó¥Ý¥ó¶Ì¤òXÃÏÅÀ¤Ç¤Ï¤Ê¤·¤ÆYÃÏÅÀ¤Ë»æ¥³¥Ã¥×£²¤òÃÖ¤¯¡£ºÇ¸å¤Ëµ¡ÂΣÁ¤Ë¿®¹æ¤òÁ÷¤Ã¤Æ½ªÎ»¤È¤¤¤¦¤è¤¦¤Êή¤ì¤Ç¤¢¤ë¡£µ¡ÂÎ£Â¤Î¥×¥í¥°¥é¥à¤Î¾ÜºÙ¤Ë¤Ä¤¤¤Æ¤Ï²¼¤Î¥Ú¡¼¥¸¤ò¤´Í÷¤¯¤À¤µ¤¤¡£
¥×¥í¥°¥é¥à¤¬¤¹¤Ù¤Æ´°àú¤ËÆ°¤¤¤¿¾ì¹ç¤ÎÍýÁۤȤ¹¤ëή¤ì¤Ç¤¢¤ë¡£
¤Þ¤ºµ¡ÂΣÁ¤¬»æ¥³¥Ã¥×£±¤ò¸¡ÃΤ·¤Æ¡¢»æ¥³¥Ã¥×£±¤ÎÁ°¤Ø¿Ê¤à¡£
µ¡ÂΣÁ¤Ï»æ¥³¥Ã¥×£±¤òÄϤࡣµ¡ÂΣ¤Ͼ¯¤·Á°¿Ê¡£
µ¡ÂΣÁ¤Ï»æ¥³¥Ã¥×£±¤òXÃÏÅÀ¤Þ¤Ç±¿¤Ó¡¢»æ¥³¥Ã¥×£±¤ÎÃæ¤Ë¤¢¤Ã¤¿¥Ô¥ó¥Ý¥ó¶Ì¤òXÃÏÅÀ¤ØÃÖ¤¯¡£µ¡ÂΣ¤ÏÂÔµ¡¡£
µ¡ÂΣÁ¤Ï»æ¥³¥Ã¥×£±¤ò»ý¤Ã¤¿¾õÂ֤ǻ楳¥Ã¥×£²¤Î¾åÊý¤Ë¤¢¤ë»æ¥³¥Ã¥×¤ò¸¡ÃΤ·¤Æ¤½¤³¤ÇÄä»ß¤·¤ÆÂÔµ¡¡£µ¡ÂΣ¤ϻ楳¥Ã¥×£²ÄϤࡣ
µ¡ÂΣÁ¤ÏÂÔµ¡¡£µ¡ÂΣ¤ϻ楳¥Ã¥×£²¤òXÃÏÅÀ¤Þ¤Ç±¿¤Ó¡¢»æ¥³¥Ã¥×£²¤ÎÃæ¤Ë¤¢¤Ã¤¿¥Ô¥ó¥Ý¥ó¶Ì¤òXÃÏÅÀ¤ØÃÖ¤¯¡£
µ¡ÂΣÁ¤ÏÂÔµ¡¡£µ¡ÂΣ¤ϻ楳¥Ã¥×£²¤òYÃÏÅÀ¤ØÃÖ¤¡¢µ¡ÂΣÁ¤Ë¿®¹æ¤òÁ÷¤ë¡£
µ¡ÂΣÁ¤Ï¿®¹æ¤ò¼õ¤±¼è¤Ã¤¿¸å¡¢YÃÏÅÀ¤Î»æ¥³¥Ã¥×£²¤ò¸¡ÃΤ·¤Æ¤½¤ÎÁ°¤Þ¤Ç¹Ô¤¡¢»æ¥³¥Ã¥×£²¤Ë½Å¤Í¤Æ»æ¥³¥Ã¥×£±¤òÃÖ¤¯¡£µ¡ÂΣ¤ϼÙËâ¤Ë¤Ê¤é¤Ê¤¤¤è¤¦°ÜÆ°¡£
µ¡ÂΣÁ¤Ï»æ¥³¥Ã¥×£³¤ò¸¡ÃΤ·¤Æ°ÜÆ°¤·¡¢»æ¥³¥Ã¥×£³¤òÄϤࡣ
µ¡ÂΣÁ¤Ï»æ¥³¥Ã¥×£³¤òXÃÏÅÀ¤Þ¤Ç±¿¤Ó¡¢»æ¥³¥Ã¥×£³¤ÎÃæ¤Ë¤¢¤Ã¤¿¥Ô¥ó¥Ý¥ó¶Ì¤òXÃÏÅÀ¤ØÃÖ¤¯¡£
µ¡ÂΣÁ¤ÏYÃÏÅÀ¤Î»æ¥³¥Ã¥×£±µÚ¤Ó£²¤ò¸¡ÃΤ·¤Æ¤½¤ÎÁ°¤Þ¤Ç¹Ô¤¡¢»æ¥³¥Ã¥×£±¤Î¾å¤Ë½Å¤Í¤Æ»æ¥³¥Ã¥×£³¤òÃÖ¤¯¡£
¼ÂºÝ¤Ï»æ¥³¥Ã¥×£±¤Î¥Ô¥ó¥Ý¥ó¤òÃÖ¤¯¤È¤³¤í¤Þ¤Ç¤Ï¤¦¤Þ¤¯¤¤¤¡¢»æ¥³¥Ã¥×£²¤Î¤È¤³¤í¤Ç¤¦¤Þ¤¯¤¤¤«¤º¤½¤³¤Þ¤Ç¤Ë¤Ê¤Ã¤Æ¤·¤Þ¤¦¤³¤È¤¬Â¿¤«¤Ã¤¿¡£Îý½¬¤Ç¤â»æ¥³¥Ã¥×£±¤È£²¤ÎÄ´À°¤Ë»þ´Ö¤¬¤«¤«¤ê»æ¥³¥Ã¥×£³¤Ï¤Û¤È¤ó¤É¼ê¤ò¤Ä¤±¤é¤ì¤Ê¤«¤Ã¤¿¡£
ËÜÈ֤Ǥϡ¢¼Â¤Ïµ¡ÂΣ¤¬ºÇ½é¤Ë»æ¥³¥Ã¥×£²¤ò¼è¤ê¤Ë¹Ô¤¡¢¤½¤Î¸åµ¡ÂΣÁ¤¬Æ°¤½Ð¤¹¥×¥í¥°¥é¥à¤Ç¤¢¤Ã¤¿¡£¤·¤«¤·¡¢µ¡ÂΣ¤ΤȤì¤Ê¤«¤Ã¤¿»æ¥³¥Ã¥×¤ä¥Ô¥ó¥Ý¥ó¶Ì¤¬µ¡ÂΣÁ¤Î¾ã³²¤È¤Ê¤Ã¤Æ¤·¤Þ¤¤µ¡ÂΣÁ¤â¤¦¤Þ¤¯¤¤¤«¤º¡¢·ë²Ì£°ÅÀ¤È¤Ê¤Ã¤Æ¤·¤Þ¤Ã¤¿¡£ºÆÅÙ¤ÎÄ©Àï¤Î»þ¤Ï¡¢¾¯¤·¤Ç¤âÅÀ¤ò¼è¤ê¤Ë¹Ô¤¯¤³¤È¤ò¹Í¤¨¤Æ¾åµ¤Î¤è¤¦¤ËÈæ³ÓŪÀ®¸ùΨ¤Î¹â¤«¤Ã¤¿µ¡ÂΣÁ¤òÀè¤ËÆ°¤«¤·¤¿¡£·ë²Ì£µÅÀ¤ò³ÍÆÀ¤¹¤ë¤³¤È¤¬½ÐÍ褿¡£¤É¤Á¤é¤«¤¬¼ºÇÔ¤·¤Æ¤·¤Þ¤¦¤È¡¢¤â¤¦°ìÊý¤â¤À¤á¤Ë¤Ê¤Ã¤Æ¤·¤Þ¤¦¤è¤¦¤Ê¥×¥í¥°¥é¥à¤À¤Ã¤¿¤³¤È¤¬È¿¾ÊÅÀ¤À¤È´¶¤¸¤¿¡£¤â¤Ã¤È¤è¤ê¤¦¤Þ¤¯£²µ¡¤ÎÏ¢·È¤Î¤È¤ì¤¿¥×¥í¥°¥é¥à¤Ë½ÐÍ褿¤Î¤Ç¤Ï¤Ê¤¤¤«¤È´¶¤¸¤¿¡£
º£²ó¤Ï¡¢£´¿Í¤Ç¹Ô¤¦²ÝÂê¤Ç¥í¥Ü¥Ã¥È¤â£²Âæ¤À¤Ã¤¿¤Î¤Ç¤¦¤Þ¤¯Ê¬Ã´¤·¤Æ¶¨ÎϤ·¤Æ²ÝÂê¤Ë¼è¤êÁȤ᤿¡£¸Â¤é¤ì¤¿»þ´Ö¤·¤«¤Ê¤«¤Ã¤¿¤Î¤Ç¡¢¤·¤Ã¤«¤ê·×²èÀ¤ò»ý¤Ã¤Æ¤ß¤ó¤Ê¤Ç³Îǧ¤·¤Ê¤¬¤é¼è¤êÁȤó¤Ç¤¤¤¯¤³¤È¤¬Âç»ö¤À¤È´¶¤¸¤¿¡£»þ´Ö¤â¤¢¤Þ¤ê¤Ê¤¯¾Ç¤ê¤â¤¢¤Ã¤¿¤¬¡¢ºÇ¸å¤ÎºÇ¸å¤Þ¤Ç¤¢¤¤é¤á¤º¤Ë¤ä¤ì¤¿¤Î¤ÏÎɤ«¤Ã¤¿¤È»×¤¦¡£¾åµ¤Ë¤¢¤ë¤è¤¦¤ÊÍýÁۤηÁ¤Ë¤Ï¤Ê¤é¤Ê¤«¤Ã¤¿¤¬¡¢²ÝÂê¤Ø¤Î¼è¤êÁȤߤϳڤ·¤«¤Ã¤¿¡£¥á¥ó¥Ð¡¼¤Î¤ß¤ó¤Ê¤Ë¤ÏËÜÅö¤Ë´¶¼Õ¤·¤¿¤¤¤È»×¤¦¡£