- Äɲ䵤줿¹Ô¤Ï¤³¤Î¿§¤Ç¤¹¡£
- ºï½ü¤µ¤ì¤¿¹Ô¤Ï¤³¤Î¿§¤Ç¤¹¡£
[[2015b/Member]]
*Á´¥×¥í¥°¥é¥à [#f342fd28]
#define SPEED 50 //Dist¤Ê¤É¤Î¥â¡¼¥¿¤Î¥Ñ¥ï¡¼¤Ë¡¢Éý¹¤¯»ÈÍÑ
#define SPEED_SLOW 30 //´Ø¿ôhand,turnAng¤Ê¤É¤Ç»ÈÍÑ
#define THRESHOLD 45 //ïçÃÍ
#define STEP 1 //Wait¤Î°ú¿ô¤È¤·¤Æ»È¤¦
#define STOP 3 //¡Ö¥Ü¡¼¥ë¤¬¥»¥ó¥µ¤ÎÁ°¤ò²£Àڤ信פ«¤É¤¦¤«¤òÄ´¤Ù¤ëÃÍ
const float diameter=5.45; //¥¿¥¤¥ä¤Îľ·Â(cm)
const float pi=3.1415; //±ß¼þΨ
const float track = 10.35; //¥¿¥¤¥ä¤Î¥È¥ì¥Ã¥ÉÉý
sub hand(int ang, int plmi) { //¥Ü¡¼¥ë¤ò¤Ä¤«¤àÆ°ºî
/*plmi = 1 ¢ª ¥¢¡¼¥à³«
plmi = -1 ¢ª ¥¢¡¼¥àÊÄ */
RotateMotor(OUT_A, SPEED_SLOW * plmi, ang);
}
sub Dist(float d, int fwd_or_rev){ //dcmÁ°Å¾(¸åž)
/*fwd_or_rev = 1 ¢ª Á°Å¾
fwd_or_rev = -1 ¢ª ¸åž */
long angle;
angle = d / (diameter * pi) * 360.0;
RotateMotorEx(OUT_BC, SPEED * fwd_or_rev, angle, 0, true, true);
Wait(100);
}
sub turnAng(int ang, int R_or_L) //angÅÙÀû²ó
{
/* R_or_L = 1 ¢ª »þ·×²ó¤ê
R_or_L = -1 ¢ª Ⱦ»þ·×²ó¤ê */
long angle;
angle = track / diameter * ang;
RotateMotorEx(OUT_BC, SPEED_SLOW , angle, 100* R_or_L, true, true);
Wait(100);
}
int searchDirection(int ang) //¥Ü¡¼¥ë¤òõ¤¹´Ø¿ô
{
SetSensorLowspeed(S1);
long angle, tacho_min = 0, tacho_corr;
int d_min;
d_min= 300; //²¾¤ÎºÇ¾®ÃÍ
angle = (track / diameter) * ang;
turnAng(10, 1); //10ÅÙ±¦Àû²ó(´Ì¤Î¸íǧ¤òËɤ°¤¿¤á¡¢10ÅÙ¤·¤«Àû²ó¤·¤Ê¤¤)
ResetTachoCount(OUT_B);
OnFwdSync(OUT_BC, SPEED, -100);
while(MotorTachoCount(OUT_B) <= angle) { //angle¤À¤±º¸Àû²ó
if (SensorUS(S1) < d_min) {
d_min = SensorUS(S1);
tacho_min = MotorTachoCount(OUT_B);
}
}
if (d_min < 30) { //¡ÖĶ²»ÇÈ¥»¥ó¥µ¤Î¼¨¤·¤¿ºÇ¾®Ã͡פ¬30°Ê²¼¤Î¤È¤
OnFwdSyncEx(OUT_BC,SPEED, 100, RESET_NONE); //ºÇ¾®ÃͤÀ¤Ã¤¿Êý¸þ¤ò¸þ¤¯
until(MotorTachoCount(OUT_B) <= tacho_min || SensorUS(S1) <= d_min);
}
else { //30°Ê¾å¤Î¤È¤
turnAng(ang - 10, 1); //ang-10¤À¤±±¦Àû²ó(´Ø¿ô³«»ÏÁ°¤ÈƱ¤¸¸þ¤¤ËÌá¤ë)
}
Off(OUT_BC); Wait(500); //0.5ÉôÖÄä»ß
return d_min; //¡ÖĶ²»ÇÈ¥»¥ó¥µ¤Î¼¨¤·¤¿ºÇ¾®Ã͡פòÊÖ¤¹
}
sub turn_left(int speedA, int speedB) { //º¸¤Ë¶Ê¤¬¤ë
OnFwd(OUT_B,speedA); //°ú¿ôspeedA¤Î¥Ñ¥ï¡¼¤Ç¡¢¥â¡¼¥¿B¤òÁ°Å¾
OnRev(OUT_C,speedB); //°ú¿ôspeedB¤Î¥Ñ¥ï¡¼¤Ç¡¢¥â¡¼¥¿C¤ò¸åž
}
sub turn_right(int speedA, int speedB) { //±¦¤Ë¶Ê¤¬¤ë
OnFwd(OUT_C,speedB); //°ú¿ôspeedB¤Î¥Ñ¥ï¡¼¤Ç¡¢¥â¡¼¥¿C¤òÁ°Å¾
OnRev(OUT_B,speedA); //°ú¿ôspeedA¤Î¥Ñ¥ï¡¼¤Ç¡¢¥â¡¼¥¿B¤ò¸åž
}
sub go_forward(int speed3) { //¤Þ¤Ã¤¹¤°¿Ê¤à
OnFwd(OUT_BC,speed3); //speed3¤Î¥Ñ¥ï¡¼¤Ç¡¢¥â¡¼¥¿BC¤òÁ°Å¾
}
sub trace(int time, long target, int speed3) { //(timeÉðÊÆâ && TachoCount < target)¤Î´Ö¥é¥¤¥ó¥È¥ì¡¼¥¹
SetSensorLight(S2);
int speed1 = 25; //¥â¡¼¥¿¤Î¥Ñ¥ï¡¼ÍÑ
int speed2 = 20; //¥â¡¼¥¿¤Î¥Ñ¥ï¡¼ÍÑ(speed1¤è¤ê¾¯¤·¼å¤¤)
long tacho = 0; //¡Ö¤½¤Î¾ì¤Çº¸Àû²ó¤¹¤ë¡×Æ°ºî¤ò¤·Â³¤±¤¿¤È¤¤Î¡¢²óž³Ñ¤Î¹ç·×
long t; //¥¿¥¤¥Þ¡¼¤ò»È¤¦¤È¤¤Ë»ÈÍÑ
t = CurrentTick(); //¤³¤³¤Ç¸½ºß»þ¹ï¤òt¤ËÂåÆþ(CurrentTick() - t¡¡¤È»È¤¦¤³¤È¤Ç¡¢·Ð²á»þ´Ö¤¬¤ï¤«¤ë)
while (CurrentTick() - t < time && tacho < target) {
if (SENSOR_2 < THRESHOLD -15){ //¸÷¥»¥ó¥µ¤Î¿ôÃͤ¬¡¢THRESHOLD-15¤è¤êÄ㤤¤È¤
turn_left(speed1, speed1); //º¸±¦¤Î¥â¡¼¥¿¤òspeed1¤Çº¸Àû²ó(¤½¤Î¾ì¤Çº¸²óž)
} else if (SENSOR_2 < THRESHOLD -7) { //¸÷¥»¥ó¥µ¤Î¿ôÃͤ¬¡¢THRESHOLD-7¤è¤êÄ㤤¤È¤
tacho = 0; //tacho(turn_left1¤·Â³¤±¤¿»þ¤Î¡¢¥¿¥¤¥ä¤Î²óž³Ñ¤Î¹ç·×)¤ò0¤Ë¤¹¤ë¡£
turn_left(speed1, speed2); //±¦¥â¡¼¥¿¤òspeed1¡¢º¸¥â¡¼¥¿¤òspeed2(speed1¤è¤ê¾¯¤·¼å¤¤)¤Ë¤·¤Æ¡¢º¸Àû²ó
} else if (SENSOR_2 < THRESHOLD +7){ //¸÷¥»¥ó¥µ¤Î¿ôÃͤ¬¡¢THRESHOLD+7¤è¤êÄ㤤¤È¤
tacho = 0; //tacho¤ò0¤Ë¤¹¤ë
go_forward(speed3); //º¸±¦¤Î¥â¡¼¥¿¤òspeed3¤Ë¤·¤Æ¡¢Ä¾¿Ê
} else if (SENSOR_2 < THRESHOLD +15){ //¸÷¥»¥ó¥µ¤Î¿ôÃͤ¬¡¢THRESHOLD+15¤è¤êÄ㤤¤È¤
tacho = 0; /*tacho¤ò0¤Ë¤¹¤ë*/
turn_right(speed1, speed2); /*±¦¥â¡¼¥¿¤òspeed2¡¢º¸¥â¡¼¥¿¤òspeed1¤Ë¤·¤Æ¡¢±¦Àû²ó*/
} else { /*¸÷¥»¥ó¥µ¤Î¿ôÃͤ¬¡¢THRESHOLD+15¤è¤ê¹â¤¤¤È¤*/
tacho = 0; /*tacho¤ò0¤Ë¤¹¤ë*/
turn_right(speed1, speed1); /*º¸±¦¤Î¥â¡¼¥¿¤òspeed1¤Ç±¦Àû²ó(¤½¤Î¾ì¤Ç±¦²óž)*/
}
Wait(STEP); //STEP(0.001)ÉäÀ¤±Æ°¤¯
if (SENSOR_2 >= THRESHOLD-15) { /*¸÷¥»¥ó¥µ¤Î¿ôÃͤ¬¡¢THRESHOLD-15¤è¤ê¹â¤¤¤È¤ (=¤½¤Î¾ì¤Çº¸²óž¤·¤Æ¤¤¤Ê¤¤¤È¤) */
ResetTachoCount(OUT_B); /*¥â¡¼¥¿¤Î²óž¿ô¤ò¥ê¥»¥Ã¥È(¤½¤Î¾ì¤Çº¸²óž¤·¤¿¤È¤°Ê³°¤Ï¥ê¥»¥Ã¥È)*/
}
tacho += MotorTachoCount(OUT_B); /*tacho¤Ë¡¢¥â¡¼¥¿¤Î²óž³Ñ¤òÂåÆþ(¡Ö¤½¤Î¾ì¤Çº¸²óž¤·¤¿»þ¡×¤Î²óž³Ñ¤·¤«ÂåÆþ¤µ¤ì¤Ê¤¤)*/
}
Off(OUT_BC); Wait(STEP);
PlaySound(SOUND_CLICK);
}
task main() {
long t;
Wait(30000); //30ÉÃÂÔµ¡
Dist(48, 1); //48cmÁ°¿Ê
turnAng(91, -1); //90ÅÙº¸Àû²ó
SetSensorLowspeed(S1); //Ķ²»ÇÈ¥»¥ó¥µµ¯Æ°
int ball = 0; //ÊÑ¿ô¡Öball¡×...Ķ²»ÇÈ¥»¥ó¥µ¤ÎÃÍ < 30 ¤ò¼¨¤·¤¿²ó¿ô¤ò¼¨¤¹
while (1) { //¥Ü¡¼¥ë¤¬Ä̲᤹¤ëÍͻҤòÄ´¤Ù¤ë
if (SensorUS(S1) < 30) { /*¥»¥ó¥µÃÍ < 30(¥Ü¡¼¥ë¤¬¥»¥ó¥µ¤ÎÁ°¤òÄ̤ê²á¤®¤¿¤È¤¡¢¥»¥ó¥µÃͤ¬20ÄøÅÙ¤ò¼¨¤¹¤¿¤á)*/
ball++;
}
if (ball == STOP) { //Ä̲ᤷ¤¿²ó¿ô¤¬STOP(=3)¤Ë¤Ê¤Ã¤¿¤é
break; //¥ë¡¼¥×¤òÈ´¤±½Ð¤¹
}
Wait(STEP); //STEP(0.001)ÉôÖ
}
Wait(800); //0.8ÉôÖÂÔµ¡
PlaySound(SOUND_CLICK); //¼¡¤ÎÆ°ºî¤ËÆþ¤ë¤³¤È¤òÃΤ餻¤ë
t = CurrentTick();
while (CurrentTick() - t < 2000) { //2Éäδַ«¤êÊÖ¤¹
if (SensorUS(S1) < 30) { //¥»¥ó¥µÃÍ < 30¤Î¤È¤
ball++; //ball + 1 ..¤Ä¤Þ¤êball = STOP + 1
break; //¥ë¡¼¥×¤òÈ´¤±½Ð¤¹
}
Wait(STEP);
}
if (ball == STOP + 1) { //Ķ²»ÇÈ¥»¥ó¥µ < 30¤ò¼¨¤¹¤Þ¤ÇÁ°¿Ê
OnFwd(OUT_BC, SPEED_SLOW); //SPEED_SLOW(=30)¤Î¥Ñ¥ï¡¼¤ÇÁ°¿Ê
while (ball < STOP + 2) {
if (SensorUS(S1) < 30) {
ball++;
}
}
Off(OUT_BC); //1ÉÃÄä»ß
Wait(1);
Dist(35, 1); //35cmÁ°¿Ê
hand(70, -1); //¥Ü¡¼¥ë¤ò¤Ä¤«¤à
turnAng(30, -1); //30ÅÙ±¦Àû²ó(¥é¥¤¥ó¥È¥ì¡¼¥¹¤Î¹õÀþ¤ËÆþ¤ë¤¿¤á)
Dist(8, 1); //8cmÁ°¿Ê(¥é¥¤¥ó¥È¥ì¡¼¥¹¤Î¹õÀþ¤ËÆþ¤ë¤¿¤á)
trace(20000, 15, 40); /*¡Ö20ÉðÊÆâ && TachoCount < 15¡×¤Î´Ö¥é¥¤¥ó¥È¥ì¡¼¥¹*/
trace(7000, 1000, 50); /*¡Ö7ÉðÊÆâ && TachoCount < 1000¡×¤Î´Ö¥é¥¤¥ó¥È¥ì¡¼¥¹*/
trace(15000, 10, 40); /*¡Ö15ÉðÊÆâ && TachoCount < 10¡×¤Î´Ö¥é¥¤¥ó¥È¥ì¡¼¥¹*/
turnAng(75, 1); //75ÅÙ±¦Àû²ó
Dist(15,1); //15cmÁ°¿Ê
turnAng(20, 1); //20ÅÙ±¦Àû²ó
SetSensorLight(S2); //¸÷¥»¥ó¥µ¡¼µ¯Æ°
OnFwd(OUT_BC, SPEED); //¹õÀþ¤Þ¤ÇSPEED(=50)¤Î¥Ñ¥ï¡¼¤ÇÁ°¿Ê
while (SENSOR_2 > THRESHOLD - 15) {}
Off(OUT_BC); //Ää»ß
hand(70, 1); //¥Ü¡¼¥ë¤òÎ¥¤¹
}
else {
turnAng(90, -1); //90ÅÙº¸Àû²ó
Dist(40, 1); //40cm¿Ê¤à
turnAng(90, 1); //90ÅÙ±¦Àû²ó
Off(OUT_BC); Wait(STEP); //0.001ÉôÖÄä»ß
while (1) {
int d = searchDirection(60); //60ÅÙ¤ÎÈϰϤǥܡ¼¥ë¤òõ¤¹
if (d < 30) { /*searchDirection(60)¤ÎÊÖ¤êÃÍ < 30¤Î¤È¤(õ¤·¤¿ÈϰϤ˥ܡ¼¥ë¤¬¤¢¤Ã¤¿¤È¤)*/
Dist(20, 1); //20cm¿Ê¤à
hand(70, -1); //¥Ü¡¼¥ë¤ò¤Ä¤«¤à
break; //¥ë¡¼¥×¤«¤éÈ´¤±½Ð¤¹
}
else {
OnFwd(OUT_BC, SPEED); //0.5Éÿʤà
Wait(500);
searchDirection(0); /*searchDirection¤ò2Ϣ³¤Ç»È¤¦¤È¥Ð¥°¤ë¤¿¤áseachDirection(0)¤ò¤Ï¤µ¤à*/
}
}
trace(10000, 10000, 45); //¤É¤³¤«¤Î¥é¥¤¥ó¤Ë¤Î¤ë,¥í¥Ü¥Ã¥È¤Î¸þ¤¤ò¥´¡¼¥ëÊý¸þ¤Ë¸þ¤±¤ë
Dist(40, 1); //40cm¿Ê¤à¡¢
SetSensorLight(S2); //¸÷¥»¥ó¥µ¡¼µ¯Æ°
OnFwd(OUT_BC, SPEED); //SPEED(=50)¤Î¥Ñ¥ï¡¼¤Ç¡¢¹õÀþ¤ËÅþ㤹¤ë¤Þ¤Çľ¿Ê
while (SENSOR_2 > THRESHOLD - 15) {}
trace(3000, 10000, 40); //¡Ö3ÉðÊÆâ && TachoCount < 10000¡×¤Î´Ö¥é¥¤¥ó¥È¥ì¡¼¥¹
hand(70, 1); //¥Ü¡¼¥ë¤òÎ¥¤¹
}
}
Ìܼ¡
#contents
#aname
*¥í¥Ü¥³¥ó¤ÎÀâÌÀ [#p90c333d]
#ref(2015b/Member/rmsun/Mission3/2015b-mission31.png, 480*332)
¥í¥Ü¥Ã¥ÈA¤ÏSTART AÃÏÅÀ¤«¤é½Ðȯ¤·¡¢PÃÏÅÀ¤Ë¤ª¤«¤ì¤¿ÀÖ¤¤¥Ü¡¼¥ë¤ò½¦¤Ã¤Æ350ml¤Î¥¢¥ë¥ß´Ì¤ËÅö¤Æ¤ë¡£¤½¤Î¸å¡¢¥í¥Ü¥Ã¥ÈB¤¬¥Ü¡¼¥ë¤ò²ó¼ý¤¹¤ë¡£°ú¤Â³¤ÀĤ¤¥Ü¡¼¥ë¤Ë¤Ä¤¤¤Æ¤âƱÍͤÎÆ°ºî¤ò·«¤êÊÖ¤¹¡£¥í¥Ü¥Ã¥ÈB¤Ï¥Ü¡¼¥ë¤ò²ó¼ý¤·¤¿¸å¡¢ºÇ½ªÅª¤ËGOAL B¤Þ¤Ç¥Ü¡¼¥ë¤ò±¿¤Ö¡£
¾Ü¤·¤¤ÀâÌÀ¤Ïhttp://yakushi.shinshu-u.ac.jp/robotics/?2015b%2FMission3¤ò»²¾È
*¥í¥Ü¥Ã¥È¤ÎÀâÌÀ [#p06dabf5]
#ref(2015b/Member/rmsun/Mission3/IMG_1411.JPG, 480*360);
¼Ì¿¿º¸:¥Ü¡¼¥ë¤ò´Ì¤ËÅö¤Æ¤ë¥í¥Ü¥Ã¥È
¼Ì¿¿±¦:¥Ü¡¼¥ë¤ò²ó¼ý¤¹¤ë¥í¥Ü¥Ã¥È
**¡Ö¥Ü¡¼¥ë¤ò´Ì¤ËÅö¤Æ¤ë¥í¥Ü¥Ã¥È¡× [#k9cf5b32]
***¥í¥Ü¥Ã¥È¤Î¹½Â¤ [#i789b231]
&ref(2015b/Member/rmsun/Mission3/IMG_1410a.JPG,360*480);
&ref(2015b/Member/rmsun/Mission3/IMG_1409a.JPG,480*360);
-¥â¡¼¥¿A¤Ï¥¢¡¼¥à¤ÎÉôʬ¡¢¥â¡¼¥¿B¤Ï±¦¥¿¥¤¥ä¡¢¥â¡¼¥¿C¤Ïº¸¥¿¥¤¥ä¤ÎÌò³ä
-»È¤Ã¤¿¥»¥ó¥µ¤Ï¡¢¸÷¥»¥ó¥µ¤ÈĶ²»ÇÈ¥»¥ó¥µ¡£
-Ķ²»ÇÈ¥»¥ó¥µ¤Ï¾å²¼¤ËÆ°¤¯¡£
-¥¢¡¼¥à¤ÎÆ°¤¤ÈĶ²»ÇÈ¥»¥ó¥µ¤¬Ï¢Æ°¤·¤Æ¤¤¤ë(Îã : ¥¢¡¼¥à³«¤¯,Ķ²»ÇÈ¥»¥ó¥µ²¼¸þ¤¢Î¥¢¡¼¥àÊĤ¸¤ë,Ķ²»ÇÈ¥»¥ó¥µÀµÌ̤ò¸þ¤¯)
-¥¢¡¼¥à¤ÎÆ°¤¤ÈĶ²»ÇÈ¥»¥ó¥µ¤Î¸þ¤¤¬Ï¢Æ°¤·¤Æ¤¤¤ë(Îã : ¥¢¡¼¥à³«¤¯,Ķ²»ÇÈ¥»¥ó¥µ²¼¸þ¤¢Î¥¢¡¼¥àÊĤ¸¤ë,Ķ²»ÇÈ¥»¥ó¥µÀµÌ̤ò¸þ¤¯)
***¥í¥Ü¥Ã¥È¤ÎÆ°¤ [#q2a47db6]
¡Ä¶²»ÇÈ¥»¥ó¥µ¤ò²¼¸þ¤¤Ë¤·¤Æ¡¢¥Ü¡¼¥ë¤òõ¤¹(¥é¥¤¥ó¥È¥ì¡¼¥¹¤·¤Ê¤¬¤é)
¢¢
¢¥Ü¡¼¥ë¤ò¸«¤Ä¤±¤ë¤È¥¢¡¼¥à¤òÊĤ¸¡¢Ä¶²»ÇÈ¥»¥ó¥µ¤ÏÀµÌ̸þ¤¤Ë
¢¢
£Ä¶²»ÇÈ¥»¥ó¥µ¤Ç´Ì¤òõ¤¹
¢¢
¤´Ì¤ÎÊý¤ò¸þ¤¯
¢¢
¥¥Ü¡¼¥ë¤òÊü¤Ä
-¥Ü¡¼¥ë¤ÎÊü¤ÁÊý
#ref(2015b/Member/rmsun/Mission3/¥í¥Ü¥³¥ó6.gif,480*332)
**¡Ö¥Ü¡¼¥ë¤ò²ó¼ý¤¹¤ë¥í¥Ü¥Ã¥È¡× [#l707841a]
***¥í¥Ü¥Ã¥È¤Î¹½Â¤ [#y585dbcb]
#ref(2015b/Member/rmsun/Mission3/IMG_1406a.JPG,360*480)
&ref(2015b/Member/rmsun/Mission3/IMG_1407a.JPG,240*180);
&ref(2015b/Member/rmsun/Mission3/IMG_1408a.JPG,180*240);
¥â¡¼¥¿A¤Ï¥¢¡¼¥àÉôʬ¡¢¥â¡¼¥¿B¤Ï±¦¥¿¥¤¥ä¡¢¥â¡¼¥¿C¤Ïº¸¥¿¥¤¥ä¡£¥â¡¼¥¿A¤À¤±¤Ç¡¢2¤Ä¤Î¥¢¡¼¥à¤òÆ°¤«¤¹¡£
-¥â¡¼¥¿A¤Ï¥¢¡¼¥àÉôʬ¡¢¥â¡¼¥¿B¤Ï±¦¥¿¥¤¥ä¡¢¥â¡¼¥¿C¤Ïº¸¥¿¥¤¥ä¡£¥â¡¼¥¿A¤À¤±¤Ç¡¢2¤Ä¤Î¥¢¡¼¥à¤òÆ°¤«¤¹¡£
»È¤Ã¤¿¥»¥ó¥µ¤Ï¡¢¸÷¥»¥ó¥µ¤ÈĶ²»ÇÈ¥»¥ó¥µ¡£Ä¶²»ÇÈ¥»¥ó¥µ¤Ï¡¢(¥»¥ó¥µ¤Î°ÌÃ֤δط¸¤â¤¢¤ê)ÂоݤȤε÷Î¥¤òÀµ³Î¤Ë¬¤ì¤Ê¤«¤Ã¤¿¡£¤Ê¤Î¤Ç¡¢¡Ö¤½¤³¤Ë¥Ü¡¼¥ë¤¬¤¢¤ë¤Î¤«¤É¤¦¤«¡×¤ÎȽÃǤ˻Ȥä¿¡£¥»¥ó¥µ¤¬¥Ü¡¼¥ë¤ò´¶ÃΤ·¤¿»þ¡¢ÃͤÏ20Á°¸å¤ò¼¨¤·¤¿¤Î¤Ç¡¢¡ÖĶ²»ÇÈ¥»¥ó¥µ¤ÎÃÍ < 30¡×¤È¤Ê¤Ã¤¿»þ¤Ë¡Ö¥Ü¡¼¥ë¤¬Â¸ºß¤¹¤ë¡×¤È¤ß¤Ê¤·¤¿¡£
-»È¤Ã¤¿¥»¥ó¥µ¤Ï¡¢¸÷¥»¥ó¥µ¤ÈĶ²»ÇÈ¥»¥ó¥µ¡£
-Ķ²»ÇÈ¥»¥ó¥µ¤Ï¡¢(¥»¥ó¥µ¤Î°ÌÃ֤δط¸¤â¤¢¤ê)ÂоݤȤε÷Î¥¤òÀµ³Î¤Ë¬¤ì¤Ê¤«¤Ã¤¿¡£¤Ê¤Î¤Ç¡¢¡Ö¤½¤³¤Ë¥Ü¡¼¥ë¤¬¤¢¤ë¤Î¤«¤É¤¦¤«¡×¤ÎȽÃǤ˻Ȥä¿¡£¥»¥ó¥µ¤¬¥Ü¡¼¥ë¤ò´¶ÃΤ·¤¿»þ¡¢ÃͤÏ20Á°¸å¤ò¼¨¤·¤¿¤Î¤Ç¡¢¡ÖĶ²»ÇÈ¥»¥ó¥µ¤ÎÃÍ < 30¡×¡á¡Ö¥Ü¡¼¥ë¤¬Â¸ºß¤¹¤ë¡×¤È¤ß¤Ê¤·¤¿¡£
***¥í¥Ü¥Ã¥È¤ÎÆ°¤ [#mb50a946]
´Ì¢«¢«¥Ü¡¼¥ë
¡¡¡¡ ¢¬
¡¡¡¡ ¢¬Ä¶²»ÇÈ¥»¥ó¥µ
¡¡¡¡ ¥í¥Ü¥Ã¥È
¾å¿Þ¤Î¤è¤¦¤ËĶ²»ÇÈ¥»¥ó¥µ¤ò¸þ¤±¤ë¡£
¥Ü¡¼¥ë¤¬Ä̤ê²á¤®¤¿»þ : Ķ²»ÇÈ¥»¥ó¥µ¤Ï¥Ü¡¼¥ë¤ò´¶Ã΢ª0.8ÉÃÄä»ß¡£¤½¤·¤ÆĶ²»ÇÈ¥»¥ó¥µºÆµ¯Æ°¡£¤½¤Î¸å¤Î¥Ü¡¼¥ë¤ÎÆ°¤¯¸þ¤¤Ë¤è¤Ã¤Æ¡¢¥í¥Ü¥Ã¥È¤ÎÆ°ºî¤¬ÊѤï¤ë¡£
1. ¥Ü¡¼¥ë¤¬Ä·¤ÍÊ֤俤Ȥ
´Ì¢ª¢ª¥Ü¡¼¥ë
¢¬
¢¬Ä¶²»ÇÈ¥»¥ó¥µ
¥í¥Ü¥Ã¥È
Ķ²»ÇÈ¥»¥ó¥µ¤ÇºÆ¤Ó´¶Ã΢ª¥í¥Ü¥Ã¥Èľ¿Ê¢ª¥Ü¡¼¥ë¤ò¤Ä¤«¤à¢ª¥é¥¤¥ó¥È¥ì¡¼¥¹¤Ê¤É¤ò»È¤¤¥´¡¼¥ë¤Ø¹Ô¤¡¢¥Ü¡¼¥ë¤òÎ¥¤¹
2. ¥Ü¡¼¥ë¤¬º¸¤Ëή¤ì¤Æ¤·¤Þ¤Ã¤¿¤È¤
2.1 Ä·¤ÍÊ֤俥ܡ¼¥ë¤ò´¶ÃΤ·¤Ê¤¤
¥Ü¡¼¥ë¢«¢«¡¡¡¡
´Ì
¢¬
¢¬Ä¶²»ÇÈ¥»¥ó¥µ
¥í¥Ü¥Ã¥È
Ķ²»ÇÈ¥»¥ó¥µ¤Ï¥Ü¡¼¥ë¤ò´¶ÃΤ·¤Ê¤¤¢ª¥í¥Ü¥Ã¥Èº¸¤Ø¸þ¤«¤¦
2.2 ´Ì¤Îº¸Â¦¤Ë¸þ¤«¤¤¡¢¥Ü¡¼¥ë¤òõº÷
¥Ü¡¼¥ë
¢¬¢¬¢¬¢¬¢¬¢¬ ´Ì
¢¬¢¬¢¬¢¬
¢¬¢¬ Ìð°õ¤Ï¡¢Ä¶²»ÇÈ¥»¥ó¥µ¤¬ºîÍѤ¹¤ëÈÏ°Ï
¥í¥Ü¥Ã¥È
¡ù´Ì¤ò´¶ÃΤ·¤Ê¤¤ÄøÅ٤ˡ¢¥í¥Ü¥Ã¥È(Ķ²»ÇÈ¥»¥ó¥µ)¤ò²óž¤µ¤»¤ë¡£
-¥Ü¡¼¥ë¤¬¸«¤Ä¤«¤Ã¤¿¾ì¹ç
¥Ü¡¼¥ë¤ÎÊý¤ò¸þ¤¡¢Ä¾¿Ê¡£¤½¤Î¸å¥Ü¡¼¥ë¤ò¤Ä¤«¤à
-¥Ü¡¼¥ë¤¬¸«¤Ä¤«¤é¤Ê¤«¤Ã¤¿¾ì¹ç
¥í¥Ü¥Ã¥È¤ò²¼¿Þ¤Î¸þ¤¤ËÌᤷ¤Æ¡¢¾¯¤·¿Ê¤ó¤Ç¡¢ºÆ¤Ó¡ù¤ÎÆ°ºî
¡¡¡¡¡¡¡¡´Ì
¢¬
¢¬ Ìð°õ¤ÎÊý¤Ø¸þ¤¯
¥í¥Ü¥Ã¥È
*¡Ö¥Ü¡¼¥ë¤ò´Ì¤ËÅö¤Æ¤ë¥í¥Ü¥Ã¥È¡×¤Î¥×¥í¥°¥é¥à [#k14bab2c]
¼«Ê¬¤Î½ñ¤¤¤¿¥×¥í¥°¥é¥à¤Ç¤Ï¤Ê¤¤¤Î¤Ç¡¢¾Ü¤·¤¯ÀâÌÀ¤Ï¤Ç¤¤Þ¤»¤ó¡£
#define straight OnFwd(OUT_BC,30);
#define turn_left OnFwd(OUT_B,30);Off(OUT_C);
#define turn_right OnFwd(OUT_C,30);Off(OUT_B);
#define TURN_LEFT OnFwd(OUT_B,30);OnRev(OUT_C,30);
#define TURN_RIGHT OnFwd(OUT_C,30);OnRev(OUT_B,30);
#define THRESHOLD 35
#define get OnFwd(OUT_A,45);Wait(300);Off(OUT_A);Wait(1000);
#define open OnRev(OUT_A,45);Wait(300);Off(OUT_A);Wait(1000);
const float diameter=5.45;
const float track=10.35;
const float pi=3.1415;
void fwdDist(float d) { //dcm¿Ê¤à
long angle;
angle=d/(diameter*pi)*360.0;
RotateMotorEx(OUT_BC,30,angle,0,true,true);
}
void turnAng(long ang) { //angÅÙ±¦Àû²ó
long angle;
angle=track/diameter*ang;
RotateMotorEx(OUT_BC,30,angle,100,true,true);
}
int searchDirection(long ang) { //ºÇûµ÷Î¥¤ÎʪÂΤÎÊý¤ò¸þ¤¯
long angle,tacho_min=0,tacho_corr;
int d_min;
d_min=55;
angle=(track/diameter)*ang;
turnAng(ang/2);
ResetTachoCount(OUT_BC);
OnFwdSync(OUT_BC,30,-100);
while(MotorTachoCount(OUT_B)<=angle){
if(SensorUS(S4)<d_min){
PlaySound(SOUND_CLICK);
d_min=SensorUS(S4);
tacho_min=MotorTachoCount(OUT_B);
}
}
OnFwdSyncEx(OUT_BC,30,100,RESET_NONE);
until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S4)<=d_min);
Wait(14);
Off(OUT_BC);Wait(500);
return d_min;
}
task main() {
SetSensorLight(S3);
SetSensorLowspeed(S4);
int catch=0;
straight;
Wait(500);
RotateMotor(OUT_A,30,-80);
while(true){
if(SensorUS(S4)>=12){ //Ķ²»ÇÈ¥»¥ó¥µÃÍ >= 12 ¤Ä¤Þ¤ê¡¢¥»¥ó¥µ¤¬¥Ü¡¼¥ë¤ò´¶ÃΤ¹¤ë¤Þ¤Ç
if(SENSOR_3<THRESHOLD-9){
TURN_LEFT;
}else if(SENSOR_3<THRESHOLD-5){
turn_left;
}else if(SENSOR_3<THRESHOLD+5){
straight;
}else if(SENSOR_3<THRESHOLD+9){
turn_right;
}else{
TURN_RIGHT;
}
}else{ //¥»¥ó¥µ¤¬¥Ü¡¼¥ë¤ò´¶ÃΤ·¤¿¤È¤
Off(OUT_BC);
get;
Wait(1000);
catch++;
if(catch==1){
RotateMotorEx(OUT_BC,40,-200,0,true,false);
RotateMotorEx(OUT_BC,40,200,-100,true,false);
Off(OUT_BC);
Wait(500);
RotateMotorEx(OUT_BC,40,700,0,true,false);
Off(OUT_BC);
Wait(500);
int d=searchDirection(130);
PlaySound(SOUND_UP);
if(d<=50){ //Ķ²»ÇÈ¥»¥ó¥µ¤¬´Ì¤ò´¶ÃΤ·¤¿»þ¤·¤¿»þ
Off(OUT_BC); //¢¤³¤³¤«¤é¥Ü¡¼¥ë¤òÊü¤ÄÆ°ºî
Wait(500);
RotateMotorEx(OUT_BC,40,-300,0,true,false); //²¼¤¬¤ë
Wait(6000);
open; //¥¢¡¼¥à¤ò³«¤¯
RotateMotorEx(OUT_BC,90,340,0,true,false); //¥Ü¡¼¥ë¤òÄϤó¤À¤Þ¤Þ®¤¤Â®ÅÙ¤Çľ¿Ê
Off(OUT_BC); //µÞÄä»ß¡¢¤³¤Î¤È¤¥Ü¡¼¥ë¤òÊü¤Ä
Wait(1000);
RotateMotorEx(OUT_BC,65,-730,0,true,true); //²¼¤¬¤ë
RotateMotorEx(OUT_BC,40,150,100,true,false); //150Å٤α¦Àû²ó
RotateMotor(OUT_A,60,50); //¥¢¡¼¥à¤òÊĤ¸¤ë
straight; //¾¯¤·¤Îľ¿Ê
Wait(2500);
}
}
}
}
}
*¡Ö¥Ü¡¼¥ë¤ò²ó¼ý¤¹¤ë¥í¥Ü¥Ã¥È¡×¤Î¥×¥í¥°¥é¥à [#z16e8d55]
¼«Ê¬¤¬Ã´Åö¤·¤¿¤Î¤Ï¡¢¼ç¤Ë¤³¤ÎÉôʬ¤Ç¤¹¡£
**ÄêµÁ¡¢´Ø¿ô [#y0213ca1]
***¥Þ¥¯¥í [#m15c1f4b]
**¥Þ¥¯¥í [#m15c1f4b]
#define SPEED 50 //Dist¤Ê¤É¤Î¥â¡¼¥¿¤Î¥Ñ¥ï¡¼¤Ë¡¢Éý¹¤¯»ÈÍÑ
#define SPEED_SLOW 30 //´Ø¿ôhand,turnAng¤Ê¤É¤Ç»ÈÍÑ
#define THRESHOLD 45 //ïçÃÍ
#define STEP 1 //Wait¤Î°ú¿ô¤È¤·¤Æ»È¤¦
#define STOP 3 //¡Ö¥Ü¡¼¥ë¤¬¥»¥ó¥µ¤ÎÁ°¤ò²£Àڤ信פ«¤É¤¦¤«¤òÄ´¤Ù¤ëÃÍ
***hand...¥Ü¡¼¥ë¤ò¤Ä¤«¤à´Ø¿ô [#y627f394]
**´Ø¿ô [#i3681e58]
***hand...¥Ü¡¼¥ë¤ò¤Ä¤«¤à [#y627f394]
sub hand(int ang, int plmi) { //¥Ü¡¼¥ë¤ò¤Ä¤«¤àÆ°ºî
/*plmi = 1 ¢ª ¥¢¡¼¥à³«
plmi = -1 ¢ª ¥¢¡¼¥àÊÄ */
RotateMotor(OUT_A, SPEED_SLOW * plmi, ang);
}
***Dist...»ØÄꤷ¤¿µ÷Î¥¿Ê¤à´Ø¿ô [#gc626d6c]
***Dist...»ØÄꤷ¤¿µ÷Î¥¿Ê¤à [#gc626d6c]
const float diameter=5.45; //¥¿¥¤¥ä¤Îľ·Â(cm)
const float pi=3.1415; //±ß¼þΨ
const float track = 10.35; //¥¿¥¤¥ä¤Î¥È¥ì¥Ã¥ÉÉý
sub Dist(float d, int fwd_or_rev){ //dcmÁ°Å¾(¸åž)
/*fwd_or_rev = 1 ¢ª Á°Å¾
fwd_or_rev = -1 ¢ª ¸åž */
long angle;
angle = d / (diameter * pi) * 360.0;
RotateMotorEx(OUT_BC, SPEED * fwd_or_rev, angle, 0, true, true);
Wait(100);
}
***turnAng...»ØÄꤷ¤¿³ÑÅÙÀû²ó¤¹¤ë´Ø¿ô [#l31c83ee]
***turnAng...»ØÄꤷ¤¿³ÑÅÙÀû²ó¤¹¤ë [#l31c83ee]
sub turnAng(int ang, int R_or_L) //angÅÙÀû²ó
{
/* R_or_L = 1 ¢ª »þ·×²ó¤ê
R_or_L = -1 ¢ª Ⱦ»þ·×²ó¤ê */
long angle;
angle = track / diameter * ang;
RotateMotorEx(OUT_BC, SPEED_SLOW , angle, 100* R_or_L, true, true);
Wait(100);
}
***searchDirection...ºÇûµ÷Î¥¤ÎÊý¤ò¸þ¤¯´Ø¿ô [#wd8a8862]
***searchDirection...ºÇûµ÷Î¥¤ÎÊý¤ò¸þ¤¯ [#wd8a8862]
int searchDirection(int ang) //¥Ü¡¼¥ë¤òõ¤¹´Ø¿ô
{
SetSensorLowspeed(S1);
long angle, tacho_min = 0, tacho_corr;
int d_min;
d_min= 300; //²¾¤ÎºÇ¾®ÃÍ
angle = (track / diameter) * ang;
turnAng(10, 1); //10ÅÙ±¦Àû²ó(´Ì¤Î¸íǧ¤òËɤ°¤¿¤á¡¢10ÅÙ¤·¤«Àû²ó¤·¤Ê¤¤)
ResetTachoCount(OUT_B);
OnFwdSync(OUT_BC, SPEED, -100);
while(MotorTachoCount(OUT_B) <= angle) { //angle¤À¤±º¸Àû²ó
if (SensorUS(S1) < d_min) {
d_min = SensorUS(S1);
tacho_min = MotorTachoCount(OUT_B);
}
}
if (d_min < 30) { //¡ÖĶ²»ÇÈ¥»¥ó¥µ¤Î¼¨¤·¤¿ºÇ¾®Ã͡פ¬30°Ê²¼¤Î¤È¤
OnFwdSyncEx(OUT_BC,SPEED, 100, RESET_NONE); //ºÇ¾®ÃͤÀ¤Ã¤¿Êý¸þ¤ò¸þ¤¯
until(MotorTachoCount(OUT_B) <= tacho_min || SensorUS(S1) <= d_min);
}
else { //30°Ê¾å¤Î¤È¤
turnAng(ang - 10, 1); //ang-10¤À¤±±¦Àû²ó(´Ø¿ô³«»ÏÁ°¤ÈƱ¤¸¸þ¤¤ËÌá¤ë)
}
Off(OUT_BC); Wait(500); //0.5ÉôÖÄä»ß
return d_min; //¡ÖĶ²»ÇÈ¥»¥ó¥µ¤Î¼¨¤·¤¿ºÇ¾®Ã͡פòÊÖ¤¹
}
***¥é¥¤¥ó¥È¥ì¡¼¥¹¤¹¤ë¤¿¤á¤Î´Ø¿ô [#l58de8e2]
***¥é¥¤¥ó¥È¥ì¡¼¥¹¤¹¤ë [#l58de8e2]
´Ø¿ô¡Ötrace¡×¤Ç»È¤¦´Ø¿ô
sub turn_left(int speedA, int speedB) { //º¸¤Ë¶Ê¤¬¤ë
OnFwd(OUT_B,speedA); //°ú¿ôspeedA¤Î¥Ñ¥ï¡¼¤Ç¡¢¥â¡¼¥¿B¤òÁ°Å¾
OnRev(OUT_C,speedB); //°ú¿ôspeedB¤Î¥Ñ¥ï¡¼¤Ç¡¢¥â¡¼¥¿C¤ò¸åž
}
sub turn_right(int speedA, int speedB) { //±¦¤Ë¶Ê¤¬¤ë
OnFwd(OUT_C,speedB); //°ú¿ôspeedB¤Î¥Ñ¥ï¡¼¤Ç¡¢¥â¡¼¥¿C¤òÁ°Å¾
OnRev(OUT_B,speedA); //°ú¿ôspeedA¤Î¥Ñ¥ï¡¼¤Ç¡¢¥â¡¼¥¿B¤ò¸åž
}
sub go_forward(int speed3) { //¤Þ¤Ã¤¹¤°¿Ê¤à
OnFwd(OUT_BC,speed3); //speed3¤Î¥Ñ¥ï¡¼¤Ç¡¢¥â¡¼¥¿BC¤òÁ°Å¾
}
´Ø¿ôtrace...¡ÖtimeÉðÊÆâ¡¢¤«¤Ä¡¢TachoCount < target¡×¤Î´Ö¥é¥¤¥ó¥È¥ì¡¼¥¹(ľ¿Ê¤¹¤ë¥Ñ¥ï¡¼ = speed3)
1.¸òº¹ÅÀ¤«¤É¤¦¤«¤ò¸«¶Ë¤á¤ëºÝ¤Ë¡¢¥¿¥¤¥Þ¡¼¤Ç¤Ï¤Ê¤¯¥¿¥¤¥ä¤Î²óž¤·¤¿³ÑÅÙ(TachoCount)¤òÍøÍÑ
Î㤨¤Ð¡¢º¸Àû²ó¤·¤¿»þ(¸÷¥»¥ó¥µÃÍ < THRESHOLD - 15)¤Î¥¿¥¤¥ä¤Î²óž¿ô¤¬target¤òĶ¤¨¤¿¤È¤¡¢¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò½ªÎ»¤¹¤ë
2.¡Ö¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò½ªÎ»¤¹¤ë»þ´Ö¡×¤òÀßÄê¤Ç¤¤ë
¥é¥¤¥ó¥È¥ì¡¼¥¹¤ò³¤±¤ë»þ´Ö¤ò¡¢°ú¿ôtime¤ÇÍ¿¤¨¤é¤ì¤ë¡£1¤ÎÊýË¡¤¬¼ºÇÔ¤·¤¿¤È¤¤ÎÊݸ±¤Ç»È¤¨¤ë¡£
sub trace(int time, long target, int speed3) {
SetSensorLight(S2);
int speed1 = 25; //¥â¡¼¥¿¤Î¥Ñ¥ï¡¼ÍÑ
int speed2 = 20; //¥â¡¼¥¿¤Î¥Ñ¥ï¡¼ÍÑ(speed1¤è¤ê¾¯¤·¼å¤¤)
long tacho = 0; //¡Ö¤½¤Î¾ì¤Çº¸Àû²ó¤¹¤ë¡×Æ°ºî¤ò¤·Â³¤±¤¿¤È¤¤Î¡¢²óž³Ñ¤Î¹ç·×
long t; //¥¿¥¤¥Þ¡¼¤ò»È¤¦¤È¤¤Ë»ÈÍÑ
t = CurrentTick(); //¤³¤³¤Ç¸½ºß»þ¹ï¤òt¤ËÂåÆþ(CurrentTick() - t¡¡¤È»È¤¦¤³¤È¤Ç¡¢·Ð²á»þ´Ö¤¬¤ï¤«¤ë)
while (CurrentTick() - t < time && tacho < target) {
if (SENSOR_2 < THRESHOLD -15){ //¸÷¥»¥ó¥µ¤Î¿ôÃͤ¬¡¢THRESHOLD-15¤è¤êÄ㤤¤È¤
turn_left(speed1, speed1); //º¸±¦¤Î¥â¡¼¥¿¤òspeed1¤Çº¸Àû²ó(¤½¤Î¾ì¤Çº¸²óž)
} else if (SENSOR_2 < THRESHOLD -7) { //¸÷¥»¥ó¥µ¤Î¿ôÃͤ¬¡¢THRESHOLD-7¤è¤êÄ㤤¤È¤
tacho = 0; //tacho(turn_left1¤·Â³¤±¤¿»þ¤Î¡¢¥¿¥¤¥ä¤Î²óž³Ñ¤Î¹ç·×)¤ò0¤Ë¤¹¤ë¡£
turn_left(speed1, speed2); //±¦¥â¡¼¥¿¤òspeed1¡¢º¸¥â¡¼¥¿¤òspeed2(speed1¤è¤ê¾¯¤·¼å¤¤)¤Ë¤·¤Æ¡¢º¸Àû²ó
} else if (SENSOR_2 < THRESHOLD +7){ //¸÷¥»¥ó¥µ¤Î¿ôÃͤ¬¡¢THRESHOLD+7¤è¤êÄ㤤¤È¤
tacho = 0; //tacho¤ò0¤Ë¤¹¤ë
go_forward(speed3); //º¸±¦¤Î¥â¡¼¥¿¤òspeed3¤Ë¤·¤Æ¡¢Ä¾¿Ê
} else if (SENSOR_2 < THRESHOLD +15){ //¸÷¥»¥ó¥µ¤Î¿ôÃͤ¬¡¢THRESHOLD+15¤è¤êÄ㤤¤È¤
tacho = 0; /*tacho¤ò0¤Ë¤¹¤ë*/
turn_right(speed1, speed2); /*±¦¥â¡¼¥¿¤òspeed2¡¢º¸¥â¡¼¥¿¤òspeed1¤Ë¤·¤Æ¡¢±¦Àû²ó*/
} else { /*¸÷¥»¥ó¥µ¤Î¿ôÃͤ¬¡¢THRESHOLD+15¤è¤ê¹â¤¤¤È¤*/
tacho = 0; /*tacho¤ò0¤Ë¤¹¤ë*/
turn_right(speed1, speed1); /*º¸±¦¤Î¥â¡¼¥¿¤òspeed1¤Ç±¦Àû²ó(¤½¤Î¾ì¤Ç±¦²óž)*/
}
Wait(STEP); //STEP(0.001)ÉäÀ¤±Æ°¤¯
if (SENSOR_2 >= THRESHOLD-15) { /*¸÷¥»¥ó¥µ¤Î¿ôÃͤ¬¡¢THRESHOLD-15¤è¤ê¹â¤¤¤È¤ (=¤½¤Î¾ì¤Çº¸²óž¤·¤Æ¤¤¤Ê¤¤¤È¤) */
ResetTachoCount(OUT_B); /*¥â¡¼¥¿¤Î²óž¿ô¤ò¥ê¥»¥Ã¥È(¤½¤Î¾ì¤Çº¸²óž¤·¤¿¤È¤°Ê³°¤Ï¥ê¥»¥Ã¥È)*/
}
tacho += MotorTachoCount(OUT_B); /*tacho¤Ë¡¢¥â¡¼¥¿¤Î²óž³Ñ¤òÂåÆþ(¡Ö¤½¤Î¾ì¤Çº¸²óž¤·¤¿»þ¡×¤Î²óž³Ñ¤·¤«ÂåÆþ¤µ¤ì¤Ê¤¤)*/
}
Off(OUT_BC); Wait(STEP);
PlaySound(SOUND_CLICK);
}
**¥á¥¤¥ó´Ø¿ô [#z16c056e]
***¼ÂºÝ¤ÎÆ°ºî [#ac6032c0]
¥×¥í¥°¥é¥à¤ÎÆ°¤¤ò²èÁü¤Ç¼¨¤·¤¿¡£(/*(1)*/¤Î²èÁü¤Ï¡¢/*(1)*/¤Î¥×¥í¥°¥é¥à¤ÈϢư¤·¤Æ¤¤¤ë)
/*--------(1)----------*/
#ref(2015b/Member/rmsun/Mission3/¥í¥Ü¥³¥ó1.gif,480*332)
/*--------(2)---------*/
#ref(2015b/Member/rmsun/Mission3/2015b-missiona.jpg,480*332)
/*--------(3)---------*/
#ref(2015b/Member/rmsun/Mission3/¥í¥Ü¥³¥ó3.gif,480*332)
/*--------(4)----------*/(¢«¤³¤ÎÆ°ºî¤Ï¡¢²þÁ±¤Î;ÃϤ¢¤ê)
#ref(2015b/Member/rmsun/Mission3/¥í¥Ü¥³¥ó4.gif,480*332)
***main¤Î¥×¥í¥°¥é¥à [#y460ebf3]
task main() {
/*----------(1)--------------*/
/*-------------(1)--------------*/
long t;
Wait(30000); //30ÉÃÂÔµ¡
Dist(48, 1); //48cmÁ°¿Ê
turnAng(91, -1); //90ÅÙº¸Àû²ó
SetSensorLowspeed(S1); //Ķ²»ÇÈ¥»¥ó¥µµ¯Æ°
int ball = 0; //ÊÑ¿ô¡Öball¡×...Ķ²»ÇÈ¥»¥ó¥µ¤ÎÃÍ < 30 ¤ò¼¨¤·¤¿²ó¿ô¤ò¼¨¤¹
while (1) { //¥Ü¡¼¥ë¤¬Ä̲᤹¤ëÍͻҤòÄ´¤Ù¤ë
if (SensorUS(S1) < 30) { /*¥»¥ó¥µÃÍ < 30(¥Ü¡¼¥ë¤¬¥»¥ó¥µ¤ÎÁ°¤òÄ̤ê²á¤®¤¿¤È¤¡¢¥»¥ó¥µÃͤ¬20ÄøÅÙ¤ò¼¨¤¹¤¿¤á)*/
ball++;
}
if (ball == STOP) { //Ä̲ᤷ¤¿²ó¿ô¤¬STOP(=3)¤Ë¤Ê¤Ã¤¿¤é
break; //¥ë¡¼¥×¤òÈ´¤±½Ð¤¹
}
Wait(STEP); //STEP(0.001)ÉôÖ
}
Wait(800); //0.8ÉôÖÂÔµ¡
PlaySound(SOUND_CLICK); //¼¡¤ÎÆ°ºî¤ËÆþ¤ë¤³¤È¤òÃΤ餻¤ë
t = CurrentTick();
while (CurrentTick() - t < 2000) { //2Éäδַ«¤êÊÖ¤¹
if (SensorUS(S1) < 30) { //¥»¥ó¥µÃÍ < 30¤Î¤È¤
ball++; //ball + 1 ..¤Ä¤Þ¤êball = STOP + 1
break; //¥ë¡¼¥×¤òÈ´¤±½Ð¤¹
}
Wait(STEP);
}
/*------------(2)------------*/
/*------------(2)----------------*/
if (ball == STOP + 1) { //Ķ²»ÇÈ¥»¥ó¥µ < 30¤ò¼¨¤¹¤Þ¤ÇÁ°¿Ê
OnFwd(OUT_BC, SPEED_SLOW); //SPEED_SLOW(=30)¤Î¥Ñ¥ï¡¼¤ÇÁ°¿Ê
while (ball < STOP + 2) {
if (SensorUS(S1) < 30) {
ball++;
}
}
Off(OUT_BC); //1ÉÃÄä»ß
Wait(1);
Dist(35, 1); //35cmÁ°¿Ê
hand(70, -1); //¥Ü¡¼¥ë¤ò¤Ä¤«¤à
/*-----(²«Àþ)-----*/
turnAng(30, -1); //30ÅÙ±¦Àû²ó(¥é¥¤¥ó¥È¥ì¡¼¥¹¤Î¹õÀþ¤ËÆþ¤ë¤¿¤á)
Dist(8, 1); //8cmÁ°¿Ê(¥é¥¤¥ó¥È¥ì¡¼¥¹¤Î¹õÀþ¤ËÆþ¤ë¤¿¤á)
/*-----(ÎÐÀþ)-----*/
trace(20000, 15, 40); /*¡Ö20ÉðÊÆâ && TachoCount < 15¡×¤Î´Ö¥é¥¤¥ó¥È¥ì¡¼¥¹*/
/*-----(¿å¿§Àþ)---*/
trace(7000, 1000, 50); /*¡Ö7ÉðÊÆâ && TachoCount < 1000¡×¤Î´Ö¥é¥¤¥ó¥È¥ì¡¼¥¹*/
/*------(ÀÄÀþ)----*/
trace(15000, 10, 40); /*¡Ö15ÉðÊÆâ && TachoCount < 10¡×¤Î´Ö¥é¥¤¥ó¥È¥ì¡¼¥¹*/
/*------(»çÀþ)----*/
turnAng(75, 1); //75ÅÙ±¦Àû²ó
Dist(15,1); //15cmÁ°¿Ê
/*------(ÀÖÀþ)-----*/
turnAng(20, 1); //20ÅÙ±¦Àû²ó
SetSensorLight(S2); //¸÷¥»¥ó¥µ¡¼µ¯Æ°
OnFwd(OUT_BC, SPEED); //¹õÀþ¤Þ¤ÇSPEED(=50)¤Î¥Ñ¥ï¡¼¤ÇÁ°¿Ê
while (SENSOR_2 > THRESHOLD - 15) {}
Off(OUT_BC); //Ää»ß
hand(70, 1); //¥Ü¡¼¥ë¤òÎ¥¤¹
}
/*-----(3)--------*/
/*-------------(3)------------*/
else {
turnAng(90, -1); //90ÅÙº¸Àû²ó
Dist(40, 1); //40cm¿Ê¤à
turnAng(90, 1); //90ÅÙ±¦Àû²ó
Off(OUT_BC); Wait(STEP); //0.001ÉôÖÄä»ß
while (1) {
int d = searchDirection(60); //60ÅÙ¤ÎÈϰϤǥܡ¼¥ë¤òõ¤¹
if (d < 30) { /*searchDirection(60)¤ÎÊÖ¤êÃÍ < 30¤Î¤È¤(õ¤·¤¿ÈϰϤ˥ܡ¼¥ë¤¬¤¢¤Ã¤¿¤È¤)*/
Dist(20, 1); //20cm¿Ê¤à
hand(70, -1); //¥Ü¡¼¥ë¤ò¤Ä¤«¤à
break; //¥ë¡¼¥×¤«¤éÈ´¤±½Ð¤¹
}
else {
OnFwd(OUT_BC, SPEED); //0.5Éÿʤà
Wait(500);
searchDirection(0); /*searchDirection¤ò2Ϣ³¤Ç»È¤¦¤È¥Ð¥°¤ë¤¿¤áseachDirection(0)¤ò¤Ï¤µ¤à*/
}
}
/*------------(4)---------------*/
trace(10000, 10000, 45); //¤É¤³¤«¤Î¥é¥¤¥ó¤Ë¤Î¤ë,¥í¥Ü¥Ã¥È¤Î¸þ¤¤ò¥´¡¼¥ëÊý¸þ¤Ë¸þ¤±¤ë
Dist(40, 1); //40cm¿Ê¤à¡¢
SetSensorLight(S2); //¸÷¥»¥ó¥µ¡¼µ¯Æ°
OnFwd(OUT_BC, SPEED); //SPEED(=50)¤Î¥Ñ¥ï¡¼¤Ç¡¢¹õÀþ¤ËÅþ㤹¤ë¤Þ¤Çľ¿Ê
while (SENSOR_2 > THRESHOLD - 15) {}
trace(3000, 10000, 40); //¡Ö3ÉðÊÆâ && TachoCount < 10000¡×¤Î´Ö¥é¥¤¥ó¥È¥ì¡¼¥¹
hand(70, 1); //¥Ü¡¼¥ë¤òÎ¥¤¹
}
}
*¥³¥á¥ó¥È[#od03dfdc]
**¹©É×ÅÀ [#oe6a4c64]
-¥Ü¡¼¥ë¤¬´Ì¤ËÅö¤¿¤Ã¤¿¸å¤Î¡Ö¥Ü¡¼¥ë¤¬Ä·¤ÍÊ֤俾ì¹ç¡×¤È¡Ö¥Ü¡¼¥ë¤¬¤½¤Î¤Þ¤Þ¿Ê¤ó¤À¾ì¹ç¡×¤Î°ã¤¤¤ò¡¢Ä¶²»ÇÈ¥»¥ó¥µ¤Ç¥í¥Ü¥Ã¥È¤Ë¶èÊ̤µ¤»¤¿¡£¤½¤·¤Æ¡¢¾ì¹ç¤Ë¤è¤Ã¤ÆÆ°ºî¤òÊѤ¨¤¿¡£
-¡Ö¥Ü¡¼¥ë¤¬¤¢¤Ã¤¿¤é¥Ü¡¼¥ë¤ÎÊý¸þ¤ò¸þ¤¡¢¥Ü¡¼¥ë¤¬¤Ê¤«¤Ã¤¿¤é¸µ¤ÎÊý¸þ¤ò¸þ¤¯¡×´Ø¿ô(searchDirection)¤òÍѤ¤¡¢¥Ü¡¼¥ë¤¬¸«¤Ä¤«¤Ã¤¿¤é¥Ü¡¼¥ë¤òÄϤߡ¢¥Ü¡¼¥ë¤¬¸«¤Ä¤«¤é¤Ê¤«¤Ã¤¿¤é¾¯¤·¿Ê¤ó¤ÇƱ¤¸Æ°ºî¤ò·«¤êÊÖ¤¹¤è¤¦¤Ë¤·¤¿(¡Ömain¥×¥í¥°¥é¥à¡×¤Î/*(3)*/»²¾È)¡£¤³¤¦¤¹¤ë¤³¤È¤Ç¡¢¹ÈϰϤΥܡ¼¥ë¤òõ¤»¤ë¤è¤¦¤Ë¤Ê¤Ã¤¿¡£
**È¿¾ÊÅÀ [#b3dc3c0c]
-/*(4)*/¤ÎÉôʬ¤¬ÉÔ°ÂÄê¡£¥Ü¡¼¥ë¤Î°ÌÃ֤ˤè¤Ã¤Æ¡¢¥´¡¼¥ë¤ËÅþÃ夷¤Ê¤¤¾ì¹ç¤¬Â¿¤¤¡£
-Ķ²»ÇÈ¥»¥ó¥µ¤ËÍê¤ê¤¹¤®¤¿¡£Ä¶²»ÇÈ¥»¥ó¥µ¤¬Í½´ü¤»¤ÌÆ°ºî¤ò¤·¤¿¤é¡¢¥í¥Ü¥Ã¥È¤Ï¤¦¤Þ¤¯Æ°¤«¤Ê¤¤¡£
-´Ì¤è¤ê¼êÁ°(¥í¥Ü¥Ã¥È¦)¤Ë¥Ü¡¼¥ë¤¬Èô¤ó¤Ç¤¤¿¤È¤¡¢´Ì¤ËÅö¤¿¤ëÁ°¤Ë¥í¥Ü¥Ã¥È¤Ë¥Ü¡¼¥ë¤¬¿¨¤ì¤ë´í¸±¤¬¤¢¤ë¡£
-¡ÖÆó¸ÄÌܤΥܡ¼¥ë¤òõ¤¹¤¿¤á¤Î¥×¥í¥°¥é¥à¡×¤¬½ñ¤±¤Ê¤«¤Ã¤¿(ºî¶È»þ´Ö¤ÎÇÛʬ¥ß¥¹)
-Åꤲ¤ë¦¤Î¥í¥Ü¥Ã¥È¤òĶ²»ÇÈ¥»¥ó¥µ¤¬¸íǧ¤¹¤ë¤È¤¤¬¤¢¤Ã¤¿¡£
-Åꤲ¤ë¦¤Î¥í¥Ü¥Ã¥È¤È¾×Æͤ¹¤ë¤È¤¤¬¤¢¤Ã¤¿¡£
**´¶ÁÛ [#q20ee53a]
-¥Ï¡¼¥É¥¦¥§¥¢¤ÎÀǽ¤Ë¤è¤Ã¤Æ¡¢¥í¥Ü¥Ã¥È¤Î¤Ç¤¤ë¤³¤È¤¬Â礤¯ÊѤï¤Ã¤¿¡£Î㤨¤Ð¡¢Ä¶²»ÇÈ¥»¥ó¥µ¤ò»È¤¦¤À¤±¤Ç¤â¡¢¤Ç¤¤ë¤³¤È¤¬Â礤¯Áý¤¨¤¿¡£
-Ķ²»ÇÈ¥»¥ó¥µ¤¬¤¦¤Þ¤¯µ¡Ç½¤·¤Ê¤¤¤È¤¡¢¥í¥Ü¥Ã¥È¤Î¸íºîÆ°¤òËɤ°¤³¤È¤¬¤Ç¤¤Ê¤«¤Ã¤¿¡£¤³¤Î¤³¤È¤«¤é¡¢¥Ï¡¼¥É¥¦¥§¥¢¤ÎÀºÅÙ¤ò¹â¤á¤ë¤³¤È¤Î½ÅÍ×À¤¬¤ï¤«¤Ã¤¿¡£¤½¤·¤Æ¡¢¥Ï¡¼¥É¥¦¥§¥¢¤ËÍê¤êÀÚ¤ê¤Î¥×¥í¥°¥é¥à¤òºî¤ë¤È¡¢È¬ÊýºÉ¤¬¤ê¤Ë¤Ê¤ë¤³¤È¤¬¤ï¤«¤Ã¤¿¡£