[[2014a/Member]]

*²ÝÂê¤ÎÀâÌÀ [#r13fce05]

¾Ü¤·¤¯¤Ï[[2014a/Mission2]]¤ò¸«¤Æ¤¯¤À¤µ¤¤¡£

*¥á¥ó¥Ð¡¼ [#na188e61]

tadanobo

mame

dai

bgpad

*¥í¥Ü¥Ã¥ÈËÜÂÎ [#bdd03821]

&ref(2014a/Member/mame/Mission2/S__2424847.jpg,70%);S__2424847.jpg
&ref(2014a/Member/mame/Mission2/S__2424847.jpg,30%);

**¥¢¡¼¥à [#y6f68c61]

&ref(2014a/Member/mame/Mission2/¼Ì¿¿ 2014-07-25 16 36 37(1).jpg,70%);
&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,8%);
­¢¥¢¡¼¥à¤¬²¼¤ê¤ë
&ref(2014a/Member/mame/Mission2/aiueo.jpg,35%);
­£Â¾¤Î¥³¥Ã¥×¤Ë¤Ï¤á¤ë

**Ķ²»ÇÈ¥»¥ó¥µ¡¼ [#t96c02d0]
**¥É¥é¥¤¥Ö¥Ù¡¼¥¹ [#je9abe0b]

&ref(2014a/Member/mame/Mission2/¼Ì¿¿ 2014-08-07 16 25 32.jpg,30%);


*ºîÀï [#b8f08a37]



*¥×¥í¥°¥é¥à [#z80241ef]
 long timerStarted = 0; //¥¿¥¤¥Þ¡¼¤ò¥»¥Ã¥È
 
 #define MAX(a, b) (a > b ? a : b)
 #define MIN(a, b) (a < b ? a : b)
 #define ABS(a) (a < 0 ? -a : a)
 }long timerStarted = 0; //¥¿¥¤¥Þ¡¼¤ò¥»¥Ã¥È

 void StartTimer(){
    timerStarted = CurrentTick(); //»þ´Ö¤ò¿ô¤¨»Ï¤á¤ë 
 }

 long StopTimer(){
    return CurrentTick() - timerStarted;
 }

 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);
 }

 #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 TRACE_SENSOR S1 //¥È¥ì¡¼¥¹ÍѸ÷¥»¥ó¥µ¡¼
 #define DIST_SENSOR S2 //µ÷Υ¬ÄêÍÑĶ²»ÇÈ¥»¥ó¥µ¡¼
 #define TYPE_SENSOR S3 //»æ¥³¥Ã¥×ȽÊÌÍѸ÷¥»¥ó¥µ¡¼
 
 void InitSensor(){
    SetSensorLight(TRACE_SENSOR);
    SetSensorLowspeed(DIST_SENSOR);
    SetSensorLight(TYPE_SENSOR);
 }

 #define BODY_WIDTH 12.6
 #define DIRECTION_COE (0.044 / BODY_WIDTH * 360 / 2 / PI)
 
 #define DIRECTION_DEBUG 1
 
 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;
 }

 #define THRESHOLD 43
 
 #define SPEED_FORWARD 70
 #define SPEED_TURN 80
 #define SPEED_SPIN 60
 #define SPEED_FORWARD_LOW 25
 #define SPEED_SPIN_LOW 40
 
 #define TraceSensor SENSOR_1
 
 #define DEBUG_TRACETYPE 3
 #define DEBUG_TRACESENSOR 2
  
 #define WHEEL_STRAIGHT (80 / 1715)
 
 int lastTrace = -1;
 int forwardCount = 0;
 float rightDirection = 0;
 
 void Stay(){
    Off(BOTH_WHEEL);
    DebugText(DEBUG_TRACETYPE, "State", "Stay");
 }
 
 void GoForward(){
    OnFwdSync(BOTH_WHEEL, SPEED_FORWARD, 0);
    DebugText(DEBUG_TRACETYPE, "State", "GoForward");
 }
 
 void GoBack(){
    OnRevSync(BOTH_WHEEL, SPEED_FORWARD, 0);
    DebugText(DEBUG_TRACETYPE, "State", "GoBack");
 }
 
 void SpinLeft(){
    OnFwdSync(BOTH_WHEEL, SPEED_SPIN, -100);
    DebugText(DEBUG_TRACETYPE, "State", "SpinLeft");
 } 
 
 void SpinRight(){
    OnFwdSync(BOTH_WHEEL, SPEED_SPIN, 100);
    DebugText(DEBUG_TRACETYPE, "State", "SpinRight");
 }
 
 void GoForwardLow(){
    OnFwdSync(BOTH_WHEEL, SPEED_FORWARD_LOW, 0);
    DebugText(DEBUG_TRACETYPE, "State", "GoForwardLow");
 }
 
 void GoBackLow(){
    OnRevSync(BOTH_WHEEL, SPEED_FORWARD_LOW, 0);
    DebugText(DEBUG_TRACETYPE, "State", "GoBackLow");
 }
 
 void SpinLeftLow(){
    OnFwdSync(BOTH_WHEEL, SPEED_SPIN_LOW, -100);
    DebugText(DEBUG_TRACETYPE, "State", "SpinLeftLow");
 }
 
 void SpinRightLow(){
    OnFwdSync(BOTH_WHEEL, SPEED_SPIN_LOW, 100);
    DebugText(DEBUG_TRACETYPE, "State", "SpinRightLow");
 }
 
 void TurnLeft(){
    Stay();
    OnFwd(RIGHT_WHEEL, SPEED_TURN);
    DebugText(DEBUG_TRACETYPE, "State", "TurnLeft");
 }
 
 void TurnRight(){
    Stay();
    OnFwd(LEFT_WHEEL, SPEED_TURN);
    DebugText(DEBUG_TRACETYPE, "State", "TurnRight");
 }
 
 void UntilBlack(){
    until(TraceSensor < THRESHOLD);
 }
 
 void UntilWhite(){
    until(TraceSensor > THRESHOLD);
 }
 
 void UntilLine(){
    UntilBlack();
    UntilWhite();
 }
  
 int Trace(){
    int n;
    float d;
    if(TraceSensor < THRESHOLD - 8){
        SpinLeft();
        n = 0;
    }else if(TraceSensor < THRESHOLD - 2){
        TurnLeft();
        n = 1;
    }else if(TraceSensor < THRESHOLD + 2){
        GoForward();
        n = 2;
    }else if(TraceSensor < THRESHOLD + 8){
        TurnRight();
        n = 3;
    }else{
        SpinRight();
        d = GetDirection();
        if(lastTrace == 4){
            if(d - rightDirection < -360){
                PlayTone(440, 100);
                GoBack();
                UntilBlack();
            }
        }else{
            rightDirection = d;
        }
        n = 4;
    }
    DebugNumber(DEBUG_TRACESENSOR, "State", TraceSensor);
    if(n == 2){
        forwardCount++;
    }else{
        forwardCount = 0;
    }
    lastTrace = n;
    return n;
 }
 
 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 GoForwardTo(int x){
    float a = MotorRotationCount(LEFT_WHEEL);
    GoForward();
    until((MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT >= x)DebugNumber(6, "fwd", x - (MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT);
    Stay();
 }
 
 void GoForwardToLow(int x){
    float a = MotorRotationCount(LEFT_WHEEL);
    GoForwardLow();
    until((MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT >= x)DebugNumber(6, "fwd", x - (MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT);
    Stay();
 }
 
 void GoBackTo(int x){
    float a = MotorRotationCount(LEFT_WHEEL);
    GoBack();
    until((MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT <= -x)DebugNumber(6, "back", (MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT - x);
    Stay();
 }
 
 void GoBackToLow(int x){
    float a = MotorRotationCount(LEFT_WHEEL);
    GoBackLow();
    until((MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT <= -x)DebugNumber(6, "back", (MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT - x);
    Stay();
 }

 #define DIST_CATCH 13
 #define DIST_STACK 13
 
 #define DistSensor SensorUS(DIST_SENSOR)
 
 void TowardCup(int degree){
    int min = DistSensor;
    float right = GetDirection();
    SpinLow(-degree * 0.5);
    StartDirectionTimer();
    SpinLeftLow();
    while(StopDirectionTimer() < degree){
        if(DistSensor < min){
            min = DistSensor;
            right = GetDirection();
        }
    }
    SpinRightLow();
    until(GetDirection() <= right);
    Stay();
 }
 
 int PrepareCatch(){
    int l = 0;
    TowardCup(90);
    GoForwardToLow(DistSensor - 5);
    l += DistSensor - 5;
    Stay();
    TowardCup(120);
    GoBackToLow(DIST_CATCH - DistSensor);
    l += DistSensor - DIST_CATCH;
    Stay();
    return l * WHEEL_STRAIGHT;
 }
 
 int PrepareStack(){
    int l = 0;
    TowardCup(90);
    GoForwardToLow(DistSensor - 5);
    l += DistSensor - 5;
    Stay();
    TowardCup(120);
    GoBackToLow(DIST_STACK - DistSensor);
    l += DistSensor - DIST_STACK;
    Stay();
    return l * WHEEL_STRAIGHT;
 }

 #define DOWN_POWER 30
 #define UP_POWER 90
 #define POWER 20
 
 #define ARM1_DEGREE 90
 #define ARM2_DEGREE 210
 
 #define CUP1 45 //¥³¥Ã¥×£±¤Î¤·¤­¤¤ÃÍ
 #define CUP2 30 //¥³¥Ã¥×£²¤Î¤·¤­¤¤ÃÍ
 #define CUP3 20 //¥³¥Ã¥×£³¤Î¤·¤­¤¤ÃÍ
 
 #define TypeSensor SENSOR_3
 
 void InitArm(){
    SetSensorLight(DIST_SENSOR);
 }
 
 void DownArm(){
    Off(ARM_BASE);
    Wait(100);
    RotateMotor(ARM_BASE, DOWN_POWER, ARM1_DEGREE);
    OnFwd(ARM_BASE, 50);
    Wait(1000);
 }
 
 void UpArm(){
    Off(ARM_BASE);
    Wait(100);
    RotateMotor(ARM_BASE, UP_POWER, -ARM1_DEGREE);
    OnRev(ARM_BASE, 50);
    Wait(2000);
 }
 
 void HoldCup(){
    OnFwd(ARM_HAND, POWER);
    int s = 0;
    while(MotorRotationCount(ARM_HAND) < ARM2_DEGREE){
        if(DistSensor > s){
            s = DistSensor;
        }
    };
 }
 
 void ReleaseCup(){
    RotateMotor(ARM_HAND, 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;
    DebugNumber(2, "sensor", TypeSensor);
    if(TypeSensor > CUP1){
        n = 1;
        DebugSound(TONE_C4);
    }else if(TypeSensor > CUP2){
        n = 2;
        DebugSound(TONE_D4);
    }else if(TypeSensor > CUP3){
        n = 3;
        DebugSound(TONE_E4);
    }
    return n;
 }

 #define BT_CONN 1
 #define BT_MAILBOX MAILBOX3
 
 #define BT_NULL 0
 #define BT_CATCH 1
 #define BT_STACK 2
 #define BT_FINISH 4
 
 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 GoCup1(){
    InitSensor();
    Spin(-90);
    Stay();Wait(100);
    GoForwardTo(60);
    Stay();Wait(100);
    Spin(135);
    Stay();Wait(100);
    GoForwardTo(10);
    Stay();Wait(100);
 }

 void BackCup1(int l){
    GoBackTo(10 + l);
    Stay();Wait(100);
    Spin(-direction - 90);
    Stay();Wait(100);
    GoBackTo(50);
    Stay();Wait(100);
    Spin(90);
 }

 void GoCup2(){
    Spin(-90);
    Stay();Wait(100);
    GoForwardTo(60);
    Stay();Wait(100);
    Spin(45);
    Stay();Wait(100);
 }

 void BackCup2(int l){
    GoBackTo(10 + l); 
    Stay();Wait(100);
    Spin(-direction - 90);
    Stay();Wait(100);
    GoBackTo(50); 
    Stay();Wait(100);
    Spin(90); 
 }

 void GoCup3(){
    InitSensor();
    Spin(-90);
    Stay();Wait(100);
    GoForwardTo(60);
    Stay();Wait(100);
    Spin(90);
    Stay();Wait(100);
    GoForwardTo(30);
    Stay();Wait(100);
 }

 void BackCup3(){
    GoBackTo(35); 
    Stay();Wait(100);
    Spin(-direction - 100); 
    Stay();Wait(100);
    GoBackTo(60);
    Stay();Wait(100);
    Spin(90);
 }

 task main(){
    int l;
    BTStart("slave3.rxe");
    InitSensor();
    GoCup1();
    l = PrepareCatch();
    //MasterCatchCup();
    BTSendMessage(BT_CATCH, true);
    BTReceiveMessage();
    BackCup1(l);
    int type = GetCupType();
    if(type == 1){  //¥³¥Ã¥×£±¤Î¾ì¹ç
        Spin(45); //45ÅÙ²óž
        GoForwardTo(15);
    }else if(type == 2){
        Spin(-45);
        GoForwardTo(15);
    }else{
        GoForwardTo(30);
    }
    PrepareStack();
    //MasterCatchCup();
    BTSendMessage(BT_STACK, true);
    BTReceiveMessage();
    Stay();Wait(100);
    if(type == 1){
        Spin(45 - GetDirection());
        GoBackTo(15);
    }else if(type == 2){
        Spin(-45 - GetDirection());
        GoBackTo(15);
    }else{
        Spin(-GetDirection());
        GoBackTo(30);
    }
    Spin(-GetDirection());
    Stay();Wait(100);
    
    GoCup2();
    l = PrepareCatch();
    //MasterCatchCup();
    BTSendMessage(BT_CATCH, true);
    BTReceiveMessage();
    BackCup2(l);
    type = GetCupType();
    if(type == 1){
        Spin(45);
        GoForwardTo(15);
    }else if(type == 2){
        Spin(-45);
        GoForwardTo(15);
    }else{
        GoForwardTo(30);
    }
    PrepareStack();
    //MasterCatchCup();
    BTSendMessage(BT_STACK, true);
    BTReceiveMessage();
    Stay();Wait(100);
    if(type == 1){
        Spin(45 - GetDirection());
        GoBackTo(15);
    }else if(type == 2){
        Spin(-45 - GetDirection());
        GoBackTo(15);
    }else{
        Spin(-GetDirection());
        GoBackTo(30);
    }
    Spin(-GetDirection());
    Stay();Wait(100);
    
    GoCup3();
    PrepareCatch();
    //MasterCatchCup();
    BTSendMessage(BT_CATCH, true);
    BTReceiveMessage();
    BackCup3();
    type = GetCupType();
    if(type == 1){
        Spin(45);
        GoForwardTo(15);
    }else if(type == 2){
        Spin(-45);
        GoForwardTo(15);
    }else{
        GoForwardTo(30);
    }
    PrepareStack();
    //MasterCatchCup();
    BTSendMessage(BT_STACK, true);
    BTReceiveMessage();
    Stay();Wait(100);
    if(type == 1){
        Spin(45 - GetDirection());
        GoBackTo(15);
    }else if(type == 2){
        Spin(-45 - GetDirection());
        GoBackTo(15);
    }else{
        Spin(-GetDirection());
        GoBackTo(30);
    }
    Spin(-GetDirection());
    Stay();Wait(100);
    
    BTSendMessage(BT_FINISH, true);
 }

*È¿¾Ê¤È´¶ÁÛ [#dc1c78a7]

À®¸ùΨ¤ò¾å¤²¤ë¤¿¤á¤Ë²¿ÅÙ¤âÄ´À°¤·Ä¾¤·¤¿¡£



¥È¥Ã¥×   ¿·µ¬ °ìÍ÷ ¸¡º÷ ºÇ½ª¹¹¿·   ¥Ø¥ë¥×   ºÇ½ª¹¹¿·¤ÎRSS