Ìܼ¡ ²ÝÂê3 †¾åµ¤Î¥³¡¼¥¹¾å¤ÎÀĤΥܡ¼¥ë¤ÏÀĤδ̤ˡ¢À֤Υܡ¼¥ë¤ÏÀ֤δ̤ˤ½¤ì¤¾¤ì¾è¤»¤ë¡£´Ì¤Ï¥í¥Ü¥Ã¥Èµ¯Æ°»þ¤Ë¥µ¥¤¥³¥í¤ò¿¶¤ê¡¢¥µ¥¤¥³¥í¤Î½Ð¤¿ÌܤËÂбþ¤·¤¿°ÌÃÖ¤ËÃÖ¤¯¡£¥Ü¡¼¥ë¤òƱ¤¸ÎΰèÆâ¤Î´Ì¤Ë¾è¤»¤¿¤«¤É¤¦¤«¡¢¥Ü¡¼¥ë¤¬´Ì¤ËÅö¤¿¤Ã¤¿¤«¡¢¥À¥ß¡¼´Ì¤ò»ÈÍѤ·¤¿¤«¤Ë¤è¤Ã¤ÆÅÀ¿ô¤¬ÉÕ¤¯¡£ »ÈÍѥ롼¥È †¼«Ê¬¤¿¤Á¤Î¥°¥ë¡¼¥×¤Ï°Ê²¼¤Î¤è¤¦¤Ê¥ë¡¼¥È¡¦¼ê½ç¤ò¹Í¤¨¤¿¡£
ºîÀ®¥í¥Ü¥Ã¥È †¥í¥Ü¥Ã¥ÈÁ´ÂÎÁü †º£²ó¤Ï2¥°¥ë¡¼¥×¹çƱ¤Ç¤Î¥í¥Ü¥Ã¥ÈºîÀ®¤À¤Ã¤¿¤Î¤ÇNXT¤¬2Âæ¤¢¤ê¡¢¤½¤ÎÊÒÊý¤Ç¥Ü¡¼¥ë¤òÄϤó¤Ç»ý¤Á¾å¤²¤ë¥¢¡¼¥àÉôʬ¤ÎÁàºî¡¢¤â¤¦ÊÒÊý¤Ç¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò¹Ô¤¦¥¿¥¤¥ä¤ÎÁàºî¤ò¹Ô¤Ã¤¿¡£ ¥¿¥¤¥ä¤ÎÁàºî¡¢³Æ¥»¥ó¥µ¤ÎÃͤÎǧ¼±¤ò¹Ô¤¦NXT¤ò¥Þ¥¹¥¿¡¼Â¦¡¢¥¢¡¼¥à¤ÎÁàºî¤ò¹Ô¤¦NXT¤ò¥¹¥ì¡¼¥Ö¦¤Ë¤·¤Æ¤¤¤ë¡£
±¦¤Î¥â¡¼¥¿¡¼¤«¤é¥¿¥¤¥ä¤Ø¤Î¥®¥¢¤Î¤Ä¤Ê¤¬¤ê¤òÌϼ°Åª¤Ëɽ¤¹¤È°Ê²¼¤Î¤è¤¦¤Ë¤Ê¤Ã¤Æ¤¤¤ë¡£ ¾åµ¤Î¿Þ¤Ë¤ª¤¤¤Æ¥â¡¼¥¿¡¼¤È¥®¥¢A¡¢¥®¥¢B¤È¥®¥¢C¡¢¥®¥¢D¤È¥¿¥¤¥ä¤ÏËÀ¤Ç¤Ä¤Ê¤¬¤Ã¤Æ¤ª¤ê²óž¤ÏϢư¤¹¤ë¡£¤½¤Î¤¿¤á¥â¡¼¥¿¡¼¤ò²óž¤µ¤»¤ë¤È¥®¥¢A¤¬Ï¢Æ°¤·¤Æ²óž¤·¡¢¤½¤Î²óž¤¬¥®¥¢B¤ËÅÁ¤ï¤ë¤³¤È¤Ç¥®¥¢C¤â²óž¤·¡¢¥®¥¢D¤ò²óž¤µ¤»¥¿¥¤¥ä¤¬²óž¤¹¤ë¡¢¤È¤¤¤¦Î®¤ì¤ÇưÎϤ¬ÅÁ¤ï¤ë¡£¤Ê¤ª¥â¡¼¥¿¡¼¤È¥¿¥¤¥ä¤Î²óÅ¾Èæ¤Ï6ÂÐ1¤Ç¤¢¤ë¡£ ¥Ü¡¼¥ëÊÝ»ýÉôʬ †°Ê²¼¥Ü¡¼¥ë¤ò¤Ä¤«¤àÉôʬ¤ò¥Ï¥ó¥É¡¢¥Ü¡¼¥ë¤ò»ý¤Á¾å¤²¤ëÉôʬ¤ò¥¢¡¼¥à¤È¸Æ¾Î¤¹¤ë¡£ ¥Ï¥ó¥É †¥Ï¥ó¥ÉÉôʬ¤Ï¾å¤Î¥®¥¢¤ò²óž¤µ¤»¤ë¤³¤È¤Ç²¼¤Î¥®¥¢¤ËưÎϤ¬ÅÁ¤ï¤ê³«ÊÄÆ°ºî¤ò¹Ô¤¦¡£ ¥¢¡¼¥à †¥â¡¼¥¿¡¼A¤Î²óž¤òÜô´ÝÉôʬ¤Î·×5¤Ä¤Î¥®¥¢¤Ë¤è¤Ã¤Æ¥Ï¥ó¥ÉÉôʬ¤ËÅÁ¤¨¡¢¥Ï¥ó¥É¤Î³«ÊĤò¹Ô¤¦¡£
²¼¤Î¼Ì¿¿¤ÎÎÐÌð°õ¤ÎÊý¸þ¤Ï¥Ï¥ó¥É¤òÊĤ¸¤ëÊý¸þ¤Ç¤¢¤ë¡£ ¥â¡¼¥¿¡¼B¡¢C¤òƱ»þ¤ËƱ¤¸Êý¸þ¤Ø²óž¤µ¤»¤ë¤³¤È¤Ç¡¢¥¢¡¼¥à¤ò¾å²¼¤µ¤»¤ë¡£¤Ê¤ª¥¢¡¼¥àÉôʬ¤Î½ÅÎ̤âÂ礤¤¤¿¤á¥â¡¼¥¿¡¼¤ò2¸Ä»È¤¤¡¢¤«¤«¤ë½ÅÎ̤òʬ»¶¤·¤Æ¤¤¤ë¡£ ¿Þ¤Î¤è¤¦¤Ë²¼¤ÎÜôËÀ¤ò¥â¡¼¥¿¡¼¤Î²óž¤Ë¤è¤Ã¤Æ·¹¤¤òÊѤ¨¤ë¤³¤È¤Ç¡¢¥Ï¥ó¥É¤¬·Ò¤¬¤Ã¤Æ¤¤¤ë½ÄËÀ¤¬¾å¤¬¤ê¥Ü¡¼¥ë¤â¾å¤¬¤ë¡£ ¥Ï¥ó¥É¤È¥¢¡¼¥à¤ÎÀܹçÉôʬ †¥â¡¼¥¿¡¼A¤«¤é¥¢¡¼¥à¤òÅÁ¤ï¤Ã¤Æ¤¤¿½ÄÊý¸þ¤Î²óž¤Ï¡¢¥Ï¥ó¥É¤Î¥®¥¢¤Ë¤è¤ê²£²óž¤ËÊÑ´¹¤µ¤ì³«ÊÄ¤ÎÆ°ºî¤ò¹Ô¤¦¡£ ³Æ¼ï¥»¥ó¥µ †º£²ó¤ÏͲ»ÇÈ¥»¥ó¥µ¤ò2¤Ä¡¢¸÷¥»¥ó¥µ¤ò1¤Ä»ÈÍѤ·¤¿¡£S1¤Ç¥é¥¤¥ó¤ÎÌÀ°Å¤ò¬Äê¡¢S2¤Ç´Ì¤Îµ÷Î¥¤ò¬Äê¡¢S3¤Ç¥Ü¡¼¥ë¤Îµ÷Î¥¤ò¬Äꤹ¤ë¡£ »î¹Ôºø¸í †¥í¥Ü¥Ã¥ÈºîÀ®Åö½é¤Ï¥¢¡¼¥à¤¬Ä¹¤¯¡¢Á°ÎØ¥¿¥¤¥ä¤¬¼ÖÂΤÎÁ°Êý¤ä¤ä³°Â¦¤Ë¤Ä¤¤¤Æ¤ª¤ê¡¢¥Ð¥é¥ó¥¹¤¬°¤«¤Ã¤¿¡£¤½¤Î¤¿¤á²þÁ±¤È¤·¤Æ¥¢¡¼¥à¤òû¤¯¤·¡¢Á°ÎØ¥¿¥¤¥ä¤òÃæ±û´ó¤ê¤Ë¼è¤êÉÕ¤±¤¿¤â¤Î¤ò´°À®ÉʤȤ·¤¿¡£ ưºîÀâÌÀ †º£²ó¤Î²ÝÂê¤ÏÁ°²ó¤Î²ÝÂê¤ÈÎà»÷ÅÀ¤¬Â¿¤¯Â¸ºß¤¹¤ë¤¿¤áÎà»÷ÅÀ¤Ï´Êά²½¤·¡¢Áê°ãÅÀ¤ò¾Ü¤·¤¯ÀâÌÀ¤¹¤ë¡£Îà»÷ÅÀ¤Î¾Ü¤·¤¤ÀâÌÀ¤ÏÁ°²ó(Mission2)¤ÎưºîÀâÌÀ¤ò»²¾È¡£
NXT´Ö¤ÎÄÌ¿® †º£²ó¤Ï2¤Ä¤ÎNXT¤ò»ÈÍѤ·¤Æ¤¤¤ë¤Î¤Ç¡¢ÊÒÊý¤ò¥Þ¥¹¥¿¡¼¤È¤·¤â¤¦ÊÒÊý¤Î¥¹¥ì¡¼¥Ö¦¤òBluetooth¤Ç·Ò¤²¡¢Ï¢·È¤µ¤»¤Æ¤¤¤ë¡£º£²ó¤Ï¥¹¥ì¡¼¥Ö¦¤Ë¡Ð¥¢¡¼¥à¤ò¾å¤²¤ë¡Ñ¡Ð¥¢¡¼¥à¤ò²¼¤²¤ë¡Ñ¡Ð¥Ü¡¼¥ë¤òÄϤà¡Ñ¡Ð¥Ü¡¼¥ë¤òÊü¤¹¡Ñ¤Î4¤Ä¤Îưºî¤ò¹Ô¤¦¥×¥í¥°¥é¥à¤òÆþ¤ì¤Æ¤¤¤ë¡£¥Þ¥¹¥¿¡¼Â¦¤«¤é [RemoteStartProgram(CONN,"¥¹¥ì¡¼¥Ö¦¤Î¥×¥í¥°¥é¥à̾.rxe");] ¤òÍѤ¤¤ÆÌ¿Îá¤òÁ÷¤ë¤³¤È¤Ç¡¢¥¹¥ì¡¼¥Ö¦¤Ë»ØÄꤷ¤¿¥×¥í¥°¥é¥à̾¤Îưºî¤ò¹Ô¤ï¤»¤ë¡£ ¥é¥¤¥ó¥È¥ì¡¼¥¹ †º£²ó¤âÁ°²ó¤ÈƱÍͤËÀþ¤Î¶Ìܤò¥È¥ì¡¼¥¹¤¹¤ë¥×¥í¥°¥é¥à¤Ç¤¢¤ë¤¬¡¢¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò¹Ô¤¦ÄêµÁ´Ø¿ô follow_line(int bw_time,long min_t,int blk_cnt,string invert_info,string sensor_switch) ¤Ë¤ª¤¤¤Æ5¤Ä¤Î°ú¿ô¤òÁȤ߹þ¤à¤³¤È¤Ç¡¢ÊÑ¿ô¤ÎÃͤˤè¤Ã¤Æ¹Ô¤ï¤ì¤ëưºî¤òÊѹ¹¤·¤Æ¤¤¤ë¡£°Ê²¼¤Ë5¤Ä¤ÎÊÑ¿ô¤Ë¤è¤ëưºî¤ÎÊѹ¹ÅÀ¤ò¤Þ¤È¤á¤ë¡£¤Ê¤ªÁȤ߹þ¤ó¤Ç¤¢¤ë¤À¤±¤Çº£²ó¤Î²ÝÂê¤Ë¤ª¤¤¤Æ¤Ï»ÈÍѤ·¤Æ¤ª¤é¤º¡¢Æ°ºî¤ÎÊѹ¹¤ò¹Ô¤Ã¤Æ¤¤¤Ê¤¤´Ø¿ô¤â¤¢¤ë¡£
Àþ¤Î¶ÌܤΥȥ졼¥¹ÊýË¡¤ÏÁ°²óƱÍÍ¡ÐÀþ³°¤Ê¤é¤ÐÀþ¾å¤Ë¸þ¤«¤¦¡Ñ¡ÐÀþ¤Î¶Ìܤʤé¤Ðľ¿Ê¡Ñ¡ÐÀþ¾å¤Ê¤é¤ÐÀþ³°¤Ë¸þ¤«¤¦¡Ñ¤Î3¤Ä¤Îưºî¤ò¸÷¥»¥ó¥µ¤ÎÃͤˤè¤Ã¤ÆÀ©¸æ¤¹¤ë¤³¤È¤Ç¹Ô¤¤¡¢¸òº¹ÅÀ¡¦µÞ¥«¡¼¥Ö¤âƱÍͤËȽÃǤ¹¤ë¡£ ´Ì¤Îº÷Ũ †´Ì¤Îº÷Ũ¤Ë´Ø¤·¤Æ¤ÏÁê°ãÅÀ¤È¤·¤Æ¥¿¥¤¥ä´Ö¤Îµ÷Î¥¤Èº÷ŨÈϰϤÎÊѹ¹¤¬¤¢¤ë¡£ ¥¿¥¤¥ä´Ö¤Îµ÷Î¥¤Ïñ½ã¤Ë¥í¥Ü¥Ã¥È¤¬ÊѤï¤ê¡¢Á°²ó¤è¤êÂ礤¯¤Ê¤Ã¤¿¤¿¤áŤ¯¤Ê¤Ã¤¿¡£ º÷ŨÈϰϤÏÁ°²ó¤È°ã¤¤360ÅÙ°ì²óž¤·¤Ê¤¯¤Æ¤â¤è¤«¤Ã¤¿¤¿¤á¡¢¥é¥¤¥ó¥È¥ì¡¼¥¹¤ÈƱÍͤ˴̤òº÷Ũ¤¹¤ëÄêµÁ´Ø¿ô search_throw(string side_select,float rotate_angle) ¤Ë°ú¿ô¤òÁȤ߹þ¤ß¡¢Àû²óÊý¸þ¤ÈÀû²ó³ÑÅÙ¤òÁàºî¤Ç¤¤ë¤è¤¦¤Ë¤·¤¿¡£
Àû²ó³ÑÅÙ¤ò»»½Ð¤¹¤ë¼°¤ÏÁ°²óƱÍÍ (¥¿¥¤¥äľ·Â/Á°Îش֤ε÷Î¥)*¥¿¥¤¥ä¤Î²óž³ÑÅÙ=¥í¥Ü¥Ã¥È¤ÎÀû²ó³ÑÅÙ ¤Ç¤¢¤ë¤¬¡¢º£²ó¤Ï¥â¡¼¥¿¡¼¤È¥¿¥¤¥ä¤Î´Ö¤Ë¥®¥¢¤ò¤«¤Þ¤»¤Æ¤ª¤ê²óÅ¾Èæ¤Ï1ÂÐ6¤Î¤¿¤á¡¢¥¿¥¤¥ä¤Î²óž³Ñ¤Ï ¥â¡¼¥¿¡¼¤Î²óž³Ñ/6 ¤Ç»»½Ð¤µ¤ì¤ë¡£ ưºî»þ´Ö¤Î¬Äê †Æ°ºî»þ´Ö¤ÎÁÛÄê¤Ë´Ø¤·¤Æ¤ÏÁ°²ó(Mission2)¤È¤Û¤ÜƱ°ì¤Ç¤¢¤êÊÑ¿ô̾¤¬°Û¤Ê¤ë¤¯¤é¤¤¤Ç¤¢¤ë¡£Á°²ó¤Ît00¤¬º£²ó¤Îstart_t¤ËÂбþ¤·¡¢t0,t1¤Ë´Ø¤·¤Æ¤Ï¤½¤Î¤Þ¤ÞÂбþ¤¹¤ë¡£¤è¤Ã¤ÆÆ±ÍͤˡÐt1-t0¡Ñ¤Ï¥í¥Ü¥Ã¥È¤¬±¦º¸ÀÞ(Àû²ó¤ò´Þ¤à)¤ò·Ñ³¤·¤Æ¹Ô¤Ã¤Æ¤¤¤ë»þ´Ö¤ò¼¨¤·¡¢¡Ðstart_t-t1¡Ñ¤Ï¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò³«»Ï¤·¤Æ¤«¤é²¿Éôַвᤷ¤¿¤«¤ò¼¨¤¹¡£ »ÈÍÑ¥×¥í¥°¥é¥à †¥¹¥ì¡¼¥Ö¦ †
task main(){ RotateMotor(OUT_BC,-40,90); //¥Ü¡¼¥ë¤ò»ý¤Á¾å¤²¤ë }
task main(){ RotateMotor(OUT_BC,10,90); Wait(300); Off(OUT_BC); ResetTachoCount(OUT_BC); }
task main(){ RotateMotor(OUT_A,-30,320); //¥Ü¡¼¥ë¤òÄϤà Wait(300); RotateMotor(OUT_BC,-40,15); Off(OUT_ABC); } ¥Ï¥ó¥É¤òÊĤ¸¤ë¡Ê¥Ü¡¼¥ë¤òÄϤà¡Ë¤È¤Ê¤¼¤«¥¢¡¼¥à¤¬²¼¤¬¤Ã¤Æ¤·¤Þ¤¤¡¢¥Ï¥ó¥ÉÉôʬ¤¬ÃÏÌ̤ˤ³¤¹¤ì¤Æ¤·¤Þ¤¦¤Î¤Ç¾¯¤·¥¢¡¼¥à¤ò¾å¤²¤Æ¤¤¤ë¡£
task main(){ RotateMotor(OUT_A,30,320); //¥Ü¡¼¥ë¤òÊü¤¹ } ¥¹¥ì¡¼¥Ö¦¤Ë¤Ï¾åµ¤Îñ½ã¤Ê4¤Ä¤Î¥×¥í¥°¥é¥à¤¬Æþ¤Ã¤Æ¤¤¤ë¡£ ¥Þ¥¹¥¿¡¼Â¦ †
NXT´Ö¤ÎÄÌ¿® †until(BluetoothStatus(CONN)==NO_ERR); //¥¹¥ì¡¼¥Ö¦¤Î¥¢¡¼¥à¤ò¾å¤²¤ë¥×¥í¥°¥é¥à¤òµ¯Æ° RemoteStartProgram(CONN,"arm_up.rxe"); Wait(4000); until(BluetoothStatus(CONN)==NO_ERR); //¥¹¥ì¡¼¥Ö¦¤Î¥¢¡¼¥à¤ò²¼¤²¤ë¥×¥í¥°¥é¥à¤òµ¯Æ° RemoteStartProgram(CONN,"arm_down.rxe"); Wait(4000); until(BluetoothStatus(CONN)==NO_ERR); //¥¹¥ì¡¼¥Ö¦¤Î¥Ï¥ó¥É¤òÊĤ¸¤ë¥×¥í¥°¥é¥à¤òµ¯Æ° RemoteStartProgram(CONN,"hand_close.rxe"); Wait(4000); until(BluetoothStatus(CONN)==NO_ERR); //¥¹¥ì¡¼¥Ö¦¤Î¥Ï¥ó¥É¤ò³«¤±¤ë¥×¥í¥°¥é¥à¤òµ¯Æ° RemoteStartProgram(CONN,"hand_open.rxe");¡¡ Wait(4000); [until(BluetoothStatus(CONN)==NO_ERR);]¤ÏNXTƱ»Î¤¬·Ò¤¬¤Ã¤Æ¤¤¤Ê¤¤¤È°Ê²¼¤Îưºî¤ò¹Ô¤ï¤Ê¤¤¤è¤¦¤ËÀ©¸Â¤¹¤ë¾ò·ïʸ¤Ç¤¢¤ë¡£ ¥é¥¤¥ó¥È¥ì¡¼¥¹ †
void follow_line(int bw_time,long min_t,int blk_cnt,string invert_info,string sensor_switch){ SetSensorLight(S1); //¸÷¥»¥ó¥µÄêµÁ int right_var; //¸÷¥»¥ó¥µÃÍÍÑÊÑ¿ô SetSensorLowspeed(S3); //Ͳ»ÇÈ¥»¥ó¥µÄêµÁ long t0,t1,start_t; // t0,t1,start_t »þ´ÖÊÑ¿ô¤ÎÄêµÁ t1=CurrentTick(); //¡¡º£¤½¤Î»þ¤Î»þ¹ï t0=CurrentTick(); //¡¡Ä¾¿Ê½ªÎ»¡¦±¦º¸ÀÞ³«»Ï»þ¹ï start_t=CurrentTick(); // ¥é¥¤¥ó¥È¥ì¡¼¥¹³«»Ï»þ¹ï while(t1-start_t<min_t || min_t<0){¡¡//¥é¥¤¥ó¥È¥ì¡¼¥¹³«»Ï¤«¤é»ØÄꤷ¤¿»þ´Ö[min_t]°ÊÆâ¤Ç·«¤êÊÖ¤¹ if(sensor_switch=="on"){ //sensor_switch¤Ëon¤ò»ØÄꤷ¤Æ¤¤¤¿¤é if(SensorUS(S3)<16){ //S3¤¬16Ñ°ÊÆâ¤Ë¾ã³²Êª¤òõÃΤ·¤¿¤é break;¡¡¡¡¡¡¡¡¡¡ //whileʸ¤«¤éÈ´¤±½Ð¤¹ ¡¡¡¡¡¡ } } right_var = SENSOR_1; // ¸÷¥»¥ó¥µÃÍÂåÆþ if(invert_info=="on"){ //invert_info¤Ëoff¤ò»ØÄꤷ¤¿¤éº¸¤Î¶Ìܤò¥È¥ì¡¼¥¹¤¹¤ë¡¡ right_var = right_var*(-1)+91;} //on¤ò»ØÄꤷ¤¿¤é¸÷¥»¥ó¥µ¤ÎÃͤòȿž¤µ¤»¤Æ±¦¤Î¶Ìܤò¥È¥ì¡¼¥¹ if (right_var>50){ if (t1-t0>bw_time){ //¡¡±¦²ó¤Ç»ØÄꤷ¤¿»þ´Ö[bw_time]°Ê¾å¤Î¤È¤±¦Àû²ó if (blk_cnt>=0 && blk_cnt<=2 && invert_info=="on"){¡¡//¸òº¹ÅÀ¤ÎÄ̲᤬Ää»ß¤«Ä̲á¤Ç¤¢¤ê¡¢±¦¤Î¶Ìܤò¥È¥ì¡¼¥¹¤·¤Æ¤¤¤¿¤Ê¤é blk_selection(blk_cnt); //blk_cnt¤Ë»ØÄꤵ¤ì¤¿¿ô»ú¤ËŬ¤·¤¿Æ°ºî¤ò¹Ô¤¤ break; //¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò½ªÎ»¤¹¤ë }else{ rrt;} }else{ //¡¡±¦²ó rt;} }else if (right_var>40){ fw; t0=CurrentTick(); //ľ¿Ê½ªÎ»¡¦±¦º¸ÀÞ³«»Ï»þ¹ï¡¡ }else{ if (t1-t0>bw_time){ //¡¡º¸²ó¤Ç bw_time[ms] °Ê¾å¤Î¤È¤º¸Àû²ó if (blk_cnt>=0 && blk_cnt<=2 && invert_info!="on") { blk_selection(blk_cnt); break; }else{ llt;} }else{ //¡¡º¸²ó lt;}} t1=CurrentTick(); //º£¤½¤Î»þ¤Î»þ¹ï }Off(OUT_BC); } ´Ì¤Îº÷Ũ †void search_throw(string side_select,float rotate_angle){ SetSensorLowspeed(S2); //Ͳ»ÇÈ¥»¥ó¥µÄêµÁ int d_min=1000,d_gosa=11,side_flag=0; long ang_min=0,angle=0,d_angle=0; if (side_select=="right"){ //right¤ò»ØÄꤷ¤Æ¤¤¤ì¤Ð±¦Àû²ó side_flag=1; }else if(side_select=="left"){ //left¤ò»ØÄꤷ¤Æ¤¤¤ì¤Ðº¸Àû²ó side_flag=-1; } ResetTachoCount(OUT_BC); OnFwdSync(OUT_BC,30,100*side_flag); while(((tire_diameter/(tire_axis*gear_ratio))*MotorTachoCount(OUT_C)*side_flag)<=rotate_angle){ //»ØÄꤷ¤¿³ÑÅÙ[rotate_angle]ʬÀû²ó if(d_min>SensorUS(S2)){ d_min=SensorUS(S2); //µ÷Î¥¤ÎºÇ¾®Ã͹¹¿· angle=MotorTachoCount(OUT_C); } } ang_min=MotorTachoCount(OUT_C)-angle; Off(OUT_BC); Wait(1000); ResetTachoCount(OUT_BC); RotateMotorEx(OUT_BC, 40, ang_min, -100*side_flag, true, true);//ʪÂΤÎÊý¤ò¸þ¤¯ Off(OUT_BC); ResetTachoCount(OUT_BC); if(7<SensorUS(S2)){¡¡¡¡//´Ì¤Þ¤Ç¤Îµ÷Î¥¤¬7Ѱʲ¼¤Ë¤Ê¤é OnFwd(OUT_BC,40); //ʪÂΤÎÊý¤Ë¶á¤Å¤¯ while(7<SensorUS(S2) || 40>(pi*tire_diameter*MotorTachoCount(OUT_C))/en*gear_ratio){µ÷Î¥¤¬7Ѱʲ¼¤Ë¤Ê¤ë¤Þ¤¿¤Ï40ѿʤà¤Þ¤Ç d_angle=MotorTachoCount(OUT_C); //ʪÂΤ˶á¤Å¤¤¤¿Ê¬¤Î¥â¡¼¥¿¡¼¤Î²óž³Ñ¤òµÏ¿ } Off(OUT_BC); RotateMotor(OUT_BC,30,en*2*gear_ratio/(pi*tire_diameter)); } def_arm_up;¡¡¡¡//¥¢¡¼¥à¤ò¾å¤²¤ë RotateMotor(OUT_BC,30,en*d_gosa*gear_ratio/(pi*tire_diameter));¡¡¡¡//µ÷Î¥¤ÎÈùÄ´À° Wait(700); def_hand_open;¡¡¡¡//¥Ü¡¼¥ë¤òÊü¤¹ RotateMotor(OUT_BC,-30,en*d_gosa*gear_ratio/(pi*tire_diameter)+d_angle);¡¡¡¡//¶á¤Å¤¤¤¿Ê¬Ìá¤ë def_arm_down;¡¡¡¡//¥¢¡¼¥à¤ò²¼¤²¤ë ResetTachoCount(OUT_BC); RotateMotorEx(OUT_BC, 40, angle, -100*side_flag, true, true);//¸µ¤Î¸þ¤¤ØÌ᤹ Off(OUT_BC); } ¥×¥í¥°¥é¥àÁ´ÂÎ †task main(){ string invert_str="off"; int b_tyokusin=10,r_back=22;¡¡//ľ¿Ê¤¹¤ëµ÷Î¥¤È¸åÂह¤ëµ÷Î¥¤òÄêµÁ follow_line(1200, -1,1,invert_str,"off");¡¡//Y¤«¤é½Ð¤ÆÀĥܡ¼¥ë¤Ø¸þ¤«¤¦ follow_line(800, -1,0,invert_str,"off"); def_hand_close;¡¡//Àĥܡ¼¥ë¤òÄϤà ResetTachoCount(OUT_BC); RotateMotor(OUT_BC,30,en*b_tyokusin*gear_ratio/(pi*tire_diameter)); //±ß¤ÎÃæ¤ËÆþ¤ë Off(OUT_BC); search_throw("right",55.0);¡¡//±¦Àû²ó³ÑÅÙ55Å٤Ǻ÷Ũ Wait(2000); RotateMotor(OUT_BC,-30,en*b_tyokusin*gear_ratio/(pi*tire_diameter));¡¡//±ß¤«¤é½Ð¤ë Off(OUT_BC); def_arm_up;¡¡//¥¢¡¼¥à¤ò¾å¤²¤ë RotateMotor(OUT_BC,-30,en*r_back*gear_ratio/(pi*tire_diameter));¡¡//E¤Î¸òº¹ÅÀ¤Þ¤Ç¸åÂह¤ë follow_line(600,1200000,-1,invert_str,"on"); //E¤«¤éF¤Ø¸þ¤«¤¤µÞ¥«¡¼¥Ö¤Ø¹Ô¤¡¢À֥ܡ¼¥ë¤òǧ¼±¤·¤¿¤é¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò½ª¤¨¤ë RotateMotor(OUT_BC,-30,en*3*gear_ratio/(pi*tire_diameter));¡¡//3¥»¥ó¥Á²¼¤¬¤ë def_arm_down; //¥¢¡¼¥à¤ò²¼¤²¤ë def_hand_close; //À֥ܡ¼¥ë¤òÄϤà search_throw("left",270);¡¡//º¸Àû²ó270Å٤Ǻ÷Ũ Wait(1000);Off(OUT_BC); } ·ë²Ì †¼«Ê¬¤¿¤Á¤Î¥°¥ë¡¼¥×¡ÊN1N2¡Ë¤ÏËÜÈ֤Ǥϴ̤κ÷Ũ¤¬¤º¤ì¤ë¡¢¥Ï¥ó¥É¤¬³«¤«¤Ê¤¤¤Ê¤É¤ÎÉÔ¶ñ¹ç¤¬½Ð¤Æ¡¢´Ì¤ËÅö¤Æ¤ë¤³¤È¤¹¤é¤Ç¤¤º´ðËÜÅÀ¤Ï0ÅÀ¤À¤Ã¤¿¡£¤·¤«¤·µ»½ÑÅÀ¤Ï¿¤¯Ì㤨ºÇ½ª·ë²Ì¤È¤·¤Æ¤Ï3°Ì¤Ç¤¢¤Ã¤¿¡£ ´¶ÁÛ¡¦È¿¾Ê †¥í¥Ü¥Ã¥ÈËÜÂΤ˴ؤ·¤Æ¤ÏÂ礤¯¡¢½Å¤¯¤Ê¤Ã¤Æ¤·¤Þ¤Ã¤¿¤¬¡¢¤½¤ì¤Ë¤è¤ë°±Æ¶Á¤ò´ËϤǤ¤ë¤è¤¦¤Ê¹©Éפò¹Ô¤¨¡¢µ»½ÑÅÀ¤â¿¤¯Ì㤨¤¿¤Î¤ÇÎɤ¤¤Û¤¦¤Ç¤¢¤ë¤è¤¦¤Ë´¶¤¸¤ë¡£¤·¤«¤·º÷Ũ³ÑÅ٤Υº¥ì¤Ï´Ì¤¬±ó¤¤¤È¤³¤í¤Ë¤¢¤ë¤³¤È¤Ç¡¢Ä¶²»ÇÈ¥»¥ó¥µ¤ÎÈ¿¼Í¤Ë»þ´Ö¤¬¤«¤«¤Ã¤¿¤³¤È¤Ë¤è¤ë¤â¤Î¤À¤È¹Í¤¨¤é¤ì¤ë¡£¼ÂºÝ»î±¿Å¾¤Î»þ¤â¡¢´Ì¤¬1¡¤2¡¤3¤Ê¤É¤ÎÈæ³ÓŪ¶á¤¤°ÌÃ֤ξì¹ç¤Ï¤Û¤Ü¤Û¤Ü¸íº¹¤Ê¤¯´Ì¤ÎÊý¸þ¤ò¸þ¤±¤Æ¤¤¤¿¤Î¤ËÂФ·¡¢5¡¤6¤Ê¤É¤Î±ó¤¤°ÌÃÖ¤Ç¤ÏÆ±Íͤ˥º¥ì¤¬À¸¤¸¤Æ¤¤¤¿¡£¤½¤ì¤òÍÞÀ©¤¹¤ë¤¿¤á¤ËÀû²ó®ÅÙ¤òÍî¤È¤·¤Æ¤ß¤è¤¦¤È¤â¤·¤¿¤¬¡¢¥í¥Ü¥Ã¥È¤¬½Å¤¤¤»¤¤¤«¥â¡¼¥¿¡¼¤ÎÇÏÎϤ¬Â¤ê¤º¡¢¥¿¥¤¥ä¤¬²óž¤·¤Ê¤¤Åù¤Î»öÂÖ¤¬µ¯¤³¤Ã¤Æ¤·¤Þ¤Ã¤¿¡£¤½¤Î¤¿¤á¤¤¤¯¤é½ÅÎ̤ò´ËϤǤ¤ë¤è¤¦¤Ê¹©Éפ¬¤Ç¤¤¿¤«¤é¤È¸À¤Ã¤Æ¤â¡¢¤ä¤Ï¤ê¥í¥Ü¥Ã¥ÈËÜÂΤηÚÎ̲½¤ÏÂç»ö¤Ç¤¢¤ë¤è¤¦¤Ë»×¤¦¡£ |