2014b/Member/arisyka/Mission3
¤ò¥Æ¥ó¥×¥ì¡¼¥È¤Ë¤·¤ÆºîÀ®
[
¥È¥Ã¥×
] [
¿·µ¬
|
°ìÍ÷
|
¸¡º÷
|
ºÇ½ª¹¹¿·
|
¥Ø¥ë¥×
|
¥í¥°¥¤¥ó
]
³«»Ï¹Ô:
[[2014b/Member]]
#contents
*¤Ï¤¸¤á¤Ë [#o4d8a879]
²ÝÂê3¤Ï¥¹¥¿¡¼¥ÈÃÏÅÀ¤«¤é¥í¥Ü¥Ã¥È¤ò¿Ê¤á¡¢2¤Ä¤Î»æ¥³¥Ã¥×¤Ë¤½¤ì¤¾¤ìÆþ¤Ã¤Æ¤¤¤ë2¸Ä¤Î¥Ü¡¼¥ë¤òÆþ¤ìÂؤ¨¤ë¤È¤¤¤¦¤â¤Î¤Ç¤¢¤ë¡£
([[2014ǯ¸å´ü/²ÝÂê3:http://yakushi.shinshu-u.ac.jp/robotics/?2014b%2FMission3]]»²¾È)
¡¡»ä¤¿¤Á¤ÎÈɤΥá¥ó¥Ð¡¼¤Ï¡¢
-[[edaeda:http://yakushi.shinshu-u.ac.jp/robotics/?2014b%2FMember%2Fedaeda%2FMission3]]
-[[knee:http://yakushi.shinshu-u.ac.jp/robotics/?2014b%2FMember%2Fknee%2FMission3]]
- kuryu
¤È»ä¤Î4¿Í¤Ç¤¢¤ë¡£
»ä¤¿¤Á¤ÎÈɤιͰƤϡ¢¥¢¡¼¥à¤È¥ì¡¼¥ë¤òºî¤ê¡¢¥¢¡¼¥à¤ò»È¤Ã¤Æ£´¤Ä¤Î¥Ü¡¼¥ë¤ò¥ì¡¼¥ë¤Ë¤Î¤»¡¢¥ì¡¼¥ë¤ò·¹¤«¤»¤ë¤³¤È¤Ç»æ¥³¥Ã¥×¤Ë¥Ü¡¼¥ë¤òÆþ¤ì¤ë¤È¤¤¤¦¤â¤Î¤Ç¤¢¤ë¡£
*¥í¥Ü¥Ã¥È¡¢¤Þ¤¿¤Ï¤½¤ÎÆ°ºî¤ÎÀâÌÀ [#h9193c48]
**Á´ÂÎ [#ne8db113]
#ref(2014b/Member/arisyka/Mission3/IMG_3870.JPG,20%)
¼Ì¿¿¤Î¤è¤¦¤Ê¹½Â¤¤Ç¤¢¤ë¡£
±¦¤Èº¸¤Î¥¿¥¤¥ä¤ÎÉé²Ù¤¬¤½¤ì¤¾¤ì°Û¤Ê¤ë¤¿¤á¡¢¥×¥í¥°¥é¥à¤Çº¸±¦¤Î¥¿¥¤¥ä¤Î½ÐÎϤòÊѤ¨¤ë¤³¤È¤Çľ¿Ê¤ä²óž¡¢Àû²ó¤ò¥¹¥à¡¼¥º¤Ë¹Ô¤ï¤»¤ë¡£
**¥¢¡¼¥à [#y4bd3434]
#ref(2014b/Member/arisyka/Mission3/IMG_3869.gif,30%)
¾å¤Î¼Ì¿¿¤ÎÀ֤ǰϤޤ줿Éôʬ¡£
1¤Î¥â¡¼¥¿¡¼¤Ç¥¢¡¼¥àÁ´ÂΤò¾å¤²²¼¤²¤·¡¢2¤Î¥â¡¼¥¿¡¼¤Ç¥¢¡¼¥à¤Î¶´¤àÆ°¤¤òÁàºî¤¹¤ë¡£
¥¢¡¼¥à¤òÆ°¤«¤¹¤¿¤á¤Ë»È¤Ã¤¿ÄêµÁ¤Ï°Ê²¼¤Î¤è¤¦¤Ç¤¢¤ë¡£
#define SPEED1 45
#define SPEED2 60
#define SPEED3 10
#define SPEED4 30
#define SPEED5 50
#define SPEED6 25
float GetAngle(float d) {¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¿Ê¤àµ÷Î¥¤ò·×»»¤¹¤ë
const float diameter=5.45;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¿¥¤¥ä¤Îľ·Â
const float pi=3.1415;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//±ß¼þΨ
float ang = d/(diameter*pi)*360.0; return ang; //³ÑÅÙ¤ò·×»»¤¹¤ë
}
¤³¤³¤ÎÄêµÁ¤Ï¥¢¡¼¥àÁ´ÂΤÎÆ°¤¤òÄêµÁ¤·¤Æ¤¤¤ë¡£(¾å¤Î¼Ì¿¿¤Î¡¤Î¥â¡¼¥¿¡¼¤Ë¤è¤ëÆ°¤)
#define DOWN RotateMotor(OUT_A,SPEED3,105);¡¡¡¡//¥¢¡¼¥àÁ´ÂΤò²¼¤²¤ë
#define UP RotateMotor(OUT_A,-SPEED5,105); ¡¡//¥¢¡¼¥àÁ´ÂΤò¾å¤²¤ë
#define LDown RotateMotor(OUT_A,SPEED3,25);¡¡¡¡
#define LUp RotateMotor(OUT_A,-SPEED2,25);
#define Little LDown;Wait(2000);LUp; //¥Ü¡¼¥ë¤ò¥ì¡¼¥ë¤Ë³Î¼Â¤Ë¤Î¤»¤ë¤¿¤á¤ÎÆ°¤
#ref(2014b/Member/arisyka/Mission3/arm¥ë¡¼¥×2.gif,45%)
¤³¤³¤ÎÄêµÁ¤Ï¥¢¡¼¥à¤Î¥Ï¥µ¥ßÉôʬ¤ÎÆ°¤¤òÄêµÁ¤·¤Æ¤¤¤ë¡£(¾å¤Î¼Ì¿¿¤Î¢¤Î¥â¡¼¥¿¡¼¤Ë¤è¤ëÆ°¤)
#define Catch RotateMotor(OUT_B,-SPEED4,105);¡¡¡¡//¥¢¡¼¥à¤Ç¤Ä¤«¤à
#define Separate RotateMotor(OUT_B,SPEED4,105); //¥¢¡¼¥à¤òÎ¥¤¹
#ref(2014b/Member/arisyka/Mission3/catch&separate.gif,40%)
**¥ì¡¼¥ë [#k920454a]
#ref(2014b/Member/arisyka/Mission3/IMG_3873.gif,30%)
¾å¤Î¼Ì¿¿¤ÎÀ֤ǰϤޤ줿Éôʬ¡£¤Þ¤¿¡¢Üô¿§¤Ç°Ï¤Þ¤ì¤¿Éôʬ¤Ï¥Ü¡¼¥ë¤ò»æ¥³¥Ã¥×¤ËÆþ¤ì¤ëºÝ¤Ë¡¢»æ¥³¥Ã¥×¤¬Åݤì¤Ê¤¤¤è¤¦¤Ë¤¹¤ë¤¿¤á¤Î¥¹¥È¥Ã¥Ñ¡¼¤Ç¤¢¤ë¡£Åݤ¹³ÑÅÙ¤òÄ´À°¤¹¤ë¤³¤È¤Ç»æ¥³¥Ã¥×¤ËÆþ¤ì¤ë¥Ü¡¼¥ë¤Î¿ô¤òÊѤ¨¤ë¡£
¥ì¡¼¥ë¤òÆ°¤«¤¹¤¿¤á¤Ë»È¤Ã¤¿Æ°ºî¤Ï¾åµ¤Ë½ñ¤¤¤¿¥¢¡¼¥à¤ÎÄêµÁ¤ÈƱ¤¸¤â¤Î¤ò»ÈÍѤ·¤¿¡£
#define Into1 RotateMotor(OUT_C,SPEED3,20); ¡¡¡¡//¥Ü¡¼¥ë¤òÆþ¤ì¤ë(1²óÌÜ)
#define Return1 RotateMotor(OUT_C,-SPEED3,25);¡¡ //¥ì¡¼¥ë¤òÌ᤹
#define Into2 RotateMotor(OUT_C,SPEED4,30);¡¡¡¡¡¡//¥Ü¡¼¥ë¤òÆþ¤ì¤ë(2²óÌÜ)
#define Return2 RotateMotor(OUT_C,-SPEED4,15);¡¡ //¥ì¡¼¥ë¤òÌ᤹
*¥×¥í¥°¥é¥à [#ec7cd753]
º£²ó»ÈÍѤ¹¤ë¥×¥í¥°¥é¥à¤Ï¾åµ¤Ë¤â¼¨¤·¤¿¥¢¡¼¥àŽ¥¥ì¡¼¥ë¤ÎÆ°ºî¤Î¥×¥í¥°¥é¥à¤Î¾¤Ë¡¢Ä¶²»ÇÈ¥»¥ó¥µ¤òÍøÍѤ·¤¿Æ°ºî¤Î¥×¥í¥°¥é¥à¡¢Bluetooth¤òÍøÍѤ·¤¿¥×¥í¥°¥é¥à¤Ç¤¢¤ë¡£
**Ķ²»ÇÈ¥»¥ó¥µ¤òÍøÍѤ·¤¿Æ°¤ [#l3e463dd]
¤³¤³¤Ç¾Ò²ð¤·¤Æ¤¤¤ë¥×¥í¥°¥é¥à¤ÏÇÛ¤é¤ì¤¿¥×¥ê¥ó¥È¤Ë½ñ¤«¤ì¤Æ¤¤¤ë»²¹Í¤Î¥×¥í¥°¥é¥à¤È¤Û¤ÜƱ¤¸¤â¤Î¤Ç¤¢¤ë¡£
#define SPEED 50
#define SPEED_SLOW 40
float GetAngle(float d) {¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¿Ê¤àµ÷Î¥¤ò·×»»¤¹¤ë
const float diameter=5.45;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¿¥¤¥ä¤Îľ·Â
const float pi=3.1415;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//±ß¼þΨ
float ang = d/(diameter*pi)*360.0; return ang; //³ÑÅÙ¤ò·×»»¤¹¤ë
}
const float diameter = 5.45;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡
const float track = 15;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¿¥¤¥ä¤Î¥È¥ì¥Ã¥ÉÉý
const float pi = 3.1415;
void fwdDist(float d) {¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//µ÷Î¥ d ÑÁ°¿Ê
long angle;
angle = d/(diameter*pi)*360.0;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,0,true,true);
}
void turnAng(long ang) {¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//³ÑÅÙ ang Å٤λþ·×²ó¤ê¤ÎÀû²ó
long angle;
angle = track/diameter * ang;
RotateMotorEx(OUT_BC,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_BC);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È
OnFwdSync(OUT_BC,SPEED_SLOW,-100);¡¡¡¡¡¡¡¡//È¿»þ·×²ó¤ê¤ËÀû²ó
while(MotorTachoCount(OUT_B)<=angle){
if(SensorUS(S1)<d_min){
d_min=SensorUS(S1);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//²¾¤ÎºÇ¾®Ãͤò¹¹¿·
PlaySound(SOUND_UP);
tacho_min=MotorTachoCount(OUT_B);
}
}
OnFwdSyncEx(OUT_BC,SPEED_SLOW,100,RESET_NONE);
until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S1)<=d_min);
Wait(14);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À°
Off(OUT_BC);Wait(500);
return d_min;
}
task main()
{
int d = searchDirection(120);¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¸½ºß¤ÎÊý¸þ¤òÃæ¿´¤Ë 120 ÅÙ¤ÎÈϰϤÇõ¤·¡¢¾ã³²Êª¤Þ¤Ç¤Îµ÷Î¥¤òÊÖ¤¹
if(d > ¡û){
fwdDist(d-¡û);}¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡
else {fwdDist(d-¡û);}¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//»æ¥³¥Ã¥×¤ÎÊý¸þ¤òõ¤·¤¿¸å²»¤òÌĤ餷¡¢Á°¸å¤ÎÆ°¤¤Ç¡ûѼêÁ°¤ÇÄä»ß
PlaySound(SOUND_UP);
}
**Bluetooth¤òÍøÍѤ·¤¿¥×¥í¥°¥é¥à [#x935b665]
2¤Ä¤ÎNXC¤¬Bluetooth¤Ç¤Ä¤Ê¤¬¤Ã¤Æ¤¤¤ë¾õÂ֤Ǥ¢¤ë¤³¤È¤òÁ°Äó¤ÇÏäò¿Ê¤á¤ë¡£
¤³¤³¤Ç¤Ï¥Þ¥¹¥¿¡¼Â¦¤«¤é¥¹¥ì¡¼¥Ö¦¤Ë»Ø¼¨¤ò½Ð¤¹¥×¥í¥°¥é¥à¤ò¾Ò²ð¤¹¤ë¡£¡¡
#define CONN 1 ¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¹¥ì¡¼¥Ö(ÄÌ¿®Áê¼ê)¤ÎÀܳÈÖ¹æ
task main()
{
until (BluetoothStatus(CONN) == NO_ERR);¡¡¡¡//Àܳ¤Ç¤¤ë¤Þ¤ÇÂÔµ¡
//¥Þ¥¹¥¿¡¼Â¦¤Î¥×¥í¥°¥é¥à//
RemoteStartProgram(CONN, "¡Á.rxe");¡¡¡¡¡¡¡¡¡¡//¥¹¥ì¡¼¥Ö¤Î¡Ö¡Á¡×¤Î¥×¥í¥°¥é¥à¤òµ¯Æ°
}¡¡
µÕ¤Ë¥¹¥ì¡¼¥Ö¦¤«¤é¥Þ¥¹¥¿¡¼Â¦¤Ë»Ø¼¨¤ò½Ð¤¹¾ì¹ç¤ÏÀܳÈÖ¹æ¤ò0¤ÈÊѹ¹¤¹¤ë¡£
**ÌäÂêÅÀ¡¦²þÁ±ÅÀ [#l23eb6c6]
¥¢¡¼¥à¤ä¥ì¡¼¥ë¤ÎÆ°ºî¤ò¤¹¤ë¥×¥í¥°¥é¥à¤Ç¡¢»Ï¤á¤ÏÁ´¤Æ³ÑÅÙ¤ò»ØÄꤵ¤»¤Æ¥â¡¼¥¿¡¼¤òÆ°¤«¤·¤Æ¤¤¤¿¤¬¡¢¥â¡¼¥¿¡¼¤¬¤½¤Î³ÑÅÙ¤ò¤Ô¤Ã¤¿¤ê²ó¤é¤Ê¤¤¸Â¤êÆ°¤Â³¤±¤ë¡¢¤¹¤Ê¤ï¤Á¾¤ÎÉôÉÊÅù¤Ç¤Ò¤Ã¤«¤«¤Ã¤Æ¤¤¤¿¤é¤½¤ÎÆ°ºî¤Ï½ª¤ï¤é¤Ê¤¤¤«¤é¼¡¤ÎÆ°ºî¤Ë°Ü¤êÊѤï¤é¤Ê¤¤¤³¤È¤¬ÅÓÃæ¤ÇȽÌÀ¤·¤¿¤¿¤á¡¢¤Ê¤ë¤Ù¤¯³ÑÅÙ»ØÄê¤ò¤»¤º¤Ë½ÐÎϤȻþ´Ö¤Ç¥â¡¼¥¿¡¼¤òÆ°¤«¤·¡¢³Î¼Â¤ËÆ°ºî¤ò¤µ¤»¤ë¤è¤¦¤Ë²þÁ±¤·¤¿¡£
º£¤Þ¤Ç¾Ò²ð¤·¤Æ¤¤¿¥×¥í¥°¥é¥à¤òÊ£¿ô¤Î¥×¥í¥°¥é¥à¤Ëʬ¤±¡¢Bluetooth¤òÍøÍѤ¹¤ë¤³¤È¤ÇÁ´¤Æ·Ò¤®¹ç¤ï¤»¤è¤¦¤È»î¤ß¤¿¤¬¡¢¥Þ¥¹¥¿¡¼Â¦¤«¤é¥¹¥ì¡¼¥Ö¦¤Ë¥×¥í¥°¥é¥à¤ò»Ø¼¨½Ð¤¹¤³¤È¤ÏÀ®¸ù¤·¤¿¤â¤Î¤Î¡¢Â³¤±¤Æ¥¹¥ì¡¼¥Ö¦¤«¤é¥Þ¥¹¥¿¡¼Â¦¤Î¥×¥í¥°¥é¥à¤òµ¯Æ°¤µ¤»¤è¤¦¤È¤¹¤ë¤È¥Þ¥¹¥¿¡¼Â¦¤¬È¿±þ¤·¤Æ¤¯¤ì¤Ê¤¤ÌäÂ꤬µ¯¤¤¿¡£¥Þ¥¹¥¿¡¼Â¦¤È¥¹¥ì¡¼¥Ö¦¤ÎNXC¤òÊѤ¨¤Æ¤ß¤¿¤ê¡¢¥×¥í¥°¥é¥à¤ò¾¯¤·¤À¤±ÊѤ¨¤Æ¤ß¤¿¤ê¤·¤Æ»î¤·¤¿¤â¤Î¤Î¡¢È¿±þ¤·¤Ê¤¤¤³¤È¤ËÊѤï¤ê¤Ï¤Ê¤«¤Ã¤¿¤¿¤á¡¢¥Þ¥¹¥¿¡¼Â¦¤Î¥×¥í¥°¥é¥à¤ò1¤Ä¤Ë¤Þ¤È¤á¤Æ¡¢¥¹¥ì¡¼¥Ö¦¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤·¤Æ¤¤¤ë»þ¤ÏWait¤òÍøÍѤ·¤ÆÂÔµ¡¤µ¤»¡¢¥¹¥ì¡¼¥Ö¦¤Î¥×¥í¥°¥é¥à¤¬½ª¤ï¤ì¤Ð¥Þ¥¹¥¿¡¼Â¦¤Î¼¡¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤µ¤»¤ë¤è¤¦Êѹ¹¤·¤¿¡£
**¥¹¥ì¡¼¥Ö¦ [#p93594c5]
***¥¢¡¼¥àÆ°ºî¡Êarm time¡Ë [#dd328ea5]
#define SPEED1 45
#define SPEED2 60
#define SPEED3 10
#define SPEED4 30
#define SPEED5 50
#define SPEED6 25
float GetAngle(float d)
{
const float diameter=5.45;
const float pi=3.1415;
float ang = d/(diameter*pi)*360.0;
return ang;
}
#define DOWN RotateMotor(OUT_A,SPEED3,105);
#define UP RotateMotor(OUT_A,-SPEED5,105);
#define LDown RotateMotor(OUT_A,SPEED3,25);
#define LUp RotateMotor(OUT_A,-SPEED2,25);
#define Catch RotateMotor(OUT_B,-SPEED4,105);
#define Separate RotateMotor(OUT_B,SPEED4,105);
task main ()
{
OnFwd(OUT_A,10);
Wait(1500);
Catch; ¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¢¡¼¥àÁ´ÂΤò²¼¤²¡¢»æ¥³¥Ã¥×¤ò¤Ä¤«¤à
OnRev(OUT_A,50);
Wait(3500);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¢¡¼¥àÁ´ÂΤò¾å¤²¡¢¥ì¡¼¥ë¤Î¾å¤Ë¥Ü¡¼¥ë¤ò¤Î¤»¤ë
Wait(2000);
OnFwd(OUT_A,10);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡
Wait(3500);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¢¡¼¥àÁ´ÂΤò²¼¤²¡¢»æ¥³¥Ã¥×¤òÃÖ¤¯
Separate;
OnRev(OUT_A,35);
Wait(2000); ¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¢¡¼¥àÁ´ÂΤò¾å¤²¤ë(¸µ¤Î°ÌÃÖ¤ËÌ᤹)
Wait(500);
}
***¥ì¡¼¥ëÆ°ºî¡Êrail¡Ë [#u382c2a1]
#define SPEED1 45
#define SPEED2 60
#define SPEED3 10
#define SPEED4 30
#define SPEED5 50
float GetAngle(float d)
{
const float diameter=5.45;
const float pi=3.1415;
float ang = d/(diameter*pi)*360.0;
return ang;
}
#define Into1 RotateMotor(OUT_C,SPEED3,20); //¥Ü¡¼¥ë¤òÆþ¤ì¤ë
#define Return1 RotateMotor(OUT_C,-SPEED3,25);
#define Into2 RotateMotor(OUT_C,SPEED4,30);
#define Return2 RotateMotor(OUT_C,-SPEED4,15);
task main(){
OnFwd(OUT_C,15);
Wait(1000);
Wait(3000); ¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥ì¡¼¥ë¤ò·¹¤±¡¢4¤Ä¤ÎÆâ2¤Ä¤À¤±¥Ü¡¼¥ë¤ò»æ¥³¥Ã¥×¤ËÆþ¤ì¤ë
OnRev(OUT_C,30);
Wait(500);
Wait(5000);
OnFwd(OUT_C,25);
Wait(1000);
Wait(3000); ¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥ì¡¼¥ë¤ò·¹¤±¡¢¥ì¡¼¥ë¤Ë»Ä¤Ã¤Æ¤¤¤ë¥Ü¡¼¥ë¤ò»æ¥³¥Ã¥×¤ËÆþ¤ì¤ë
OnRev(OUT_C,30);
Wait(2500);
}
**¥Þ¥¹¥¿¡¼Â¦ [#wa6a612a]
¾åµ¤Î¥×¥í¥°¥é¥à¤ÎÀâÌÀ¤ÇÍøÍѤ·¤¿ÄêµÁ¤Ë¥×¥é¥¹¤¹¤ë¡£
#define turn_right_here OnFwd(OUT_B,SPEED_SLOW);OnFwd(OUT_C,-SPEED_SLOW); //±¦Àû²ó
***ËÜÈÖÍѤËÍÑ°Õ¤·¤¿¥×¥í¥°¥é¥à [#ne735f23]
task main() {
SetSensorLowspeed(S1);
until (BluetoothStatus(CONN) == NO_ERR);
OnFwd(OUT_BC,50);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//100cmÁ°¤ËÁ°¿Ê¤¹¤ë
Wait(5500);
int d1 = searchDirection(120);¡¡¡¡¡¡¡¡¡¡//»æ¥³¥Ã¥×¤ÎÊý¸þ¤òõ¤¹
if(d1 > 15){
fwdDist(d1-15.0);}
else {fwdDist(d1-15);}¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ºÇÄãÃͤòȯ¸«¤·¤¿¤é²»¤òÌĤ餷¡¢Á°¸å¤ÎÆ°¤¤Ç15ѼêÁ°¤ÇÄä»ß
PlaySound(SOUND_UP);
turn_right_here;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¢¡¼¥à¤¬ÄϤá¤ë°ÌÃ֤ޤǥí¥Ü¥Ã¥ÈÁ´ÂΤòÀû²ó¤µ¤»¤ë
Wait(2500);
RemoteStartProgram(CONN, "arm time.rxe");¡¡//¥¹¥ì¡¼¥Ö¤Î¡Öarm time¡×¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤µ¤»¤ë
Off(OUT_BC);
Wait(10000);
Wait(10000);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¹¥ì¡¼¥Ö¦¤Î¥×¥í¥°¥é¥à¼Â¹ÔÃæ¤Î¤¿¤áÂÔµ¡
Wait(10000);
int angle1 = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle1);¡¡¡¡¡¡¡¡//20cm¸å¿Ê¤¹¤ë
OnFwd(OUT_B,-SPEED);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡
OnFwd(OUT_C,SPEED);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//180ÅÙ²óž
Wait(1500);
angle1 += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle1);¡¡¡¡¡¡¡¡//60cmÁ°¿Ê¤¹¤ë
int d2 = searchDirection(120);
if(d2 > 15){
fwdDist(d2-15.0);}
else {fwdDist(d2-15);}
PlaySound(SOUND_UP);
turn_right_here;
Wait(200);
RemoteStartProgram(CONN, "arm time.rxe");
Off(OUT_BC);
Wait(10000);
Wait(10000);
Wait(10000);
int angle2 = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle2);
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(1500);
angle2 += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle2);
int d3 = searchDirection(120);
turn_right_here;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À°
Wait(600);
if(d3 > 15){
fwdDist(d3);}
else {fwdDist(d3);}¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//»æ¥³¥Ã¥×¤ò¸«¤Ä¤±¤¿¤é¡¢Î¥¤ì¤Æ¤¤¤ëµ÷Î¥¤À¤±Á°¿Ê(¥ì¡¼¥ë¤Î²¼¤Ë»æ¥³¥Ã¥×¤ò»ý¤Ã¤Æ¤¤¤¯¤¿¤á¤ÎÆ°ºî)
PlaySound(SOUND_UP);
RemoteStartProgram(CONN, "rail.rxe");¡¡//¥¹¥ì¡¼¥Ö¤Î¡Örail¡×¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤µ¤»¤ë
Off(OUT_BC);
Wait(10000);
Wait(10000);
Wait(10000);
int angle3 = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle3);
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(1500);
angle3 += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle3);
int d4 = searchDirection(120);
turn_right_here;
Wait(600);
if(d4 > 15){
fwdDist(d4);}
else {fwdDist(d4);}
PlaySound(SOUND_UP);
RemoteStartProgram(CONN, "rail.rxe");
}
***ºÇ½ªÅª¤Ë»ÈÍѤ·¤¿¥×¥í¥°¥é¥à [#sb4129f9]
ËÜÈÖ¤ÇsearchDirection¤¬¾å¼ê¤¯ºîÆ°¤·¤Ê¤«¤Ã¤¿¤¿¤á¡¢ºÇ½ª¼êÃʤȤ·¤ÆĶ²»ÇÈ¥»¥ó¥µ¤òÍøÍѤ»¤º¤Ë¥Ü¡¼¥ë¤ÎÆþ¤Ã¤¿»æ¥³¥Ã¥×¤Î¤È¤³¤í¤Þ¤Ç¹Ô¤«¤»¡¢Ê̤λ楳¥Ã¥×¤Ë¥Ü¡¼¥ë¤òÆþ¤ì¤ë¤È¤³¤í¤Þ¤Ç¡¢¤Û¤ÜOnFwd¤ò»È¤Ã¤Æ°ÜÆ°¤µ¤»¤¿¡£
task main() {
SetSensorLowspeed(S1);
until (BluetoothStatus(CONN) == NO_ERR);
OnFwd(OUT_BC,50);
Wait(5500);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//100cm¤Û¤ÉÁ°¿Ê¤¹¤ë
turn_right_here;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¢¡¼¥à¤¬ÄϤá¤ë°ÌÃÖ¤Þ¤ÇÀû²ó¤¹¤ë
Wait(2000);
RemoteStartProgram(CONN, "arm time.rxe");¡¡//¥¹¥ì¡¼¥Ö¤Î¡Örail¡×¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤µ¤»¤ë
Off(OUT_BC);
Wait(10000);
Wait(10000);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¹¥ì¡¼¥Ö¦¤Î¥×¥í¥°¥é¥à¼Â¹ÔÃæ¤Î¤¿¤áÂÔµ¡
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(700);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//Ê̤λ楳¥Ã¥×¤ò¥ì¡¼¥ë²¼¤Ë»ý¤Ã¤Æ¤¤¤¯¤¿¤á¤Î¥×¥í¥°¥é¥à
OnFwd(OUT_BC,50);
Wait(4000);
RemoteStartProgram(CONN, "rail.rxe");¡¡¡¡¡¡¡¡//¥¹¥ì¡¼¥Ö¤Î¡Örail¡×¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤µ¤»¤ë
Off(OUT_BC);
}
¤³¤Î¥×¥í¥°¥é¥à¤ÇËÜÈÖ¤Ï1¤Ä¤Î¥Ü¡¼¥ë¤Î°ÜÆ°¤ËÀ®¸ù¤¹¤ë¤³¤È¤¬½ÐÍ褿¡£
*´¶ÁÛ [#f09587f0]
¥í¥Ü¥Ã¥È¡¦¥×¥í¥°¥é¥à¤ÎÀ½ºî¤¬º£¤Þ¤Ç¤ÈÈæ¤Ùʪ¤Ë¤Ê¤é¤Ê¤¤¤Û¤ÉÂçÊѤǡ¢¤·¤«¤â»þ´Ö¤ò¤«¤±¤¿Ê¬¡¢Á´Éô¤¬Á´Éô·ë²Ì¤Ë¤Ê¤ë¤È¤Ï¸Â¤é¤Ê¤¤¤³¤È¤ò»×¤¤ÃΤ餵¤ì¤¿µ¤¤¬¤·¤¿¡£¥×¥í¥°¥é¥à¤ÎÎ̤¬Â¿¤¯¤Ê¤ì¤Ð¤Ê¤ë¤Û¤É¥ß¥¹¤¬µ¯¤³¤ê¤ä¤¹¤¤¡¢¤Þ¤¿Ê£»¨¤Ê¥×¥í¥°¥é¥à¤Ê¤Û¤É1¤Ä1¤Ä¤Á¤ã¤ó¤ÈÍý²ò¤·¤Æ³Îǧ¤·¤Ê¤¤¤È¤¤¤±¤Ê¤¤¤³¤È¤Ëµ¤¤Å¤«¤µ¤ì¤¿¡£¤³¤ì¤«¤é¤â¥í¥Ü¥Ã¥È¤òÀ½ºî¤·¤Æ¥×¥í¥°¥é¥à¤òºî¤Ã¤Æ¤¤¤¯µ¡²ñ¤¬¤¢¤ë¤Î¤Ç¡¢¤³¤Î¥¼¥ß¤Ç³Ø¤ó¤À¤³¤È¤ò³è¤«¤·¤Æ¤¤¤¤¿¤¤¤È»×¤¦¡£
½ªÎ»¹Ô:
[[2014b/Member]]
#contents
*¤Ï¤¸¤á¤Ë [#o4d8a879]
²ÝÂê3¤Ï¥¹¥¿¡¼¥ÈÃÏÅÀ¤«¤é¥í¥Ü¥Ã¥È¤ò¿Ê¤á¡¢2¤Ä¤Î»æ¥³¥Ã¥×¤Ë¤½¤ì¤¾¤ìÆþ¤Ã¤Æ¤¤¤ë2¸Ä¤Î¥Ü¡¼¥ë¤òÆþ¤ìÂؤ¨¤ë¤È¤¤¤¦¤â¤Î¤Ç¤¢¤ë¡£
([[2014ǯ¸å´ü/²ÝÂê3:http://yakushi.shinshu-u.ac.jp/robotics/?2014b%2FMission3]]»²¾È)
¡¡»ä¤¿¤Á¤ÎÈɤΥá¥ó¥Ð¡¼¤Ï¡¢
-[[edaeda:http://yakushi.shinshu-u.ac.jp/robotics/?2014b%2FMember%2Fedaeda%2FMission3]]
-[[knee:http://yakushi.shinshu-u.ac.jp/robotics/?2014b%2FMember%2Fknee%2FMission3]]
- kuryu
¤È»ä¤Î4¿Í¤Ç¤¢¤ë¡£
»ä¤¿¤Á¤ÎÈɤιͰƤϡ¢¥¢¡¼¥à¤È¥ì¡¼¥ë¤òºî¤ê¡¢¥¢¡¼¥à¤ò»È¤Ã¤Æ£´¤Ä¤Î¥Ü¡¼¥ë¤ò¥ì¡¼¥ë¤Ë¤Î¤»¡¢¥ì¡¼¥ë¤ò·¹¤«¤»¤ë¤³¤È¤Ç»æ¥³¥Ã¥×¤Ë¥Ü¡¼¥ë¤òÆþ¤ì¤ë¤È¤¤¤¦¤â¤Î¤Ç¤¢¤ë¡£
*¥í¥Ü¥Ã¥È¡¢¤Þ¤¿¤Ï¤½¤ÎÆ°ºî¤ÎÀâÌÀ [#h9193c48]
**Á´ÂÎ [#ne8db113]
#ref(2014b/Member/arisyka/Mission3/IMG_3870.JPG,20%)
¼Ì¿¿¤Î¤è¤¦¤Ê¹½Â¤¤Ç¤¢¤ë¡£
±¦¤Èº¸¤Î¥¿¥¤¥ä¤ÎÉé²Ù¤¬¤½¤ì¤¾¤ì°Û¤Ê¤ë¤¿¤á¡¢¥×¥í¥°¥é¥à¤Çº¸±¦¤Î¥¿¥¤¥ä¤Î½ÐÎϤòÊѤ¨¤ë¤³¤È¤Çľ¿Ê¤ä²óž¡¢Àû²ó¤ò¥¹¥à¡¼¥º¤Ë¹Ô¤ï¤»¤ë¡£
**¥¢¡¼¥à [#y4bd3434]
#ref(2014b/Member/arisyka/Mission3/IMG_3869.gif,30%)
¾å¤Î¼Ì¿¿¤ÎÀ֤ǰϤޤ줿Éôʬ¡£
1¤Î¥â¡¼¥¿¡¼¤Ç¥¢¡¼¥àÁ´ÂΤò¾å¤²²¼¤²¤·¡¢2¤Î¥â¡¼¥¿¡¼¤Ç¥¢¡¼¥à¤Î¶´¤àÆ°¤¤òÁàºî¤¹¤ë¡£
¥¢¡¼¥à¤òÆ°¤«¤¹¤¿¤á¤Ë»È¤Ã¤¿ÄêµÁ¤Ï°Ê²¼¤Î¤è¤¦¤Ç¤¢¤ë¡£
#define SPEED1 45
#define SPEED2 60
#define SPEED3 10
#define SPEED4 30
#define SPEED5 50
#define SPEED6 25
float GetAngle(float d) {¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¿Ê¤àµ÷Î¥¤ò·×»»¤¹¤ë
const float diameter=5.45;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¿¥¤¥ä¤Îľ·Â
const float pi=3.1415;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//±ß¼þΨ
float ang = d/(diameter*pi)*360.0; return ang; //³ÑÅÙ¤ò·×»»¤¹¤ë
}
¤³¤³¤ÎÄêµÁ¤Ï¥¢¡¼¥àÁ´ÂΤÎÆ°¤¤òÄêµÁ¤·¤Æ¤¤¤ë¡£(¾å¤Î¼Ì¿¿¤Î¡¤Î¥â¡¼¥¿¡¼¤Ë¤è¤ëÆ°¤)
#define DOWN RotateMotor(OUT_A,SPEED3,105);¡¡¡¡//¥¢¡¼¥àÁ´ÂΤò²¼¤²¤ë
#define UP RotateMotor(OUT_A,-SPEED5,105); ¡¡//¥¢¡¼¥àÁ´ÂΤò¾å¤²¤ë
#define LDown RotateMotor(OUT_A,SPEED3,25);¡¡¡¡
#define LUp RotateMotor(OUT_A,-SPEED2,25);
#define Little LDown;Wait(2000);LUp; //¥Ü¡¼¥ë¤ò¥ì¡¼¥ë¤Ë³Î¼Â¤Ë¤Î¤»¤ë¤¿¤á¤ÎÆ°¤
#ref(2014b/Member/arisyka/Mission3/arm¥ë¡¼¥×2.gif,45%)
¤³¤³¤ÎÄêµÁ¤Ï¥¢¡¼¥à¤Î¥Ï¥µ¥ßÉôʬ¤ÎÆ°¤¤òÄêµÁ¤·¤Æ¤¤¤ë¡£(¾å¤Î¼Ì¿¿¤Î¢¤Î¥â¡¼¥¿¡¼¤Ë¤è¤ëÆ°¤)
#define Catch RotateMotor(OUT_B,-SPEED4,105);¡¡¡¡//¥¢¡¼¥à¤Ç¤Ä¤«¤à
#define Separate RotateMotor(OUT_B,SPEED4,105); //¥¢¡¼¥à¤òÎ¥¤¹
#ref(2014b/Member/arisyka/Mission3/catch&separate.gif,40%)
**¥ì¡¼¥ë [#k920454a]
#ref(2014b/Member/arisyka/Mission3/IMG_3873.gif,30%)
¾å¤Î¼Ì¿¿¤ÎÀ֤ǰϤޤ줿Éôʬ¡£¤Þ¤¿¡¢Üô¿§¤Ç°Ï¤Þ¤ì¤¿Éôʬ¤Ï¥Ü¡¼¥ë¤ò»æ¥³¥Ã¥×¤ËÆþ¤ì¤ëºÝ¤Ë¡¢»æ¥³¥Ã¥×¤¬Åݤì¤Ê¤¤¤è¤¦¤Ë¤¹¤ë¤¿¤á¤Î¥¹¥È¥Ã¥Ñ¡¼¤Ç¤¢¤ë¡£Åݤ¹³ÑÅÙ¤òÄ´À°¤¹¤ë¤³¤È¤Ç»æ¥³¥Ã¥×¤ËÆþ¤ì¤ë¥Ü¡¼¥ë¤Î¿ô¤òÊѤ¨¤ë¡£
¥ì¡¼¥ë¤òÆ°¤«¤¹¤¿¤á¤Ë»È¤Ã¤¿Æ°ºî¤Ï¾åµ¤Ë½ñ¤¤¤¿¥¢¡¼¥à¤ÎÄêµÁ¤ÈƱ¤¸¤â¤Î¤ò»ÈÍѤ·¤¿¡£
#define Into1 RotateMotor(OUT_C,SPEED3,20); ¡¡¡¡//¥Ü¡¼¥ë¤òÆþ¤ì¤ë(1²óÌÜ)
#define Return1 RotateMotor(OUT_C,-SPEED3,25);¡¡ //¥ì¡¼¥ë¤òÌ᤹
#define Into2 RotateMotor(OUT_C,SPEED4,30);¡¡¡¡¡¡//¥Ü¡¼¥ë¤òÆþ¤ì¤ë(2²óÌÜ)
#define Return2 RotateMotor(OUT_C,-SPEED4,15);¡¡ //¥ì¡¼¥ë¤òÌ᤹
*¥×¥í¥°¥é¥à [#ec7cd753]
º£²ó»ÈÍѤ¹¤ë¥×¥í¥°¥é¥à¤Ï¾åµ¤Ë¤â¼¨¤·¤¿¥¢¡¼¥àŽ¥¥ì¡¼¥ë¤ÎÆ°ºî¤Î¥×¥í¥°¥é¥à¤Î¾¤Ë¡¢Ä¶²»ÇÈ¥»¥ó¥µ¤òÍøÍѤ·¤¿Æ°ºî¤Î¥×¥í¥°¥é¥à¡¢Bluetooth¤òÍøÍѤ·¤¿¥×¥í¥°¥é¥à¤Ç¤¢¤ë¡£
**Ķ²»ÇÈ¥»¥ó¥µ¤òÍøÍѤ·¤¿Æ°¤ [#l3e463dd]
¤³¤³¤Ç¾Ò²ð¤·¤Æ¤¤¤ë¥×¥í¥°¥é¥à¤ÏÇÛ¤é¤ì¤¿¥×¥ê¥ó¥È¤Ë½ñ¤«¤ì¤Æ¤¤¤ë»²¹Í¤Î¥×¥í¥°¥é¥à¤È¤Û¤ÜƱ¤¸¤â¤Î¤Ç¤¢¤ë¡£
#define SPEED 50
#define SPEED_SLOW 40
float GetAngle(float d) {¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¿Ê¤àµ÷Î¥¤ò·×»»¤¹¤ë
const float diameter=5.45;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¿¥¤¥ä¤Îľ·Â
const float pi=3.1415;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//±ß¼þΨ
float ang = d/(diameter*pi)*360.0; return ang; //³ÑÅÙ¤ò·×»»¤¹¤ë
}
const float diameter = 5.45;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡
const float track = 15;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¿¥¤¥ä¤Î¥È¥ì¥Ã¥ÉÉý
const float pi = 3.1415;
void fwdDist(float d) {¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//µ÷Î¥ d ÑÁ°¿Ê
long angle;
angle = d/(diameter*pi)*360.0;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,0,true,true);
}
void turnAng(long ang) {¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//³ÑÅÙ ang Å٤λþ·×²ó¤ê¤ÎÀû²ó
long angle;
angle = track/diameter * ang;
RotateMotorEx(OUT_BC,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_BC);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//³ÑÅٷ׬¤ò¥ê¥»¥Ã¥È
OnFwdSync(OUT_BC,SPEED_SLOW,-100);¡¡¡¡¡¡¡¡//È¿»þ·×²ó¤ê¤ËÀû²ó
while(MotorTachoCount(OUT_B)<=angle){
if(SensorUS(S1)<d_min){
d_min=SensorUS(S1);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//²¾¤ÎºÇ¾®Ãͤò¹¹¿·
PlaySound(SOUND_UP);
tacho_min=MotorTachoCount(OUT_B);
}
}
OnFwdSyncEx(OUT_BC,SPEED_SLOW,100,RESET_NONE);
until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S1)<=d_min);
Wait(14);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À°
Off(OUT_BC);Wait(500);
return d_min;
}
task main()
{
int d = searchDirection(120);¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¸½ºß¤ÎÊý¸þ¤òÃæ¿´¤Ë 120 ÅÙ¤ÎÈϰϤÇõ¤·¡¢¾ã³²Êª¤Þ¤Ç¤Îµ÷Î¥¤òÊÖ¤¹
if(d > ¡û){
fwdDist(d-¡û);}¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡
else {fwdDist(d-¡û);}¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//»æ¥³¥Ã¥×¤ÎÊý¸þ¤òõ¤·¤¿¸å²»¤òÌĤ餷¡¢Á°¸å¤ÎÆ°¤¤Ç¡ûѼêÁ°¤ÇÄä»ß
PlaySound(SOUND_UP);
}
**Bluetooth¤òÍøÍѤ·¤¿¥×¥í¥°¥é¥à [#x935b665]
2¤Ä¤ÎNXC¤¬Bluetooth¤Ç¤Ä¤Ê¤¬¤Ã¤Æ¤¤¤ë¾õÂ֤Ǥ¢¤ë¤³¤È¤òÁ°Äó¤ÇÏäò¿Ê¤á¤ë¡£
¤³¤³¤Ç¤Ï¥Þ¥¹¥¿¡¼Â¦¤«¤é¥¹¥ì¡¼¥Ö¦¤Ë»Ø¼¨¤ò½Ð¤¹¥×¥í¥°¥é¥à¤ò¾Ò²ð¤¹¤ë¡£¡¡
#define CONN 1 ¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¹¥ì¡¼¥Ö(ÄÌ¿®Áê¼ê)¤ÎÀܳÈÖ¹æ
task main()
{
until (BluetoothStatus(CONN) == NO_ERR);¡¡¡¡//Àܳ¤Ç¤¤ë¤Þ¤ÇÂÔµ¡
//¥Þ¥¹¥¿¡¼Â¦¤Î¥×¥í¥°¥é¥à//
RemoteStartProgram(CONN, "¡Á.rxe");¡¡¡¡¡¡¡¡¡¡//¥¹¥ì¡¼¥Ö¤Î¡Ö¡Á¡×¤Î¥×¥í¥°¥é¥à¤òµ¯Æ°
}¡¡
µÕ¤Ë¥¹¥ì¡¼¥Ö¦¤«¤é¥Þ¥¹¥¿¡¼Â¦¤Ë»Ø¼¨¤ò½Ð¤¹¾ì¹ç¤ÏÀܳÈÖ¹æ¤ò0¤ÈÊѹ¹¤¹¤ë¡£
**ÌäÂêÅÀ¡¦²þÁ±ÅÀ [#l23eb6c6]
¥¢¡¼¥à¤ä¥ì¡¼¥ë¤ÎÆ°ºî¤ò¤¹¤ë¥×¥í¥°¥é¥à¤Ç¡¢»Ï¤á¤ÏÁ´¤Æ³ÑÅÙ¤ò»ØÄꤵ¤»¤Æ¥â¡¼¥¿¡¼¤òÆ°¤«¤·¤Æ¤¤¤¿¤¬¡¢¥â¡¼¥¿¡¼¤¬¤½¤Î³ÑÅÙ¤ò¤Ô¤Ã¤¿¤ê²ó¤é¤Ê¤¤¸Â¤êÆ°¤Â³¤±¤ë¡¢¤¹¤Ê¤ï¤Á¾¤ÎÉôÉÊÅù¤Ç¤Ò¤Ã¤«¤«¤Ã¤Æ¤¤¤¿¤é¤½¤ÎÆ°ºî¤Ï½ª¤ï¤é¤Ê¤¤¤«¤é¼¡¤ÎÆ°ºî¤Ë°Ü¤êÊѤï¤é¤Ê¤¤¤³¤È¤¬ÅÓÃæ¤ÇȽÌÀ¤·¤¿¤¿¤á¡¢¤Ê¤ë¤Ù¤¯³ÑÅÙ»ØÄê¤ò¤»¤º¤Ë½ÐÎϤȻþ´Ö¤Ç¥â¡¼¥¿¡¼¤òÆ°¤«¤·¡¢³Î¼Â¤ËÆ°ºî¤ò¤µ¤»¤ë¤è¤¦¤Ë²þÁ±¤·¤¿¡£
º£¤Þ¤Ç¾Ò²ð¤·¤Æ¤¤¿¥×¥í¥°¥é¥à¤òÊ£¿ô¤Î¥×¥í¥°¥é¥à¤Ëʬ¤±¡¢Bluetooth¤òÍøÍѤ¹¤ë¤³¤È¤ÇÁ´¤Æ·Ò¤®¹ç¤ï¤»¤è¤¦¤È»î¤ß¤¿¤¬¡¢¥Þ¥¹¥¿¡¼Â¦¤«¤é¥¹¥ì¡¼¥Ö¦¤Ë¥×¥í¥°¥é¥à¤ò»Ø¼¨½Ð¤¹¤³¤È¤ÏÀ®¸ù¤·¤¿¤â¤Î¤Î¡¢Â³¤±¤Æ¥¹¥ì¡¼¥Ö¦¤«¤é¥Þ¥¹¥¿¡¼Â¦¤Î¥×¥í¥°¥é¥à¤òµ¯Æ°¤µ¤»¤è¤¦¤È¤¹¤ë¤È¥Þ¥¹¥¿¡¼Â¦¤¬È¿±þ¤·¤Æ¤¯¤ì¤Ê¤¤ÌäÂ꤬µ¯¤¤¿¡£¥Þ¥¹¥¿¡¼Â¦¤È¥¹¥ì¡¼¥Ö¦¤ÎNXC¤òÊѤ¨¤Æ¤ß¤¿¤ê¡¢¥×¥í¥°¥é¥à¤ò¾¯¤·¤À¤±ÊѤ¨¤Æ¤ß¤¿¤ê¤·¤Æ»î¤·¤¿¤â¤Î¤Î¡¢È¿±þ¤·¤Ê¤¤¤³¤È¤ËÊѤï¤ê¤Ï¤Ê¤«¤Ã¤¿¤¿¤á¡¢¥Þ¥¹¥¿¡¼Â¦¤Î¥×¥í¥°¥é¥à¤ò1¤Ä¤Ë¤Þ¤È¤á¤Æ¡¢¥¹¥ì¡¼¥Ö¦¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤·¤Æ¤¤¤ë»þ¤ÏWait¤òÍøÍѤ·¤ÆÂÔµ¡¤µ¤»¡¢¥¹¥ì¡¼¥Ö¦¤Î¥×¥í¥°¥é¥à¤¬½ª¤ï¤ì¤Ð¥Þ¥¹¥¿¡¼Â¦¤Î¼¡¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤µ¤»¤ë¤è¤¦Êѹ¹¤·¤¿¡£
**¥¹¥ì¡¼¥Ö¦ [#p93594c5]
***¥¢¡¼¥àÆ°ºî¡Êarm time¡Ë [#dd328ea5]
#define SPEED1 45
#define SPEED2 60
#define SPEED3 10
#define SPEED4 30
#define SPEED5 50
#define SPEED6 25
float GetAngle(float d)
{
const float diameter=5.45;
const float pi=3.1415;
float ang = d/(diameter*pi)*360.0;
return ang;
}
#define DOWN RotateMotor(OUT_A,SPEED3,105);
#define UP RotateMotor(OUT_A,-SPEED5,105);
#define LDown RotateMotor(OUT_A,SPEED3,25);
#define LUp RotateMotor(OUT_A,-SPEED2,25);
#define Catch RotateMotor(OUT_B,-SPEED4,105);
#define Separate RotateMotor(OUT_B,SPEED4,105);
task main ()
{
OnFwd(OUT_A,10);
Wait(1500);
Catch; ¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¢¡¼¥àÁ´ÂΤò²¼¤²¡¢»æ¥³¥Ã¥×¤ò¤Ä¤«¤à
OnRev(OUT_A,50);
Wait(3500);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¢¡¼¥àÁ´ÂΤò¾å¤²¡¢¥ì¡¼¥ë¤Î¾å¤Ë¥Ü¡¼¥ë¤ò¤Î¤»¤ë
Wait(2000);
OnFwd(OUT_A,10);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡
Wait(3500);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¢¡¼¥àÁ´ÂΤò²¼¤²¡¢»æ¥³¥Ã¥×¤òÃÖ¤¯
Separate;
OnRev(OUT_A,35);
Wait(2000); ¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¢¡¼¥àÁ´ÂΤò¾å¤²¤ë(¸µ¤Î°ÌÃÖ¤ËÌ᤹)
Wait(500);
}
***¥ì¡¼¥ëÆ°ºî¡Êrail¡Ë [#u382c2a1]
#define SPEED1 45
#define SPEED2 60
#define SPEED3 10
#define SPEED4 30
#define SPEED5 50
float GetAngle(float d)
{
const float diameter=5.45;
const float pi=3.1415;
float ang = d/(diameter*pi)*360.0;
return ang;
}
#define Into1 RotateMotor(OUT_C,SPEED3,20); //¥Ü¡¼¥ë¤òÆþ¤ì¤ë
#define Return1 RotateMotor(OUT_C,-SPEED3,25);
#define Into2 RotateMotor(OUT_C,SPEED4,30);
#define Return2 RotateMotor(OUT_C,-SPEED4,15);
task main(){
OnFwd(OUT_C,15);
Wait(1000);
Wait(3000); ¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥ì¡¼¥ë¤ò·¹¤±¡¢4¤Ä¤ÎÆâ2¤Ä¤À¤±¥Ü¡¼¥ë¤ò»æ¥³¥Ã¥×¤ËÆþ¤ì¤ë
OnRev(OUT_C,30);
Wait(500);
Wait(5000);
OnFwd(OUT_C,25);
Wait(1000);
Wait(3000); ¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥ì¡¼¥ë¤ò·¹¤±¡¢¥ì¡¼¥ë¤Ë»Ä¤Ã¤Æ¤¤¤ë¥Ü¡¼¥ë¤ò»æ¥³¥Ã¥×¤ËÆþ¤ì¤ë
OnRev(OUT_C,30);
Wait(2500);
}
**¥Þ¥¹¥¿¡¼Â¦ [#wa6a612a]
¾åµ¤Î¥×¥í¥°¥é¥à¤ÎÀâÌÀ¤ÇÍøÍѤ·¤¿ÄêµÁ¤Ë¥×¥é¥¹¤¹¤ë¡£
#define turn_right_here OnFwd(OUT_B,SPEED_SLOW);OnFwd(OUT_C,-SPEED_SLOW); //±¦Àû²ó
***ËÜÈÖÍѤËÍÑ°Õ¤·¤¿¥×¥í¥°¥é¥à [#ne735f23]
task main() {
SetSensorLowspeed(S1);
until (BluetoothStatus(CONN) == NO_ERR);
OnFwd(OUT_BC,50);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//100cmÁ°¤ËÁ°¿Ê¤¹¤ë
Wait(5500);
int d1 = searchDirection(120);¡¡¡¡¡¡¡¡¡¡//»æ¥³¥Ã¥×¤ÎÊý¸þ¤òõ¤¹
if(d1 > 15){
fwdDist(d1-15.0);}
else {fwdDist(d1-15);}¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ºÇÄãÃͤòȯ¸«¤·¤¿¤é²»¤òÌĤ餷¡¢Á°¸å¤ÎÆ°¤¤Ç15ѼêÁ°¤ÇÄä»ß
PlaySound(SOUND_UP);
turn_right_here;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¢¡¼¥à¤¬ÄϤá¤ë°ÌÃ֤ޤǥí¥Ü¥Ã¥ÈÁ´ÂΤòÀû²ó¤µ¤»¤ë
Wait(2500);
RemoteStartProgram(CONN, "arm time.rxe");¡¡//¥¹¥ì¡¼¥Ö¤Î¡Öarm time¡×¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤µ¤»¤ë
Off(OUT_BC);
Wait(10000);
Wait(10000);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¹¥ì¡¼¥Ö¦¤Î¥×¥í¥°¥é¥à¼Â¹ÔÃæ¤Î¤¿¤áÂÔµ¡
Wait(10000);
int angle1 = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle1);¡¡¡¡¡¡¡¡//20cm¸å¿Ê¤¹¤ë
OnFwd(OUT_B,-SPEED);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡
OnFwd(OUT_C,SPEED);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//180ÅÙ²óž
Wait(1500);
angle1 += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle1);¡¡¡¡¡¡¡¡//60cmÁ°¿Ê¤¹¤ë
int d2 = searchDirection(120);
if(d2 > 15){
fwdDist(d2-15.0);}
else {fwdDist(d2-15);}
PlaySound(SOUND_UP);
turn_right_here;
Wait(200);
RemoteStartProgram(CONN, "arm time.rxe");
Off(OUT_BC);
Wait(10000);
Wait(10000);
Wait(10000);
int angle2 = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle2);
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(1500);
angle2 += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle2);
int d3 = searchDirection(120);
turn_right_here;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//ÈùÄ´À°
Wait(600);
if(d3 > 15){
fwdDist(d3);}
else {fwdDist(d3);}¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//»æ¥³¥Ã¥×¤ò¸«¤Ä¤±¤¿¤é¡¢Î¥¤ì¤Æ¤¤¤ëµ÷Î¥¤À¤±Á°¿Ê(¥ì¡¼¥ë¤Î²¼¤Ë»æ¥³¥Ã¥×¤ò»ý¤Ã¤Æ¤¤¤¯¤¿¤á¤ÎÆ°ºî)
PlaySound(SOUND_UP);
RemoteStartProgram(CONN, "rail.rxe");¡¡//¥¹¥ì¡¼¥Ö¤Î¡Örail¡×¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤µ¤»¤ë
Off(OUT_BC);
Wait(10000);
Wait(10000);
Wait(10000);
int angle3 = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle3);
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(1500);
angle3 += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle3);
int d4 = searchDirection(120);
turn_right_here;
Wait(600);
if(d4 > 15){
fwdDist(d4);}
else {fwdDist(d4);}
PlaySound(SOUND_UP);
RemoteStartProgram(CONN, "rail.rxe");
}
***ºÇ½ªÅª¤Ë»ÈÍѤ·¤¿¥×¥í¥°¥é¥à [#sb4129f9]
ËÜÈÖ¤ÇsearchDirection¤¬¾å¼ê¤¯ºîÆ°¤·¤Ê¤«¤Ã¤¿¤¿¤á¡¢ºÇ½ª¼êÃʤȤ·¤ÆĶ²»ÇÈ¥»¥ó¥µ¤òÍøÍѤ»¤º¤Ë¥Ü¡¼¥ë¤ÎÆþ¤Ã¤¿»æ¥³¥Ã¥×¤Î¤È¤³¤í¤Þ¤Ç¹Ô¤«¤»¡¢Ê̤λ楳¥Ã¥×¤Ë¥Ü¡¼¥ë¤òÆþ¤ì¤ë¤È¤³¤í¤Þ¤Ç¡¢¤Û¤ÜOnFwd¤ò»È¤Ã¤Æ°ÜÆ°¤µ¤»¤¿¡£
task main() {
SetSensorLowspeed(S1);
until (BluetoothStatus(CONN) == NO_ERR);
OnFwd(OUT_BC,50);
Wait(5500);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//100cm¤Û¤ÉÁ°¿Ê¤¹¤ë
turn_right_here;¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¢¡¼¥à¤¬ÄϤá¤ë°ÌÃÖ¤Þ¤ÇÀû²ó¤¹¤ë
Wait(2000);
RemoteStartProgram(CONN, "arm time.rxe");¡¡//¥¹¥ì¡¼¥Ö¤Î¡Örail¡×¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤µ¤»¤ë
Off(OUT_BC);
Wait(10000);
Wait(10000);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//¥¹¥ì¡¼¥Ö¦¤Î¥×¥í¥°¥é¥à¼Â¹ÔÃæ¤Î¤¿¤áÂÔµ¡
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(700);¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡//Ê̤λ楳¥Ã¥×¤ò¥ì¡¼¥ë²¼¤Ë»ý¤Ã¤Æ¤¤¤¯¤¿¤á¤Î¥×¥í¥°¥é¥à
OnFwd(OUT_BC,50);
Wait(4000);
RemoteStartProgram(CONN, "rail.rxe");¡¡¡¡¡¡¡¡//¥¹¥ì¡¼¥Ö¤Î¡Örail¡×¤Î¥×¥í¥°¥é¥à¤ò¼Â¹Ô¤µ¤»¤ë
Off(OUT_BC);
}
¤³¤Î¥×¥í¥°¥é¥à¤ÇËÜÈÖ¤Ï1¤Ä¤Î¥Ü¡¼¥ë¤Î°ÜÆ°¤ËÀ®¸ù¤¹¤ë¤³¤È¤¬½ÐÍ褿¡£
*´¶ÁÛ [#f09587f0]
¥í¥Ü¥Ã¥È¡¦¥×¥í¥°¥é¥à¤ÎÀ½ºî¤¬º£¤Þ¤Ç¤ÈÈæ¤Ùʪ¤Ë¤Ê¤é¤Ê¤¤¤Û¤ÉÂçÊѤǡ¢¤·¤«¤â»þ´Ö¤ò¤«¤±¤¿Ê¬¡¢Á´Éô¤¬Á´Éô·ë²Ì¤Ë¤Ê¤ë¤È¤Ï¸Â¤é¤Ê¤¤¤³¤È¤ò»×¤¤ÃΤ餵¤ì¤¿µ¤¤¬¤·¤¿¡£¥×¥í¥°¥é¥à¤ÎÎ̤¬Â¿¤¯¤Ê¤ì¤Ð¤Ê¤ë¤Û¤É¥ß¥¹¤¬µ¯¤³¤ê¤ä¤¹¤¤¡¢¤Þ¤¿Ê£»¨¤Ê¥×¥í¥°¥é¥à¤Ê¤Û¤É1¤Ä1¤Ä¤Á¤ã¤ó¤ÈÍý²ò¤·¤Æ³Îǧ¤·¤Ê¤¤¤È¤¤¤±¤Ê¤¤¤³¤È¤Ëµ¤¤Å¤«¤µ¤ì¤¿¡£¤³¤ì¤«¤é¤â¥í¥Ü¥Ã¥È¤òÀ½ºî¤·¤Æ¥×¥í¥°¥é¥à¤òºî¤Ã¤Æ¤¤¤¯µ¡²ñ¤¬¤¢¤ë¤Î¤Ç¡¢¤³¤Î¥¼¥ß¤Ç³Ø¤ó¤À¤³¤È¤ò³è¤«¤·¤Æ¤¤¤¤¿¤¤¤È»×¤¦¡£
¥Ú¡¼¥¸Ì¾: