[[2014a/Member]] Ìܼ¡ #contents *²ÝÂê¤ÎÀâÌÀ [#r13fce05] ¾Ü¤·¤¯¤Ï[[2014a/Mission2]]¤ò¸«¤Æ¤¯¤À¤µ¤¤¡£ *¥á¥ó¥Ð¡¼ [#na188e61] tadanobo mame dai bgpad *¥í¥Ü¥Ã¥ÈËÜÂÎ [#bdd03821] &ref(2014a/Member/mame/Mission2/S__2424847.jpg,30%); ¥¢¡¼¥à¡¢Æ¹ÂΡ¢¥É¥é¥¤¥Ö¥Ù¡¼¥¹¤È¡¢¥Ñ¡¼¥Ä¤´¤È¤Ëʬ¤±¤¿¤Î¤ÇÁȤßΩ¤Æ¡¢¼è¤ê³°¤·¤¬ÍưפǤ¢¤ë **¥¢¡¼¥à [#y6f68c61] &ref(2014a/Member/mame/Mission2/¼Ì¿¿ 2014-07-25 16 36 37(1).jpg,25%); ¡¥³¥Ã¥×¤ò¤Ä¤«¤à &ref(2014a/Member/mame/Mission2/¼Ì¿¿ 2014-08-07 16 24 50.jpg,7%); ¢¥¢¡¼¥à¤¬²¼¤ê¤ë &ref(2014a/Member/mame/Mission2/aiueo.jpg,35%); £Â¾¤Î¥³¥Ã¥×¤Ë½Å¤Í¤ë **¥É¥é¥¤¥Ö¥Ù¡¼¥¹ [#je9abe0b] &ref(2014a/Member/mame/Mission2/¼Ì¿¿ 2014-08-07 16 25 32.jpg,10%); µ¡ÂΤ¬½Å¤¤¤Î¤Ç¥Ù¡¼¥¹¤Ï´è¾æ¤Ë¤·¤¿¡£¤Þ¤¿¡¢¸åÎؤϥ¹¥à¡¼¥º¤ËÆ°¤¯¤è¤¦¤ËÂ礤ʤâ¤Î¤Ë¤·¤¿ *ºîÀï [#b8f08a37] ¥³¥Ã¥×¤ò³Î¼Â¤Ë¤È¤ë¤¿¤á¤ËÆó²ó¥³¥Ã¥×¤òȽÃǤ¹¤ë¥×¥í¥°¥é¥à¤òÆþ¤ì¤¿¡£ Ķ²»ÇÈ¥»¥ó¥µ¡¼¤Ç¥³¥Ã¥×¤Î°ÌÃÖ¤ò³Îǧ¤·¡¢¸÷¥»¥ó¥µ¡¼¤Ç¥³¥Ã¥×¤ÎÈÖ¹æ¤ò¸«Ê¬¤±¤ë¤è¤¦¤Ë¤·¤¿¡£ ¤Þ¤¿¡¢¾¯¤·¤Ç¤â³ÑÅÙ¤¬¤º¤ì¤Ê¤¤¤è¤¦¤Ë¥³¥Ã¥×¤ò¤È¤ëËè¤Ë³ÑÅÙ¤ò½¤Àµ¤¹¤ë¥×¥í¥°¥é¥à¤òÆþ¤ì¤¿¡£ ¼ç¤Ë¹ÔÆ°¤¹¤ë¤¿¤á¤Î¿Æµ¡¡¢¼ç¤Ë¥³¥Ã¥×¤ò¤È¤ë¤¿¤á¤Î»Òµ¡¤Ë¥×¥í¥°¥é¥à¤òʬ¤±¤¿¡£ *¥×¥í¥°¥é¥à [#z80241ef] **´ðËÜ¤Î¥×¥í¥°¥é¥à [#yb10803f] ***ÄêµÁ [#ua28c278] #define LEFT_WHEEL OUT_C //º¸¤Î¥¿¥¤¥ä¤Î¥â¡¼¥¿¡¼ #define RIGHT_WHEEL OUT_A //±¦¤Î¥¿¥¤¥ä¤Î¥â¡¼¥¿¡¼ #define BOTH_WHEEL OUT_AC //ξ¥¿¥¤¥ä¤Î¥â¡¼¥¿¡¼ #define ARM_BASE OUT_A //ËÜÂΦ¤Î¥¢¡¼¥à¤Î¥â¡¼¥¿¡¼ #define ARM_HAND OUT_B //¥¢¡¼¥àÀè¤Î¥â¡¼¥¿¡¼ #define DIST_SENSOR S2 //µ÷Υ¬ÄêÍÑĶ²»ÇÈ¥»¥ó¥µ¡¼ #define TYPE_SENSOR S3 //»æ¥³¥Ã¥×ȽÊÌÍѸ÷¥»¥ó¥µ¡¼ ***¥á¥¤¥ó¥×¥í¥°¥é¥à [#k87d7c7e] void InitSensor(){ SetSensorLowspeed(DIST_SENSOR); SetSensorLight(TYPE_SENSOR); } **µ¡ÂΤιÔÆ°¤¹¤ë¥×¥í¥°¥é¥à [#edcd34a9] ***ÄêµÁ [#w23ba2df] #define SPEED_STRAIGHT 50 #define SPEED_SPIN 40 #define SPEED_STRAIGHT_LOW 20 #define SPEED_SPIN_LOW 30 #define STAY_WAIT 50 #define WHEEL_STRAIGHT (80 / 1715) ***¥á¥¤¥ó¥×¥í¥°¥é¥à [#h5000208] void DebugDriveType(string type, int speed){ DebugNumber(DEBUG_DRIVETYPE, type, speed); } void Stay(){ Off(BOTH_WHEEL); DebugDriveType("Stay", 0); } void StayWait(){ Stay(); Wait(STAY_WAIT); } void Drive(int speed, int rate, string type){ if(speed){ if(speed > 0){ OnFwdSync(BOTH_WHEEL, speed, rate); }else{ OnRevSync(BOTH_WHEEL, -speed, rate); } DebugDriveType(type, speed); }else{ } } void DriveStraight(int speed){ Drive(speed, 0, "Straight"); } void DriveLeft(int speed){ Drive(speed, -100, "Left"); } void DriveRight(int speed){ Drive(speed, 100, "Right"); } void Forward(){ DriveStraight(SPEED_STRAIGHT); } void ForwardLow(){ DriveStraight(SPEED_STRAIGHT_LOW); } void Back(){ DriveStraight(-SPEED_STRAIGHT); } void BackLow(){ DriveStraight(-SPEED_STRAIGHT_LOW); } void SpinLeft(){ DriveLeft(SPEED_SPIN); } void SpinLeftLow(){ DriveLeft(SPEED_SPIN_LOW); } void SpinRight(){ DriveRight(SPEED_SPIN); } void SpinRightLow(){ DriveRight(SPEED_SPIN_LOW); } void Spin(int degree){ if(degree > 0){ StartDirectionTimer(); SpinLeft(); until(StopDirectionTimer() >= degree); }else{ StartDirectionTimer(); SpinRight(); until(StopDirectionTimer() <= degree); } } void SpinLow(int degree){ if(degree > 0){ StartDirectionTimer(); SpinLeftLow(); until(StopDirectionTimer() >= degree); }else{ StartDirectionTimer(); SpinRightLow(); until(StopDirectionTimer() <= degree); } } void GoForward(int x){ int d; float p = MotorRotationCount(LEFT_WHEEL); Forward(); until((d = x - (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){ DebugDriveType("Forward", d); } Stay(); } void GoForwardLow(int x){ int d; float p = MotorRotationCount(LEFT_WHEEL); ForwardLow(); until((d = x - (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){ DebugDriveType("ForwardLow", d); } Stay(); } void GoBack(int x){ int d; float p = MotorRotationCount(LEFT_WHEEL); Back(); until((d = x + (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){ DebugDriveType("Back", d); } Stay(); } void GoBackLow(int x){ int d; float p = MotorRotationCount(LEFT_WHEEL); BackLow(); until((d = x + (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){ DebugDriveType("BackLow", d); } Stay(); } void GoStraight(int x){ if(x > 0){ GoForward(x); }else{ GoBack(-x); } } void GoStraightLow(int x){ if(x > 0){ GoForwardLow(x); }else{ GoBackLow(-x); } } **¥³¥Ã¥×¤òõ¤¹¤¿¤á¤Î¥×¥í¥°¥é¥à [#w225d5ad] ***ÄêµÁ [#p8f79d0c] #define DIST_CATCH 12 #define DIST_STACK 11 #define DIST_SEARCH 6 #define SEARCH_DIRECTION1 90 #define SEARCH_DIRECTION2 120 #define DistSensor SensorUS(DIST_SENSOR) ***¥á¥¤¥ó¥×¥í¥°¥é¥à [#ie6d8cf2] void TowardCup(int degree){ int min = DistSensor; float dir = GetDirection(); SpinLow(-degree * 0.5); StartDirectionTimer(); SpinLeftLow(); while(StopDirectionTimer() < degree){ if(DistSensor < min){ min = DistSensor; dir = GetDirection(); } } SpinRightLow(); until(GetDirection() <= dir); Stay(); } int PrepareCup(int dist){ int x, y; TowardCup(SEARCH_DIRECTION1); x = DistSensor - DIST_SEARCH; GoForwardLow(x); StayWait(); TowardCup(SEARCH_DIRECTION2); y = dist - DistSensor; GoBackLow(y); StayWait(); return x - y; } int PrepareCatch(){ return PrepareCup(DIST_CATCH); } int PrepareStack(){ return PrepareCup(DIST_STACK); } **¥³¥Ã¥×¤òȽÃǤ¹¤ë¤¿¤á¤Î¥×¥í¥°¥é¥à [#h13b91a8] ***ÄêµÁ [#uac0f5e7] #define DIRECTION_DEBUG 1 #define DEBUG_DRIVETYPE 2 #define DEBUG_CUPTYPE 3¡¡ #define DEBUG_CUPSENSOR 3 ***¥á¥¤¥ó¥×¥í¥°¥é¥à [#c68e677d] void Debug(int line, string text){ TextOut(0, (8 - line) * 8, StrCat(text, " ")); } void DebugText(int line, string name, string value){ Debug(line, StrCat(name, ": ", value)); } void DebugNumber(int line, string name, int num){ DebugText(line, name, NumToStr(num)); } void DebugSound(int freq){ PlayTone(freq, 100); } **Bluetooth¤Î¥×¥í¥°¥é¥à [#e5c11d2e] ***ÄêµÁ [#pea36e5f] #define BT_CONN 1 #define BT_MAILBOX MAILBOX3 #define BT_NULL 0 #define BT_CATCH 1 #define BT_STACK 2 #define BT_FINISH 3 ***¥á¥¤¥ó¥×¥í¥°¥é¥à [#m0f1915a] void BTStart(string file){ until(BluetoothStatus(BT_CONN) == NO_ERR); RemoteStartProgram(BT_CONN, file); } int BTReceiveMessage(){ int msg = BT_NULL; while(msg == BT_NULL){ ReceiveRemoteNumber(BT_MAILBOX, true, msg); DebugNumber(5, "debug", msg); } return msg; } void BTSendMessage(int msg, bool isMaster){ if(isMaster){ SendRemoteNumber(BT_CONN, BT_MAILBOX, msg); }else{ SendResponseNumber(BT_MAILBOX, msg); } } void BTStop(){ BTSendMessage(BT_FINISH, true); } **¥¢¡¼¥à¤Î¥×¥í¥°¥é¥à [#n48ddf38] ***ÄêµÁ [#x6c230d3] #define DOWN_POWER 30 #define HOLD_DOWN_POWER 50 #define UP_POWER 90 #define HOLD_UP_POWER 50 #define HOLD_POWER 20 #define RELEASE_POWER 80 #define ARM1_DEGREE 90 #define ARM2_DEGREE 210 #define CUP1 55 #define CUP2 37 #define CUP3 23 #define TypeSensor SENSOR_3 ***¥á¥¤¥ó¥×¥í¥°¥é¥à [#o58a78f8] void DownArm(){ Off(ARM_BASE); Wait(100); RotateMotor(ARM_BASE, DOWN_POWER, ARM1_DEGREE); OnFwd(ARM_BASE, HOLD_DOWN_POWER); Wait(1000); } void UpArm(){ Off(ARM_BASE); Wait(100); RotateMotor(ARM_BASE, UP_POWER, -ARM1_DEGREE); OnRev(ARM_BASE, HOLD_UP_POWER); Wait(2000); } void HoldCup(){ OnFwd(ARM_HAND, HOLD_POWER); while(MotorRotationCount(ARM_HAND) < ARM2_DEGREE); } void ReleaseCup(){ RotateMotor(ARM_HAND, RELEASE_POWER, -ARM2_DEGREE); } void CatchCup(){ HoldCup(); Wait(500); UpArm(); Wait(500); ReleaseCup(); HoldCup(); } void StackCup(){ DownArm(); Wait(500); ReleaseCup(); Wait(500); } int GetCupType(){ int n = 0; if(TypeSensor > (CUP1 + CUP2) * 0.5){ n = 1; }else if(TypeSensor > (CUP2 + CUP3) * 0.5){ n = 2; }else{ n = 3; } DebugNumber(DEBUG_CUPTYPE, "CupNumber", n); DebugNumber(DEBUG_CUPSENSOR, "CupSensor", TypeSensor); return n; } void MasterCatchCup(){ BTSendMessage(BT_CATCH, true); BTReceiveMessage(); } void MasterStackCup(){ BTSendMessage(BT_STACK, true); BTReceiveMessage(); } **³ÑÅÙÄ´À°¤Î¥×¥í¥°¥é¥à [#b5f9e30c] ***ÄêµÁ [#xe4a73ec] #define BODY_WIDTH 12.6 #define DIRECTION_COE (0.044 / BODY_WIDTH * 360 / 2 / PI) ***¥á¥¤¥ó¥×¥í¥°¥é¥à [#d9b4c61d] float direction = 0; float leftMoterCount = 0; float rightMoterCount = 0; float directionTimer = 0; float GetDirection(){ float out = MotorRotationCount(RIGHT_WHEEL); float in = MotorRotationCount(LEFT_WHEEL); direction += (out - leftMoterCount - in + rightMoterCount) * DIRECTION_COE; leftMoterCount = out; rightMoterCount = in; DebugNumber(DIRECTION_DEBUG, "Direction", direction); return direction; } void StartDirectionTimer(){ directionTimer = GetDirection(); } float StopDirectionTimer(){ return GetDirection() - directionTimer; } **¿Æµ¡¤Î¥×¥í¥°¥é¥à [#w0bdc2b7] ***ÄêµÁ [#p7c1d702] #include "debug.h" #include "bluetooth.h" #include "robot.h" #include "direction.h" #include "drivebase.h" #include "search.h" #include "arm.h" ***¥á¥¤¥ó¥×¥í¥°¥é¥à [#reaffc66] void GotoCatchCup(){ GoForward(2); StayWait(); Spin(-90); StayWait(); GoForward(60); StayWait(); Spin(90); StayWait(); } void GotoStackCup(){ GoForward(2); StayWait(); Spin(-90); StayWait(); GoBack(55); StayWait(); Spin(90); StayWait(); } void BeginCupA(){ Spin(45); StayWait(); GoForward(15); StayWait(); } void BeginCupB(){ Spin(-60); StayWait(); } void BeginCupC(){ GoForward(30); StayWait(); } void EndCupA(){ Spin(45 - GetDirection()); StayWait(); GoBack(10); StayWait(); } void EndCupB(){ Spin(-60 - GetDirection()); StayWait(); } void EndCupC(){ Spin(-GetDirection()); StayWait(); GoBack(25); StayWait(); } void BeginCup(int n){ switch(n){ case 1: DebugSound(TONE_C4); BeginCupA(); break; case 2: DebugSound(TONE_C4); BeginCupB(); break; case 3: DebugSound(TONE_E4); BeginCupC(); break; } } void EndCup(int n){ switch(n){ case 1: EndCupA(); break; case 2: EndCupB(); break; case 3: EndCupC(); break; } } void Cup(int phase){ int x, n; GotoCatchCup(); BeginCup(phase); x = PrepareCatch(); MasterCatchCup(); GoStraight(-x); EndCup(phase); Spin(-GetDirection()); GotoStackCup(); n = GetCupType(); BeginCup(n); x = PrepareStack(); MasterStackCup(); GoStraight(-x); EndCup(n); Spin(-GetDirection()); } ***¥á¥¤¥ó¥¿¥¹¥¯ [#i8a27cb2] task main(){ GoForward(4); StayWait(); BTStart("slave.rxe"); InitSensor(); for(int i = 1; i <= 3; i++){ Cup(i); } BTStop(); } **»Òµ¡¤Î¥×¥í¥°¥é¥à [#r0bb1a13] ***ÄêµÁ [#g2a189fc] #include "debug.h" #include "bluetooth.h" #include "robot.h" #include "arm.h" ***¥á¥¤¥ó¥¿¥¹¥¯ [#i1fb1f38] task main(){ int msg; while(msg != BT_FINISH){ msg = BTReceiveMessage(); switch(msg){ case BT_CATCH: Debug(1, "Catch"); DownArm(); CatchCup(); BTSendMessage(1, false); break; case BT_STACK: Debug(1, "Stack"); StackCup(); UpArm(); BTSendMessage(1, false); break; } Debug(1, "Wait"); } Debug(1, "Finish"); Wait(5000); } *È¿¾Ê¤È´¶ÁÛ [#dc1c78a7] À®¸ùΨ¤ò¾å¤²¤ë¤¿¤á¤Ë²¿ÅÙ¤âÄ´À°¤·Ä¾¤·¤¿¡£ ËÜÈ֤Υí¥Ü¥³¥ó¤ÇºÇ¸å¤Î¥³¥Ã¥×¤ò¤Ä¤«¤ß»¤Í¤Æ¤·¤Þ¤Ã¤¿¡£ ¤«¤Ê¤ê°ÂÄêÀ¤Î¹â¤¤µ¡ÂÎ¤È¥×¥í¥°¥é¥à¤Ë¤Ê¤Ã¤¿¡£ µ¡ÂΤγˤòºî¤Ã¤Æ¤¯¤ì¤¿¥á¥ó¥Ð¡¼¡¢¶¦¤ËÄ´À°¤·¤Æ¤¯¤ì¤¿¥á¥ó¥Ð¡¼¡¢ ¥×¥í¥°¥é¥à¤òÃæ¿´¤Ë½ñ¤¤¤Æ¤¯¤ì¤¿¥á¥ó¥Ð¡¼¤Ë´¶¼Õ¤Ç¤¢¤ë¡£