Ìܼ¡ #contents * ½ñÆ»¥í¥Ü¥Ã¥È [#kc6aef55] #ref(2015b/Member/rmsun/Mission1/IMG_1057.JPG,360*480,¹â(¸ÇÄê)) #ref(2015b/Member/rmsun/Mission1/IMG_1078.JPG,360*480,¹â(ËÜÈÖ)) * ¥í¥Ü¥Ã¥ÈËÜÂÎ [#cec79a4d] #ref(2015b/Member/rmsun/Mission1/IMG_1060.JPG,360*270,½ñÆ»¥í¥Ü¥Ã¥ÈÀµÌÌ) #ref(2015b/Member/rmsun/Mission1/IMG_1061.JPG,360*480,½ñÆ»¥í¥Ü¥Ã¥È¦ÌÌ) *¥×¥í¥°¥é¥à [#m32e30bc] **¥á¥¤¥ó´Ø¿ô°Ê³°¤Î¥×¥í¥°¥é¥à [#wb572dc0] ***¥Þ¥¯¥í¤ÎÄêµÁ [#ca3c85e6] #define SPEED 30 //Á°Å¾¸åž»þ¤Î¥¹¥Ô¡¼¥É #define WRITE_SPEED 20 //»ú¤ò½ñ¤¯¤È¤¤Î¥¹¥Ô¡¼¥É #define TURN_SPEED 100 //Êý¸þÊÑ´¹»þ¤Î¥¹¥Ô¡¼¥É #define PUT_ANGLE 91 //¥Ú¥ó¤ò¾å¤²²¼¤²¤¹¤ë»þ¡¢¥â¡¼¥¿¤¬²óž¤¹¤ë³ÑÅÙ #define down1() RotateMotor(OUT_A,-30,PUT_ANGLE-20) //¥Ú¥ó¤òÁÇÁ᤯²¼¤²¤ë(ÃÏÌ̤ˤϤĤ±¤Ê¤¤) #define down2() RotateMotor(OUT_A,-5,20) //¥Ú¥ó¤ò¤æ¤Ã¤¯¤êÃÏÌ̤ˤĤ±¤ë #define up() RotateMotor(OUT_A,70,PUT_ANGLE); //¥Ú¥ó¤ò¾å¤²¤ë ***ʸ»ú¤ÎÇÜΨ¤òÀßÄꤹ¤ë¤¿¤á¤ÎÊÑ¿ô¡Ömagni¡× [#y2c1419e] float magni=1.0; //ÇÜΨ¤òÀßÄê Îã:magni=2.0 ¢ª ʸ»ú¤ÎÂ礤µ¤¬2.0ÇÜ ***¥¿¥¤¥ä¤Î²óž¿ô¤ò·×»»¤¹¤ë´Ø¿ô¡ÖGetAngle_d¡×¡ÖGetAngle_s¡× [#rbfc594a] ¥¿¥¤¥ä¤Î²óž¿ô¤ò·×»»¤¹¤ë¤¿¤á¤Ë»È¤¦ÊÑ¿ô float pen_to_center=17.0; //²óž¤ÎÃæ¿´(º¸±¦¤Î¥¿¥¤¥ä¤ÎÃæ´Ö)¤«¤é¥Ú¥ó¤Þ¤Ç¤Îµ÷Î¥ float diameter=5.45; //¥¿¥¤¥ä¤Îľ·Â(cm) float pi=3.1415; //±ß¼þΨ float tire_ang; //µ÷Î¥(cm)¿Ê¤à¤È¤¤ËɬÍפʡ¢¥¿¥¤¥ä¤Î²óž¤¹¤ë³ÑÅÙ ¿Ê¤ß¤¿¤¤µ÷Î¥(cm)¤«¤é¡¢¥¿¥¤¥ä¤Î²óž¿ô¤ò·×»»¤¹¤ë´Ø¿ô¡Êhttp://yakushi.shinshu-u.ac.jp/robotics-jr/robotics-jr-2013.pdf¤Î31¥Ú¡¼¥¸¤Î¥×¥í¥°¥é¥à¤ò»²¹Í¤Ë¤·¤Þ¤·¤¿¡Ë float GetAngle_d(float distance) { //¿Ê¤ß¤¿¤¤µ÷Î¥(cm)¡Ödistance¡×¤«¤é¡¢¥¿¥¤¥ä¤Î²óž¿ô¤ò·×»»¤¹¤ë´Ø¿ô tire_ang=(distance)/(diameter*pi)*360.0; //³ÑÅÙ¤ò·×»»¤¹¤ë return tire_ang; //³ÑÅÙ¤òÊÖ¤¹(float·¿) } Êý¸þÊÑ´¹¤·¤¿¤¤³ÑÅÙ¤«¤é¡¢¥¿¥¤¥ä¤Î²óž¿ô¤ò·×»»¤¹¤ë´Ø¿ô(http://yakushi.shinshu-u.ac.jp/robotics-jr/robotics-jr-2013.pdf¤Î31¥Ú¡¼¥¸¤Ë¤¢¤ëÎý½¬ÌäÂê62¤ò»²¹Í¤Ë¤·¤Þ¤·¤¿) float GetAngle_s(float spin_ang) { //Êý¸þÊÑ´¹¤·¤¿¤¤³ÑÅÙ¡Öspin_ang¡×¤«¤é¡¢¥¿¥¤¥ä¤Î²óž¿ô¤ò·×»»¤¹¤ë´Ø¿ô const float spin_dia=11.4; //¥í¥Ü¥Ã¥È¤¬¤½¤Î¾ì¤Ç1¼þ²óž¤·¤¿»þ¤Ë¡¢¥¿¥¤¥ä¤¬ÃÏÌ̤ËÉÁ¤¯±ß¤Îľ·Âspin_dia(cm) //spin_dia...º¸±¦¤Î¥¿¥¤¥ä¤Î´Ö³Ö tire_ang=(spin_dia*spin_ang)/(diameter); //Êý¸þÊÑ´¹¤·¤¿¤¤³ÑÅÙ¤«¤é¡¢¥¿¥¤¥ä¤Î²óž¤¹¤ë³ÑÅÙ¤ò·×»»¤¹¤ë /*ÌóʬÁ°¤Î¼°¤Ï tire_ang=(spin_dia*pi*(spin_ang/360.0)*360.0)/ (diameter*pi) */ return tire_ang; //³ÑÅÙ¤òÊÖ¤¹(float·¿) } ¾å¤ÎGetAngle_d¤òÍѤ¤¤ë¤³¤È¤Ç¡¢Á°Å¾¸åž¤¹¤ëµ÷Î¥(cm)¤ò¡¢¤½¤Î¤Þ¤Þ°ú¿ô¤Ë¤È¤ì¤ë ¥µ¥Ö¥ë¡¼¥Á¥ó¡Öforward¡×...»ØÄꤷ¤¿µ÷Î¥(cm)¤À¤±Á°Å¾¤¹¤ë sub forward(float fwd){ RotateMotorEx(OUT_BC,SPEED,GetAngle_d(fwd)*magni,0,true,true); //fwd(cm)*magni(ÇÜΨ)Á°Å¾ } ¥µ¥Ö¥ë¡¼¥Á¥ó¡Öreverse¡×...»ØÄꤷ¤¿µ÷Î¥(cm)¤À¤±¸åž¤¹¤ë sub reverse(float rev) { RotateMotorEx(OUT_BC,-SPEED,GetAngle_d(rev)*magni,0,true,true); //rev(cm)*magni(ÇÜΨ)¸åž } ¾å¤ÎGetAngle_s¤òÍѤ¤¤ë¤³¤È¤Ç¡¢²óž¤¹¤ë³ÑÅÙ¤ò¡¢¤½¤Î¤Þ¤Þ°ú¿ô¤Ë¤È¤ì¤ë sub turn(float tn,int pl_mi) { //tn(ÅÙ)Êý¸þÊÑ´¹ /*pl_mi=1 ¢ª ±¦¤Þ¤ï¤ê¤Ë²óž pl_mi=-1 ¢ª º¸¤Þ¤ï¤ê¤Ë²óž*/ RotateMotorEx(OUT_BC,TURN_SPEED,GetAngle_s(tn),100*pl_mi,true,true); //º¸±¦¤Î¥¿¥¤¥ä¤ÎµÕ¸þ¤¤Ë²ó¤¹¤³¤È¤ÇÊý¸þÊÑ´¹ Off(OUT_BC); Wait(100); } ***Êý¸þÊÑ´¹¤¹¤ë¤È¤¤Î´Ø¿ô¡Öspin_re¡× [#d4c2619e] sub spin_re(float spin_ang,int pl_mi) { //¡Ö¥Ú¥ó¤òÃæ¿´¤Ë¡¢spin_angÅٲ󞤹¤ë¡×(¤È¤¤ÈƱ¤¸·ë²Ì¤Ë¤Ê¤ë)´Ø¿ô /*±¦¤Ë²óž¤¹¤ë¤È¤...pl_mi=1 º¸¤Ë²óž¤¹¤ë¤È¤...pl_mi=-1*/ ¡Ö¥í¥Ü¥Ã¥È¤¬²óž¤¹¤ë»þ¤ËÀ¸¤¸¤ë¸íº¹¡×¤ò½¤Àµ¤¹¤ë¤¿¤á¤Î¤â¤Î float gosa1=0; //±¦¤Ë²óž¤¹¤ë¤È¤¤Î¸íº¹¡Ögosa1¡× float gosa2=0; //º¸¤Ë²óž¤¹¤ë¤È¤¤Î¸íº¹¡Ögosa2¡× if (pl_mi>0) { //pl_mi>0(±¦¤Ø¤ÎÊý¸þÊÑ´¹¤Î¤È¤) gosa1=0.5; //±¦¤Ø²óž¤¹¤ë¤È¤¤Î¸íº¹¤ò½¤Àµ¤¹¤ë } else { //º¸¤Ø¤ÎÊý¸þÊÑ´¹¤Î¤È¤ gosa2=0.5; //º¸¤Ø²óž¤¹¤ë¤È¤¤Î¸íº¹¤ò½¤Àµ¤¹¤ë } ¼ÂºÝ¤ÎÊý¸þÊÑ´¹¤ÎÆ°ºî RotateMotorEx(OUT_BC,-50,GetAngle_d(pen_to_center-gosa1),0,true,true); //pen_to_center-gosa1(cm)¸åž(gosa¤Ï²óž¤Î»þ¤Î¥º¥ì¤òÄ´À᤹¤ë¤¿¤á) turn(spin_ang,pl_mi); //spin_ang(ÅÙ)²óž RotateMotorEx(OUT_BC,20,GetAngle_d(pen_to_center-gosa2-gosa1),0,true,true); //pen_to_center-gosa2-gosa1(cm)Á°Å¾ ***»ú¤ò½ñ¤¯¤È¤¤Ë»È¤¦¥µ¥Ö¥ë¡¼¥Á¥ó¡Öwrite¡× [#x81c3f00] sub write(float kaku) //¥Ú¥ó¤ÇÀþ¤ò°ú¤¯ { ¥Ú¥ó¤òÆóÃʳ¬¤Ëʬ¤±¤Æ²¼¤²¤ë¡£°ìÃʳ¬ÌܤǤÏ®¤¤¥¹¥Ô¡¼¥É¤Ç²¼¤í¤·¡¢ÆóÃʳ¬ÌܤǤϤæ¤Ã¤¯¤ê¥Ú¥ó¤òÃÏÌ̤ˤĤ±¤ë¤³¤È¤Ç down1(); //¥Ú¥ó¤òÁÇÁ᤯²¼¤²¤ë(ÃÏÌ̤ˤϤĤ±¤Ê¤¤) Wait(100); down2(); //¥Ú¥ó¤ò¤æ¤Ã¤¯¤êÃÏÌ̤ˤĤ±¤ë Wait(10); RotateMotorEx(OUT_BC,WRITE_SPEED,GetAngle_d(kaku)*magni,0,true,true); //WRITE_SPEED¤Î®¤µ¤Çkaku(cm)¿Ê¤à ¢ª kaku(cm)¤ÎÀþ¤ò½ñ¤¯ up(); //¥Ú¥ó¤ò¾å¤²¤ë } ***¡Ö¸ý¡×¤ò½ñ¤¯¤¿¤á¤Î¥µ¥Ö¥ë¡¼¥Á¥ó¡Ökuti¡× [#k0ad77bc] sub kuti(float tate, float yoko) //¸ý¤Î»ú¤ò½ñ¤¯ //¸ý¤Î½Ä¤ÎŤµ(tate)¤È¡¢²£¤ÎŤµ(yoko)¤ò°ú¿ô¤Ë¤È¤ë { write(tate); //tate(cm)½ñ¤¯ spin_re(90.0,-1); //¥Ú¥ó¤òÃæ¿´¤Ë90ÅÙº¸²óž write(yoko); //yoko(cm)½ñ¤¯ spin_re(90.0,-1); //¥Ú¥ó¤òÃæ¿´¤Ë90ÅÙº¸²óž write(tate); // ¢¤³¤³¤«¤é¾å¤Î·«¤êÊÖ¤·(repeat¤ò»È¤Ã¤¿¤é¸íº¹¤¬½Ð¤¿) spin_re(90.0,-1); write(yoko); spin_re(90.0,-1); } **Êý¸þÊÑ´¹¤¹¤ë¤È¤¤ÎÆ°ºî¤ò¾Ü¤·¤¯ [#f0fe2ef5] ²óž¤ÎÃæ¿´¤Ï¡¢¥â¡¼¥¿B¤È¥â¡¼¥¿C¤Î´Ö¤Ë¤¢¤ë #ref(2015b/Member/rmsun/Mission1/IMG_1078.JPG,360*480,²óž¤ÎÃæ¿´) ¥Ú¥ó¤Î½é´ü°ÌÃÖ #ref(2015b/Member/rmsun/Mission1/IMG_1082.JPG,360*480,¥Ú¥ó¤Î½é´ü°ÌÃÖ) ¡¡Ö²óž¤ÎÃæ¿´¡×¤È¡Ö¥Ú¥ó¤Î°ÌÃ֡פε÷Υʬ¤À¤±¸åž ¤³¤Î¤È¤»È¤Ã¤¿¥×¥í¥°¥é¥à¢¢ RotateMotorEx(OUT_BC,-50,GetAngle_d(pen_to_center-gosa1),0,true,true); //pen_to_center-gosa1(cm)¸åž(gosa¤Ï²óž¤Î»þ¤Î¥º¥ì¤òÄ´À᤹¤ë¤¿¤á) #ref(2015b/Member/rmsun/Mission1/IMG_1085.JPG,360*480,¸åÂà) ¢¥í¥Ü¥Ã¥È¤ò»ØÄê¤Î³ÑÅٲ󞤵¤»¤ë ¼ÂºÝ¤ËÊý¸þÊÑ´¹¤·¤¿¤¤³ÑÅÙʬ¡¢²óž¤µ¤»¤ë ¤³¤Î¤È¤»È¤Ã¤¿¥×¥í¥°¥é¥à¢¢ turn(spin_ang,pl_mi); //spin_ang(ÅÙ)²óž #ref(2015b/Member/rmsun/Mission1/IMG_1086.JPG,360*480,¸åÂà»þ¤Î¥Ú¥ó¤Î°ÌÃÖ) #ref(2015b/Member/rmsun/Mission1/IMG_1089.JPG,360*480,²óž) £¡Ö²óž¤ÎÃæ¿´¡×¤È¡Ö¥Ú¥ó¤Î°ÌÃ֡פε÷Υʬ¤À¤±Á°Å¾¤¹¤ë ¤³¤Î¤È¤»È¤Ã¤¿¥×¥í¥°¥é¥à¢¢ RotateMotorEx(OUT_BC,20,GetAngle_d(pen_to_center-gosa2-gosa1),0,true,true); //pen_to_center-gosa2-gosa1(cm)Á°Å¾ #ref(2015b/Member/rmsun/Mission1/IMG_1094.JPG,360*480,¿Ê¤à) ¤·ë²Ì ¡ÖºÇ½é¤Î°ÌÃÖ¤«¤é¡¢¥Ú¥ó¤òÃæ¿´¤Ë¤·¤Æ²óž¤·¤¿¤È¤¡×¤ÈƱ¤¸·ë²Ì¤¬ÆÀ¤é¤ì¤ë #ref(2015b/Member/rmsun/Mission1/IMG_1095.JPG,360*480,·ë²Ì) **¥á¥¤¥ó´Ø¿ô [#m0a26da1] task main() { up(); //¥Ú¥ó¤òÃÏÌ̤ˤĤ±¤¿¤Þ¤Þ¥¹¥¿¡¼¥È write(1.0); //£±²èÌÜ //£²²èÌܤΰÌÃ֤ޤǰÜÆ° spin_re(90.0,-1); //¥Ú¥ó¤òÃæ¿´¤Ë90ÅÙº¸²óž reverse(2.0); //2.0(cm)¸åž write(4.0); //£²²èÌÜ //£³²èÌܡʸý¤ÎÉôʬ¡Ë¤Þ¤Ç°ÜÆ° reverse(3.0); //3.0(cm)¸åž spin_re(90.0,1); //¥Ú¥ó¤òÃæ¿´¤Ë90ÅÙ±¦²óž forward(0.5); //0.5(cm)Á°Å¾ kuti(1.0, 2.25); //¡Ö¸ý¡×¤Î»ú¤ò½ñ¤¯ //£¶²èÌÜ¡Ê¡Ö¹â¡×¤Îº¸²¼¤Î¡Ö|¡×¤ÎÉôʬ¡Ë¤Þ¤Ç°ÜÆ° forward(1.5); //1.5(cm)Á°Å¾ spin_re(90.0,-1); //¥Ú¥ó¤òÃæ¿´¤Ë90ÅÙº¸²óž reverse(1.25); //1.25(cm)¸åž spin_re(90.0,1); //¥Ú¥ó¤òÃæ¿´¤Ë90ÅÙ±¦²óž write(3.0); //£¶²èÌÜ //£·²èÌÜ(¿¿¤óÃæ¤òÄ̤ë¡Ö°ì¡×¤ÎÉôʬ)¤Þ¤Ç°ÜÆ° reverse(3.0); //3.0(cm)¸åž spin_re(90.0,-1); //¥Ú¥ó¤òÃæ¿´¤Ë90ÅÙº¸²óž write(4.75); //£·²èÌÜ //£¸²èÌÜ(±¦²¼¤Î¡Ö¡Ã¡×¤ÎÉôʬ)¤Þ¤Ç°ÜÆ° spin_re(90.0,1); //¥Ú¥ó¤òÃæ¿´¤Ë90ÅÙ±¦²óž write(3.0); //£¸²èÌÜ spin_re(90.0,1); //¥í¥Ü¥Ã¥È¤Î¸þ¤¤ò£·²èÌܤΡ֡¼¡×¤ÈÊ¿¹Ô¤Ë¤¹¤ë //ºÇ¸å¤Î¡Ö¸ý¡×¤ÎÉôʬ¤Þ¤Ç°ÜÆ° forward(3.5); //3.5(cm)Á°Å¾ spin_re(90.0,-1); //¥Ú¥ó¤òÃæ¿´¤Ë90ÅÙº¸²óž reverse(2.5); //2.5(cm)¸åž kuti(1.0, 2.25); //ºÇ¸å¤Î¡Ö¸ý¡×¤ò½ñ¤¯ }