Ìܼ¡
ÀĤÈÀ֤Υܡ¼¥ë¤ò±¿È¤·¤Æ¡¢¤½¤ì¤¾¤ì½êÄê¤Î350ml´Ì¤Î¾å¤Ë¾è¤»¤ë¡£
°Ê²¼¤ÎÆ°ºî¤ÎÀºÅÙ¡¦¥¹¥Ô¡¼¥É¡¦³Î¼ÂÀ¤Ê¤É¤ò´Þ¤á¤¿µ»½ÑŪ¤Ê¹©Éפä·Ý½ÑÀ¤Ë¤Ä¤¤¤Æ¾¤ÎÁ´¤Æ¤Î¥Á¡¼¥à(5¥Á¡¼¥à)¤¬20ÅÀËþÅÀ¤ÇºÎÅÀ¤·¡¢¤½¤ÎÊ¿¶ÑÅÀ¤òµá¤á¤ë¡£ ÆÀÅÀ¤ÎÌܰ¡§
º£²ó¤Î¥í¥Ü¥Ã¥È¤Ï2Âæ¤ÎNXT¤òÄÌ¿®¤·¤Æ¥×¥í¥°¥é¥à¤òÀ©¸æ¤¹¤ë¤Î¤Ç¡¢¼ç¤Ë¡Ö2Âæ¤ÎNXT¤òÊÌ¡¹¤ËÀ©¸æ¤¹¤ë¡×¤Þ¤¿¤Ï¡Ö2Âæ¤ÎNXT¤ò1Â⽤µ¤»¤ÆÂ礤ʥí¥Ü¥Ã¥È¤òºî¤ë¡×¤È¤¤¤¦2¤Ä¤ÎÊý¿Ë¤¬¹Í¤¨¤é¤ì¤ë¤¬»ä¤¿¤Á¤Î¥°¥ë¡¼¥×¤Ç¤Ï¡¢¸å¼Ô¤ÎÊý¿Ë¤òÍѤ¤¤ë¤³¤È¤Ë¤·¤¿¡£Íýͳ¤Ï¡¢¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò³Î¼Â¤Ë¹Ô¤¦¤³¤È¤ò½Å»ë¤·¡¢¤µ¤é¤Ë¤Ï¡¢¥í¥Ü¥Ã¥È¤òÊÌ¡¹¤ËÀ©¸æ¤¹¤ëºÝ¤Ë¥Ü¡¼¥ë¤ò´Ì¤Î¹â¤µ¤Þ¤Ç»ý¤Á¾å¤²¤ë¤³¤È¤¬Æñ¤·¤¤¤Î¤Ç¤Ï¤Ê¤¤¤«¤È¹Í¤¨¤¿¤«¤é¤Ç¤¢¤ë¡£
º£²ó¤Î²ÝÂê¤Ç¤Ï¡¢¸÷¥»¥ó¥µ¤ò2¤ÄÍѤ¤¤ë¤³¤È¤¬¤Ç¤¤¿¤Î¤Ç¡¢»ä¤¿¤Á¤Î¥°¥ë¡¼¥×¤Ç¤Ï¥»¥ó¥µ1¡Êº¸Â¦¤Î¸÷¥»¥ó¥µ¡Ë¤ò¥é¥¤¥ó¥È¥ì¡¼¥¹¤Î¤¿¤á¤Ë»È¤¤¡¢¥»¥ó¥µ4¡Ê±¦Â¦¤Î¸÷¥»¥ó¥µ¡Ë¤ò¸òº¹ÅÀǧ¼±¤Î¤¿¤á¤Ë»È¤¦¤³¤È¤Ç¥é¥¤¥ó¥È¥ì¡¼¥¹¤È¸òº¹ÅÀǧ¼±¤ò³Î¼Â¤Ë¹Ô¤¨¤ë¤è¤¦¤Ë¤·¤¿¡£
º£²ó¤Î¥í¥Ü¥Ã¥Èºî¤ê¤Ç°ìÈÖÂçÊѤÀ¤Ã¤¿¤Î¤¬¡Ö¥Ü¡¼¥ë¤ò»ý¤Á¾å¤²¤ë¡×¤È¤¤¤¦Æ°ºî¤Ç¤¢¤Ã¤¿¡£Åö½é¤Ï¥Ù¥ë¥È¥³¥ó¥Ù¥¢¡¼¤Î¤è¤¦¤ÊÉôÉʤòºî¤ê¡¢¥Ü¡¼¥ë¤ò´Ì¤Î¹â¤µ¤Þ¤Ç¾å¾º¤µ¤»¤ë¤È¤¤¤¦¥×¥é¥ó¤ò»×¤¤¤Ä¤¤¤¿¤¬¡¢ÉôÉʤòºî¤ë¤³¤È¤¬ÁÛÁü°Ê¾å¤ËÆñ¤·¤¯ÂçÊѤǤ¢¤Ã¤¿¤¿¤áÃÇÇ°¤·¤¿¡£
¼¡¤Ë»×¤¤¤Ä¤¤¤¿Êý¿Ë¤Ï¡¢¼Ì¿¿¤Î¤è¤¦¤Ë¥Ü¡¼¥ë¤òÄϤó¤Ç¤¤¤ë¥â¡¼¥¿¼«ÂΤò»ý¤Á¾å¤²¤ë¤È¤¤¤¦¥×¥é¥ó¤Ç¤¢¤ë¡£¡Ö¤³¤ÎÊý¿Ë¤Ê¤éÂç¾æÉספȻפäƤ¤¤¿¤¬¡¢¤¤¤¶¥×¥í¥°¥é¥à¤òºî¤êÆ°¤«¤·¤Æ¤ß¤ë¤È¡¢¥â¡¼¥¿¼«¿È¤Î½Å¤ß¤Ç¥¢¡¼¥à¤¬»ý¤Á¾å¤¬¤é¤Ê¤¤¡¢»ý¤Á¾å¤¬¤Ã¤Æ¤â¤¹¤°¤Ë¥¢¡¼¥à¤¬²¼¤¬¤ë¤È¤¤¤¦Í½ÁÛ³°¤ÎÌäÂ꤬ȯÀ¸¤·¤¿¡£
»ä¤¿¤Á¤Î¥°¥ë¡¼¥×¤Ç¤Ï¡¢¤³¤ÎÌäÂê¤ò²ò·è¤¹¤ë¤¿¤á¤ÎÊýË¡¤È¤·¤Æ¡¢NXTËÜÂΤ˥¢¡¼¥à¤òÅö¤Æ¤Æ¡¢¥¢¡¼¥à¤Î¹â¤µ¤ò°Ý»ý¤¹¤ë¤È¤¤¤¦ÊýË¡¤ò»×¤¤¤Ä¤¡¢¤³¤ÎÊýË¡¤Ç¥Ü¡¼¥ë¤ò»ý¤Á¾å¤²¤ë¤³¤È¤Ë¤·¤¿¡£
¡Ö2Âæ¤ÎNXT¤ò1Â⽤µ¤»¤ÆÂ礤ʥí¥Ü¥Ã¥È¤òºî¤ë¡×¤È¤¤¤¦Êý¿Ë¤òÍѤ¤¤¿¤¿¤á¡¢É¬Á³Åª¤Ë¥í¥Ü¥Ã¥È¤òÆ°¤«¤¹¥¿¥¤¥ä¤Ë¤ÏNXT2Âæʬ¤Î½Å¤µ¤È¤¤¤¦Â礤ÊÉé²Ù¤¬¤«¤«¤Ã¤Æ¤·¤Þ¤¦¡£¼ÂºÝ¤Ë¥í¥Ü¥Ã¥È¤òÆ°¤«¤·¤Æ¤ß¤¿»þ¤â¡¢¥í¥Ü¥Ã¥È¼«ÂΤνŤµ¤Î¤»¤¤¤Ç¥¿¥¤¥ä¤¬Æ°¤«¤Ê¤¯¤Ê¤Ã¤¿¤ê¡¢±ó¿´ÎϤǥí¥Ü¥Ã¥È¤¬Åݤì¤ë¤È¤¤¤¦¼ºÇÔ¤â·Ð¸³¤·¤¿¤Î¤Ç¡¢É¬Í׺Ǿ®¸Â¤ÎÉôÉʤǷڤ¤¥í¥Ü¥Ã¥È¤òºî¤ë¤³¤È¤òÌܻؤ·¤¿¡£
º£¤Þ¤Ç¤Î²ÝÂê¤È°ã¤¤¡¢¸÷¥»¥ó¥µ2¤Ä¡¢Ä¶²»ÇÈ¥»¥ó¥µ¡¢¥Ü¡¼¥ë¤òÄϤि¤á¤Î¥â¡¼¥¿¡¢¥Ü¡¼¥ë¤ò»ý¤Á¾å¤²¤ë¤¿¤á¤Î¥â¡¼¥¿¡¢¥¿¥¤¥ä¤Î°Ù¤ËÍѤ¤¤ë2¤Ä¤Î¥â¡¼¥¿¤È¤¤¤Ã¤¿·×7ËܤΥ³¡¼¥É¤ò»È¤¤¤Ê¤¬¤é¤â¡¢¾®·¿¤Î¥í¥Ü¥Ã¥È¤òºî¤Ã¤¿¤¿¤á¤Ë¥³¡¼¥É¤¬Íí¤Þ¤ê¤ä¤¹¤¯¡¢¥¿¥¤¥ä¤ÎÆ°¤¤ä¥Ü¡¼¥ë¤ò»ý¤Á¾å¤²¤ëÆ°ºî¤ò˸¤²¤ë¤³¤È¤¬¤¢¤Ã¤¿¤Î¤Ç¡¢2Âæ¤ÎNXT¤Î´Ö¤Ë¥³¡¼¥É¤ò¤·¤Þ¤¦¤¿¤á¤Î¶õ´Ö¤òºî¤ê¡¢¥³¡¼¥É¤¬¥í¥Ü¥Ã¥È¤ÎÆ°¤¤ò˸¤²¤Ê¤¤¤è¤¦¤Ë¤·¤¿¡£¡¢
º£²ó¤â²ÝÂꣲ¤Î»þ¤ÈƱÍͤˡֹõ¤ÈÇò¤Î¶³¦Àþ¤ò¥é¥¤¥ó¥È¥ì¡¼¥¹¤¹¤ë¡×¤È¤¤¤¦¥×¥í¥°¥é¥à¤òÀ©ºî¤·¤¿¡£¤¿¤À¡¢º£²ó¤Î²ÝÂê¤Ç¤Ï¡¢Àè¤Û¤É¤â½ñ¤¤¤¿Ä̤ê¸÷¥»¥ó¥µ¤ò£²¤Ä»È¤Ã¤Æ¤¤¤ë¤¿¤á¡¢¤è¤ê´Êñ¤Ë¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò¹Ô¤¦¤³¤È¤¬¤Ç¤¤¿¡£º£²ó¤Ï¸÷¥»¥ó¥µ¤òÃÏÌ̤ȶᤤ°ÌÃÖ¤ËÀßÃÖ¤·¤Æ¤¤¤ë¤¿¤á ÌÀ¤ë¤µ¤ÎÃͤ¬3Ãʳ¬¤Ç¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò¹Ô¤¦¤³¤È¤¬¤Ç¤¤¿¡£
#define go_forward OnFwd(OUT_A,40);OnFwd(OUT_C,40) //ľ¿Ê,A¤Ï°ìÈÖ±¦¤Î¥â¡¼¥¿,C¤Ï°ìÈÖº¸¤Î¥â¡¼¥¿ #define turn_left1 OnFwd(OUT_A,35);OnFwd(OUT_C,-30) //º¸Àû²ó #define turn_right1 OnFwd(OUT_C,35);OnFwd(OUT_A,-30) //±¦Àû²ó
Çò¤È¹õ¤Î¶³¦Àþ¤ÎÌÀ¤ë¤µ¤ÎÃͤ¬50¤À¤Ã¤¿¤Î¤Ç
#define THRESHOLD 50
¤ÈÄêµÁ¤·¤Æ¡¢²¼¿Þ¤Î¤è¤¦¤Ëº¸Â¦¤Î¥é¥¤¥ó¥È¥ì¡¼¥¹¤À¤±¤ò¹Ô¤¦¤è¤¦¤Ë¤·¤Æ¥é¥¤¥ó¥È¥ì¡¼¥¹¥×¥í¥°¥é¥à¤òºîÀ®¤·¤¿¡£
void Lfollow_line(long min_t) //min_t¤Ë¤¤¤ì¤¿»þ´Öʬ¤À¤±¥é¥¤¥ó¤Îº¸Â¦¤ò¥é¥¤¥ó¥È¥ì¡¼¥¹¤¹¤ë¡£ { long t0=CurrentTick(); //¸½ºß¤Î»þ´Ö¤òµÏ¿¤¹¤ë¡£ while(CurrentTick()-t0<=min_t) //¸½ºß¤Î»þ´Ö¤Èt0¤Î»þ´Ö¤Îº¹¤¬min_t°Ê²¼¤Î»þ¡¢°Ê²¼¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤¹¤ë¡£{ if ((SENSOR_1<THRESHOLD-7)&&(SENSOR_4<THRESHOLD-11)){ Off(OUT_AC);Wait(1000); //S1¤ÎÌÀ¤ë¤µ¤ÎÃͤ¬43̤Ëþ¤«¤ÄS4ÌÀ¤ë¤µ¤ÎÃͤ¬39̤Ëþ¤Î»þ¡¢¥í¥Ü¥Ã¥È¤¬Ää»ß¤¹¤ë¡£ } else if (SENSOR_1<THRESHOLD-7){ turn_left1; //ÌÀ¤ë¤µ¤ÎÃͤ¬43̤Ëþ¤Î»þ¡¢turn_left1¤ÎÆ°¤¤ò¤¹¤ë¡£ } else if (SENSOR_1<THRESHOLD+7){ go_forward;¡¡//ÌÀ¤ë¤µ¤ÎÃͤ¬43°Ê¾å57̤Ëþ¤Î»þ¡¢go_forward¤ÎÆ°¤¤ò¤¹¤ë¡£ } else { turn_right1; //¤½¤ì°Ê³°¤Î»þ¡¢turn_right1¤ÎÆ°¤¤ò¤¹¤ë¡£ } }
¤³¤Î¥×¥í¥°¥é¥à¤Ï°Ê²¼¤Î¤è¤¦¤ËÆ°¤¯¡£
#define follow_intersection ResetTachoCount(OUT_AC);RotateMotor(OUT_AC,30,100); #define follow_intersection2 ResetTachoCount(OUT_AC);RotateMotor(OUT_AC,30,300);
¤³¤Î¥×¥í¥°¥é¥à¤Ï¸òº¹ÅÀ¤ò²£ÃǤ¹¤ë¤¿¤á¤ËÍѤ¤¤ë¡£2¼ïÎฺߤ¹¤ë¤Î¤Ïfollow_intersection¤Î¥×¥í¥°¥é¥à¤ò3²óÂǤĤΤò¾Êά¤¹¤ë¤¿¤á¤ËÍѤ¤¤¿¤Î¤ÇÆä˿¼¤¤°ÕÌ£¤Ï¤Ê¤¤¡£
#define Mfollow_intersection ResetTachoCount(OUT_AC);RotateMotor(OUT_AC,-30,210);
¤³¤Î¥×¥í¥°¥é¥à¤ÏÀĤΥܡ¼¥ë¤òÄϤि¤á¤Îµ÷Î¥¤ò¼è¤ë¤¿¤á¤ËÍѤ¤¤ë¡£
void follow_line(long min_t) //min_t¤Ë¤¤¤ì¤¿»þ´Öʬ¤À¤±¥é¥¤¥ó¤Îº¸Â¦¤ò¥é¥¤¥ó¥È¥ì¡¼¥¹¤¹¤ë¡£ { long t0=CurrentTick(); //¸½ºß¤Î»þ´Ö¤òµÏ¿¤¹¤ë¡£ while(CurrentTick()-t0<=min_t) //¸½ºß¤Î»þ´Ö¤Èt0¤Î»þ´Ö¤Îº¹¤¬min_t°Ê²¼¤Î»þ¡¢°Ê²¼¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤¹¤ë¡£{ if (SENSOR_1<THRESHOLD-7){ turn_left1; //ÌÀ¤ë¤µ¤ÎÃͤ¬43̤Ëþ¤Î»þ¡¢turn_left1¤ÎÆ°¤¤ò¤¹¤ë¡£ } else if (SENSOR_1<THRESHOLD+7){ go_forward; //ÌÀ¤ë¤µ¤ÎÃͤ¬43°Ê¾å57̤Ëþ¤Î»þ¡¢go_forward¤ÎÆ°¤¤ò¤¹¤ë¡£ } else { turn_right1; //¤½¤ì°Ê³°¤Î»þ¡¢turn_right1¤ÎÆ°¤¤ò¤¹¤ë¡£ } }
¤³¤Î¥×¥í¥°¥é¥à¤Ï°Ê²¼¤Î¤è¤¦¤ËÆ°¤¯¡£
#define SPEED 50 #define SPEED_SLOW 30 #define SPEED_MSLOW -30 const float diameter = 5.45; //¥¿¥¤¥ä¤Îľ·Â(cm) const float track = 10.35; //¥¿¥¤¥ä¤Î¥È¥ì¥Ã¥ÉÉý(cm) const float pi = 3.1415; //±ß¼þΨ
void turnAng(long ang) //³ÑÅÙ ang Å٤λþ·×²ó¤ê¤ÎÀû²ó { long angle; angle = track/diameter*ang; RotateMotorEx(OUT_AC, SPEED_SLOW, angle, 100, true, true); } int searchDirection(long ang)// ¸½ºß¤ÎÊý¸þ¤òÃæ¿´¤ËangÅÙ¤ÎÈϰϤÇõ¤¹¡£ // ¾ã³²Êª¤Þ¤Ç¤Îµ÷Î¥¤òÊÖ¤¹¡£ { long angle, tacho_min=0, tacho_corr; int d_min; d_min=300; angle = (track/diameter)*ang;// Àû²ó³ÑÅÙ¤«¤é¥¿¥¤¥ä¤Î²óž¤ò·×»»¤¹¤ë¡£ turnAng(ang/2); // Àû²ó³ÑÅÙ¤«¤é¥¿¥¤¥ä¤Î²óž¤ò·×»»¤¹¤ë¡£ ResetTachoCount(OUT_AC); // ³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È¤¹¤ë¡£ OnFwdSync(OUT_AC,SPEED_SLOW,-100); while(MotorTachoCount(OUT_A)<=angle){ if(SensorUS(S2)<d_min){ d_min=SensorUS(S2); // ²¾¤ÎºÇ¾®Ãͤò¹¹¿·¤¹¤ë¡£ tacho_min=MotorTachoCount(OUT_A); } } OnFwdSyncEx(OUT_AC,SPEED_SLOW,100,RESET_NONE); until(MotorTachoCount(OUT_A)<=tacho_min||SensorUS(S2)<=d_min); Wait(14); //ÈùÄ´À° Off(OUT_AC);Wait(500); return d_min; ResetTachoCount(OUT_AC); // ³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È¤¹¤ë¡£ }
void fwdDist(float d) //µ÷Î¥ d cm Á°¿Ê¤¹¤ë¡£ { long angle; angle = d/(diameter*pi)*360.0 ; //³ÑÅÙ¤ò·×»»¤¹¤ë¡£ RotateMotorEx(OUT_AC,SPEED_SLOW,angle,0,true,true); }
¤³¤Î¥×¥í¥°¥é¥à¤Ë¤è¤ê¡¢¤µ¤¤Û¤É¤ÎsearchDirection()¤È¤¤¤¦´Ø¿ô¤Ë¤è¤Ã¤ÆÆÀ¤é¤ì¤¿d_min (cm) ¤ÎÃͤÎʬ¤À¤±¥í¥Ü¥Ã¥È¤¬Á°¿Ê¤¹¤ë¡£¤Þ¤¿¡¢¤³¤Î¥×¥í¥°¥é¥à¤Î¥â¡¼¥¿¤òµÕ²óž¤·²¼µ¤Î¤è¤¦¤Ê¥×¥í¥°¥é¥à¤ò¤Ä¤¯¤ë¤³¤È¤Çd_min (cm) ¤À¤±¥í¥Ü¥Ã¥È¤ò¸åÂव¤»¡¢´Ì¤òõ¤·»Ï¤á¤¿°ÌÃÖ¤Þ¤ÇÌá¤ë¤³¤È¤¬²Äǽ¤È¤Ê¤ë¡£
void MfwdDist(float d) //µ÷Î¥ d cm ¸åÂह¤ë¡£ { long angle; angle = d/(diameter*pi)*360.0 ; ////³ÑÅÙ¤ò·×»»¤¹¤ë¡£ RotateMotorEx(OUT_AC,SPEED_MSLOW,angle,0,true,true); }
void turnAng2(long ang2)¡¡//³ÑÅÙ ang2 Å٤λþ·×²ó¤ê¤ÎÀû²ó { long angle2; angle2 = track/diameter*ang2; RotateMotorEx(OUT_AC, SPEED_SLOW, angle2, 100, true, true); } int e_searchDirection(long ang2)¡¡//¸½ºß¤ÎÊý¸þ¤òÃæ¿´¤Ëang2ÅÙ¤ÎÈϰϤÇõ¤¹¡£ // ¾ã³²Êª¤Þ¤Ç¤Îµ÷Î¥¤òÊÖ¤¹¡£ { long angle2, tacho_min2=0, tacho_corr2; int e_min; //²¾¤ÎºÇ¾®ÃÍ e_min=300; angle2 = (track/diameter)*ang2; //Àû²ó³ÑÅÙ¤«¤é¥¿¥¤¥ä¤Î²óž¤ò·×»»¤¹¤ë¡£ turnAng2(ang2/2); //»ØÄꤵ¤ì¤¿³ÑÅÙ¤ÎȾʬ¤òÀû²ó¤¹¤ë¡£ ResetTachoCount(OUT_AC); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È¤¹¤ë¡£ OnFwdSync(OUT_AC,SPEED_SLOW,-100); //È¿»þ·×²ó¤ê¤ËÀû²ó¤¹¤ë¡£ while(MotorTachoCount(OUT_A)<=angle2){ if(SensorUS(S2)<e_min){ e_min=SensorUS(S2); //²¾¤ÎºÇ¾®Ãͤò¹¹¿·¤¹¤ë¡£ tacho_min2=MotorTachoCount(OUT_A); } } OnFwdSyncEx(OUT_AC,SPEED_SLOW,100,RESET_NONE); until(MotorTachoCount(OUT_A)<=tacho_min2||SensorUS(S2)<=e_min); Wait(14); ÈùÄ´À° Off(OUT_AC);Wait(500); return e_min; ResetTachoCount(OUT_AC); //³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È¤¹¤ë¡£ }
void e_fwdDist(float e) //µ÷Î¥ e cm Á°¿Ê¤¹¤ë¡£ { long angle2; angle2 = e/(diameter*pi)*360.0 ; //³ÑÅÙ¤ò·×»»¤¹¤ë¡£ RotateMotorEx(OUT_AC,SPEED_SLOW,angle2,0,true,true); }
¤³¤Î¥×¥í¥°¥é¥à¤Ë¤è¤ê¡¢¤µ¤¤Û¤É¤Îe_searchDirection()¤È¤¤¤¦´Ø¿ô¤Ë¤è¤Ã¤ÆÆÀ¤é¤ì¤¿e_min (cm) ¤ÎÃͤÎʬ¤À¤±¥í¥Ü¥Ã¥È¤¬Á°¿Ê¤¹¤ë¡£¤Þ¤¿¡¢¤³¤Î¥×¥í¥°¥é¥à¤Î¥â¡¼¥¿¤òµÕ²óž¤·²¼µ¤Î¤è¤¦¤Ê¥×¥í¥°¥é¥à¤ò¤Ä¤¯¤ë¤³¤È¤Çe_min (cm) ¤À¤±¥í¥Ü¥Ã¥È¤ò¸åÂव¤»¡¢´Ì¤òõ¤·»Ï¤á¤¿°ÌÃÖ¤Þ¤ÇÌá¤ë¤³¤È¤¬²Äǽ¤È¤Ê¤ë¡£
void e_MfwdDist(float e)¡¡//µ÷Î¥ e cm ¸åÂह¤ë¡£ { long angle2; angle2 = e/(diameter*pi)*360.0 ; //³ÑÅÙ¤ò·×»»¤¹¤ë¡£ RotateMotorEx(OUT_AC,SPEED_MSLOW,angle2,0,true,true); }
void Ball_catch() { RemoteStartProgram(1,"Last(S1).rxe"); //»Òµ¡Æâ¤Ë¤¢¤ëLast(S1)¤È¤¤¤¦¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤¹¤ë¤è¤¦¤ËÌ¿Î᤹¤ë¡£ while(msg != 11) //»Òµ¡¤«¤é¡Ö11¡×¤È¤¤¤¦¥á¥Ã¥»¡¼¥¸¤¬Íè¤ë¤Þ¤ÇÂԤġ£{ ReceiveRemoteNumber(MAILBOX1,true,msg); //¥á¡¼¥ë¥Ü¥Ã¥¯¥¹£±¤ò¶õ¤Ë¤¹¤ë¡£¢«»Òµ¡¤«¤é¤Î¥á¥Ã¥»¡¼¥¸¤¬¿Æµ¡¤Î±¬ÌÚµ¤¤ò˸¤²¤ë¤Î¤òËɤ°¤¿¤á¡£ } Wait(3000); }
¥Ü¡¼¥ë¤òÄϤàÆ°ºî¼«ÂΤϻҵ¡¤¬¹Ô¤¦¤¿¤á¡¢¤³¤Î¥×¥í¥°¥é¥à¤Ï»Òµ¡¤Ë¸þ¤«¤Ã¤Æ¿Æµ¡¤¬¡Ö¥Ü¡¼¥ë¤òÄϤá¡×¤ÈÌ¿Î᤹¤ë¤è¤¦¤Ê¥×¥í¥°¥é¥à¤È¤Ê¤Ã¤Æ¤¤¤ë¡£»Òµ¡¤Î¥×¥í¥°¥é¥ß¥ó¥°¤¬½ªÎ»¤·¤¿¤³¤È¤òÃΤ餻¤ë¤¿¤á¤Î¹ç¿Þ¤È¤·¤Æmsg¤È¤¤¤¦ÊÑ¿ô¤òÍѤ¤¤ë¡£¤Þ¤¿¡¢»Òµ¡¤ÎÀܳÈÖ¹æ¤Ï1¤ÈÄêµÁ¤·¤Æ¤¤¤ë¡£
int msg;// »Òµ¡¤Î¥×¥í¥°¥é¥à¤¬½ªÎ»¤·¤¿¤³¤È¤òÃΤ餻¤ë¤¿¤á¤Î¹ç¿Þ¡£ #define CONN 1 // »Òµ¡¤ÎÀܳÈÖ¹æ
Last(S1) #define Ball_catch ResetTachoCount(OUT_B);RotateMotor(OUT_B,-25,75);¡¡//ÅÅÃÓ»ÄÎ̤ˤè¤é¤º¡¢¾ï¤Ë°ìÄê¤ÎÆ°¤¤ò¤¹¤ë¡£ task main() { Ball_catch;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥Ü¡¼¥ë¤òÄϤࡣ SendResponseNumber(MAILBOX1,11);¡¡//¿Æµ¡¤Ë¡Ö11¡×¤È¤¤¤¦¥á¥Ã¥»¡¼¥¸¤òÁ÷¤ë¡£ Wait(1000); }
¤³¤ì¤é¤Î¥×¥í¥°¥é¥à¤Ï²¼¿Þ¤Î¤è¤¦¤Ë¿Ê¹Ô¤¹¤ë¡£
void Ball_release() { RemoteStartProgram(1,"Last(S2).rxe"); //»Òµ¡Æâ¤Ë¤¢¤ëLast(S1)¤È¤¤¤¦¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤¹¤ë¤è¤¦¤ËÌ¿Î᤹¤ë¡£ while(msg != 12) //»Òµ¡¤«¤é¡Ö12¡×¤È¤¤¤¦¥á¥Ã¥»¡¼¥¸¤¬Íè¤ë¤Þ¤ÇÂԤġ£{ ReceiveRemoteNumber(MAILBOX1,true,msg); //¥á¡¼¥ë¥Ü¥Ã¥¯¥¹£±¤ò¶õ¤Ë¤¹¤ë¡£¢«»Òµ¡¤«¤é¤Î¥á¥Ã¥»¡¼¥¸¤¬¿Æµ¡¤ÎÆ°¤¤ò˸¤²¤ë¤Î¤òËɤ°¤¿¤á¡£ } Wait(3000); }
¥Ü¡¼¥ë¤òÊü½Ð¤¹¤ëÆ°ºî¼«ÂΤϻҵ¡¤¬¹Ô¤¦¤¿¤á¡¢¤³¤Î¥×¥í¥°¥é¥à¤Ï»Òµ¡¤Ë¸þ¤«¤Ã¤Æ¿Æµ¡¤¬¡Ö¥Ü¡¼¥ë¤òÊü½Ð¤·¤í¡×¤ÈÌ¿Î᤹¤ë¤è¤¦¤Ê¥×¥í¥°¥é¥à¤È¤Ê¤Ã¤Æ¤¤¤ë¡£»Òµ¡¤Î¥×¥í¥°¥é¥ß¥ó¥°¤¬½ªÎ»¤·¤¿¤³¤È¤òÃΤ餻¤ë¤¿¤á¤Î¹ç¿Þ¤È¤·¤Æmsg¤È¤¤¤¦ÊÑ¿ô¤òÍѤ¤¤ë¡£¤Þ¤¿¡¢»Òµ¡¤ÎÀܳÈÖ¹æ¤Ï1¤ÈÄêµÁ¤·¤Æ¤¤¤ë¡£
int msg;// »Òµ¡¤Î¥×¥í¥°¥é¥à¤¬½ªÎ»¤·¤¿¤³¤È¤òÃΤ餻¤ë¤¿¤á¤Î¹ç¿Þ¡£ #define CONN 1 // »Òµ¡¤ÎÀܳÈÖ¹æ
Last(S2) #define Ball_release ResetTachoCount(OUT_B);RotateMotor(OUT_B,25,75); //ÅÅÃÓ»ÄÎ̤ˤè¤é¤º¡¢¾ï¤Ë°ìÄê¤ÎÆ°¤¤ò¤¹¤ë¡£ task main() { Ball_release; //¥Ü¡¼¥ëÊü½Ð¤¹¤ë¡£ SendResponseNumber(MAILBOX1,12); //¿Æµ¡¤Ë¡Ö12¡×¤È¤¤¤¦¥á¥Ã¥»¡¼¥¸¤òÁ÷¤ë¡£ Wait(1000); }
¤³¤ì¤é¤Î¥×¥í¥°¥é¥à¤Ï²¼¿Þ¤Î¤è¤¦¤Ë¿Ê¹Ô¤¹¤ë¡£
#define Ball_up RotateMotor(OUT_B,-50,160);Wait(2000); #define Ball_up2 RotateMotor(OUT_B,-50,50);Wait(2000);
½é¤á¤Î¥×¥í¥°¥é¥à¤Ï¥Ü¡¼¥ë¤òÄϤó¤À¸å¤ËNXTËÜÂΤ˥¢¡¼¥à¤òÅö¤Æ¡¢¥¢¡¼¥à¤¬²¼¤¬¤Ã¤Æ¤³¤Ê¤¤°ÌÃ֤ޤǥܡ¼¥ë¤ò»ý¤Á¾å¤²¤ë¤¿¤á¤Ë»È¤¤¡¢2¤ÄÌÜ¤Î¥×¥í¥°¥é¥à¤Ï´Ì¤Ë¥Ü¡¼¥ë¤ò¾è¤»¤¿¸å¤Ë¡¢¥¢¡¼¥à¤ò¤â¤È¤Î°ÌÃÖ¤Þ¤ÇÌ᤹¤¿¤á¤ËÍѤ¤¤é¤ì¤ë¡£
#define Ball_down RotateMotor(OUT_B,30,60);Wait(2000); #define Ball_down2 RotateMotor(OUT_B,30,150);Wait(2000);
½é¤á¤Î¥×¥í¥°¥é¥à¤Ï´Ì¤Ë¶á¤Å¤¤¤¿¸å¤Ë¡¢¥¢¡¼¥à¤ò²¼¤²¤Æ¥Ü¡¼¥ë¤ò´Ì¤Ë¾è¤»¤ë¤¿¤á¤Ë»È¤¤¡¢2¤ÄÌÜ¤Î¥×¥í¥°¥é¥à¤ÏÀĤ¤¥Ü¡¼¥ë¤òÄϤि¤á¤ËÍѤ¤¤é¤ì¤ë¡£
task main() { SetSensorLight(S1); //º¸Â¦¤Î¥»¥ó¥µ SetSensorLowspeed(S2); //Ķ²»ÇÈ¥»¥ó¥µ SetSensorLight(S4); //±¦Â¦¤Î¥»¥ó¥µ Wait(2000); //¥¹¥¿¡¼¥È¥Ü¥¿¥ó¤ò²¡¤·¤Æ¤«¤é¡¢Î¥¤ì¤ë¤Þ¤Ç¤Î»þ´Ö¡£ Ball_catch(); //»Òµ¡¤Ë¥Ü¡¼¥ë¤òÄϤޤ»¤ë¡£ follow_intersection; //¥¹¥¿¡¼¥È¥é¥¤¥ó¤ò²£ÃǤ¹¤ë¡£ Lfollow_line(3000); //¼¡¤Î¸òº¹ÅÀ(GÃÏÅÀ)¤Þ¤Ç¿Ê¤ó¤ÇÄä»ß¤¹¤ë¡£ follow_intersection; //¸òº¹ÅÀ¤ò²£ÃǤ¹¤ë¡£ Ball_up; //¥¢¡¼¥à¤ò¤¢¤²¤ë¡£¢«¤³¤Î¤Þ¤Þ¿Ê¹Ô¤·¤Æ¤¤¤¯¤ÈÀĤ¤¥Ü¡¼¥ë¤Ë¤Ö¤Ä¤«¤Ã¤Æ¤·¤Þ¤¦¡£ Lfollow_line(18000); //¼¡¤Î¸òº¹ÅÀ(EÃÏÅÀ)¤Þ¤Ç¿Ê¤ó¤ÇÄä»ß¤¹¤ë¡£ int d=searchDirection(120);//´Ì¤òõ¤¹¡£ fwdDist(d-18.0); //´Ì¤Ë¶á¤Å¤¯¡£ searchDirection(20); //´Ì¤Ë¥Ü¡¼¥ë¤ò¾è¤»¤é¤ì¤ë¤è¤¦¤ËÈùÄ´À°¤¹¤ë¡£ Ball_down; //¥¢¡¼¥à¤ò²¼¤²¤ë¡£ Ball_release(); //»Òµ¡¤Ç´Ì¤Ë¥Ü¡¼¥ë¤ò¾è¤»¤ë¡£ MfwdDist(d-17.0); //´Ì¤«¤éÎ¥¤ì¤ë¡£¢«¸µ¤Î°ÌÃÖ¤è¤ê¤â1Ѳ¼¤¬¤ë¤³¤È¤Ë¤è¤ê¡¢EÃÏÅÀ¤Î¸òº¹ÅÀ¤Çľ¿Ê¤¹¤ë¤³¤È¤òËɤ°¡£ Ball_up2; //¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò¹Ô¤¦ºÝ¤Ë¡¢¥Ü¡¼¥ë¤ä´Ì¤Ë¤Ö¤Ä¤«¤ë¤Î¤òËɤ°¤¿¤á¤Ë¥¢¡¼¥à¤ò¾å¤²¤ë¡£ follow_line(2000); //¥é¥¤¥ó¥È¥ì¡¼¥¹¤ËÌá¤ë¡£¤³¤Î»þ¡¢EÃÏÅÀ¤òº¸ÀÞ¤¹¤ë¡£ Lfollow_line(3000); //¼¡¤Î¸òº¹ÅÀ(IÃÏÅÀ)¤Þ¤Ç¿Ê¤ó¤ÇÄä»ß¤¹¤ë¡£ Mfollow_intersection; //ÀĤ¤¥Ü¡¼¥ë¤ò³Î¼Â¤Ë¤Ä¤«¤à¤³¤È¤¬¤Ç¤¤ë¤è¤¦¤Ë¸åÂह¤ë¡£ Ball_down2; //¥¢¡¼¥à¤ò²¼¤²¤ë¡£ Ball_catch(); //»Òµ¡¤Ë¥Ü¡¼¥ë¤òÄϤޤ»¤ë¡£ Ball_up; //¥¢¡¼¥à¤ò¤¢¤²¤ë¡£ follow_intersection2; //ÀÖ¤¤¥Ü¡¼¥ë¤òÅö¤Æ¤ë¤¿¤á¤Î´Ì¤òĶ²»ÇÈ¥»¥ó¥µ¤Ç´¶ÃΤ·¤Ê¤¤¤è¤¦¤Ë¡¢±ßIHKJ¤ÎÃæ¿´¤Þ¤ÇÁ°¿Ê¤¹¤ë¡£ int e=e_searchDirection(120); //´Ì¤òõ¤¹¡£ e_fwdDist(e-18.0); //´Ì¤Ë¶á¤Å¤¯¡£ e_searchDirection(20); //´Ì¤Ë¥Ü¡¼¥ë¤ò¾è¤»¤é¤ì¤ë¤è¤¦¤ËÈùÄ´À°¤¹¤ë¡£ Ball_down; //¥¢¡¼¥à¤ò²¼¤²¤ë¡£ Ball_release(); //»Òµ¡¤Ç´Ì¤Ë¥Ü¡¼¥ë¤ò¾è¤»¤ë¡£ e_MfwdDist(e-18.0); //´Ì¤«¤éÎ¥¤ì¤Æ¤â¤È¤Î°ÌÃÖ¤ËÌá¤ë¡£ }
º£²ó¤Î²ÝÂê¤Ç¤Ï¡¢¥×¥í¥°¥é¥à¼«ÂΤϴÊñ¤Ëºî¤ë¤³¤È¤¬¤Ç¤¤¿¤¬¡¢½ÅÎ̤ä²ÄÆ°°è¤Ê¤É¤¬¥í¥Ü¥Ã¥È¤ÎÆ°¤¤ËÂ礤ʱƶÁ¤òÍ¿¤¨¤Æ¤·¤Þ¤¤»×¤¤Ä̤ê¤ËÆ°¤«¤¹¤³¤È¤Î¤Ç¤¤ë¥í¥Ü¥Ã¥È¤òºî¤ë¤³¤È¤¬¤È¤Æ¤âÂçÊѤÀ¤Ã¤¿¡£ËÜÈ֤ǤÏ2¤Ä¤Î¥Ü¡¼¥ë¤òÄϤळ¤È¤Ï¤Ç¤¤¿¤¬¡¢ÀÖ¤¤¥Ü¡¼¥ë¤Ï´Ì¤ËÅö¤Æ¤ë¤³¤È¤¬¤Ç¤¤¿¤¬¡¢ÀĤ¤¥Ü¡¼¥ë¤Ï´Ì¤òÀµ³Î¤Ëǧ¼±¤¹¤ë¤³¤È¤¬¤Ç¤¤º¾å¼ê¤¯´Ì¤ËÅö¤Æ¤ë¤³¤È¤¬¤Ç¤¤Ê¤«¤Ã¤¿¡£Îý½¬»þ¤Ë¤Ï¡¢1Ãʳ¬¤Ç¤Ï¤Ê¤¯¡¢2Ãʳ¬¤Ç´Ì¤òõ¤¹¥×¥í¥°¥é¥à¤òºîÀ®¤·¤Æ¤ß¤¿¤¬¤É¤Á¤é¤Î¥×¥í¥°¥é¥à¤Ç¤âËè²óƱ¤¸¤è¤¦¤Ë¤Ï´Ì¤òõ¤¹¤³¤È¤Ï¤Ç¤¤Ê¤«¤Ã¤¿¡£
»ä¤Ï¡¢´Ì¤¬¤¦¤Þ¤¯Ç§¼±¤Ç¤¤Ê¤¤¤Î¤Ï¡¢´Ì¤Î¦Ì̤¬¶ÊÌ̤ÇĶ²»ÇȤ¬ÊѤÊÊý¸þ¤ËÈ¿¼Í¤·¤Æ¤·¤Þ¤¦¤³¤È¤¬¸¶°ø¤Ê¤Î¤Ç¤Ï¤Ê¤¤¤«¤È¹Í¤¨¤¿¡£¤Þ¤¿¡¢´Ì¤òõ¤¹¤È¤¤Ë¤â¤Ã¤È¥í¥Ü¥Ã¥È¤Î®ÅÙ¤òÃÙ¤¯¤¹¤ë¤³¤È¤Ç´Ì¤òÀµ³Î¤Ëǧ¼±¤¹¤ë¤³¤È¤¬¤Ç¤¤¿¤Î¤Ç¤Ï¤Ê¤¤¤«¤È¹Í¤¨¤¿¡£