コード例 #1
0
void Go_Ball_Area(Let *let) {
    static uint8 step = 0;
    static uint16 count = 0;
    static uint16 limit = 1;

    if(let->area != 4)
    {
        Line_Trace(let, MODE_FORWARD);
    }
    else if(let->area == 4)
    {
        if(let->place > 0)
        {
            let->mode = MODE_MOVE;
            return;
        }
        if(step == 0)
        {
            Motor_Right(0);
            Motor_Left(0);
            limit = 50;
        } else if(step == 1) {
            PID_Motor_Left(0);
            PID_Motor_Right(0);
            limit = 50;
        }
        else if(step == 2)
        {
            Motor_Right(-7000);
            Motor_Left(-7000);
            limit = 40;
        } else if(step == 3)
        {
            Motor_Right(0);
            Motor_Left(0);
            limit = 20;
        }
        else if(step == 4)
        {
            let->mode = MODE_SEEK;
            step = 0;
            return;
        }
        if(limit == count)
        {
            count = 0;
            step++;
            return;
        }
        count++;
    }
}
コード例 #2
0
void Shooting_tennis_ball(Let *let) {

    static uint8 step = 0;
    static uint16 count = 0;
    static uint16 limit = 1;

    if(let->area <= 0)
    {
        Line_Trace(let, MODE_FORWARD);
    }
    else if(let->area == 1)
    {
        if(step == 0)
        {
            Motor_Right(7000);
            Motor_Left(-200);
            limit = 80;
        } else if(step == 1)
        {
            Motor_Right(0);
            Motor_Left(0);
            let->updown = DOWN;
            let->grab = RELEASE;
            limit = 10;
        } else if(step == 2)
        {
            let->updown = DOWN;
            limit = 100;
        } else if(step == 3)
        {
            let->updown = UP;
            Motor_Right(-7000);
            limit = 75;
        } else if(step == 4)
        {
            Motor_Right(0);
            Motor_Left(0);
            let->mode = MODE_LINE_TRACE;
        }
        if(limit == count)
        {
            count = 0;
            step++;
            return;
        }
        count++;
    }
}
コード例 #3
0
void move(Let *let)
{
    static uint16 count = 0;
    uint16 limit = 10;
    if(let->place == 1)
    {
        let->mode = MODE_SEEK;
        return;
    }
    else if(let->place > 0)
    {
        Line_Trace(let,MODE_FORWARD);
    }
    count++;
    if(count == (limit*let->place))
    {
        count = 0;
        let->mode = MODE_SEEK;
        return;
    }
}
コード例 #4
0
void Return(Let *let)
{
    static uint8 step = 0;
    static uint16 count = 0;
    static uint16 limit = 1;
    Sensor_LED_Write(0);
    if(let->place < 3)
    {
        if(step == 0)
        {
            Motor_Right(-3500);
            Motor_Left(-3500);
            //limit = let->count + let->place*40;
            limit = let->count;
        } else if(step == 1)
        {
            if(let->count_r > 0)
            {
                Motor_Right(3000);//反時計
                Motor_Left(-3000);
                limit = (int)(let->count_r);
            }
            else if(let->count_r <= 0)
            {
                Motor_Right(-3000);
                Motor_Left(3000);
                limit = (int)(-let->count_r);
            }
        } else if(step == 2)
        {
            if(let->color == MISS)
            {
                Motor_Right(0);
                Motor_Left(0);
                step = 0;
                count = 0;
                let->count = 0;
                let->count_r = 0;
                let->mode = MODE_SEEK;
                return;
            }
            if((Switch_L_Read() == 1) && (Switch_R_Read() == 1))
            {
                step++;
                Motor_Right(0);
                Motor_Left(0);
                count = 0;
                return;
            }
            else
            {
                Motor_Right(-7000);
                Motor_Left(-7000);
                limit = 0;
            }
        } else if(step == 3)
        {
            Motor_Right(7000);
            Motor_Left(7000);
            limit = 35;
        }
        else if(step == 4)
        {
            Motor_Right(0);
            Motor_Left(7000);
            limit = 60;
        } else if(step == 5)
        {
            Motor_Right(7000);
            Motor_Left(7000);
            let->area = 3;
            PID_init(let);
            UART_Line_Sensor_ClearRxBuffer();
            let->slave.Trans = 0;
            limit = 35;
        } else if(step == 6)
        {
            Line_Trace(let,MODE_BACKWARD);
            limit = 0;
        }
    }
    else if(let->place >= 3)
    {
        if(step == 0)
        {
            Motor_Right(-3500);
            Motor_Left(-3500);
            limit = 0;
            if(UART_Line_Sensor_GetRxBufferSize())
            {
                let->slave.Trans = (uint8)UART_Line_Sensor_GetChar();
            }
            if(let->slave.status.e)
            {
                step = 1;
                count = 0;
                Debug_LED_Write(1);
                return;

            }
        } else if(step == 1)
        {
            if(let->count_r > 0)
            {
                Motor_Right(3000);//反時計
                Motor_Left(-3000);
                limit = (int)(let->count_r + 20);
            }
            else if(let->count_r <= 0)
            {
                Motor_Right(-3000);
                Motor_Left(3000);
                limit = (int)(-let->count_r);
            }
            UART_Line_Sensor_ClearRxBuffer();
            let->slave.Trans = 0;
        } else if(step == 2)
        {
            if(let->color == MISS)
            {
                Motor_Right(0);
                Motor_Left(0);
                step = 0;
                count = 0;
                let->count = 0;
                let->count_r = 0;
                let->mode = MODE_SEEK;
                return;
            }
            if((Switch_L_Read() == 1) && (Switch_R_Read() == 1))
            {
                step++;
                Motor_Right(0);
                Motor_Left(0);
                count = 0;
                return;
            }
            else
            {
                Motor_Right(-7000);
                Motor_Left(-7000);
                limit = 0;
            }
        } else if(step == 3)
        {
            Motor_Right(7000);
            Motor_Left(7000);
            limit = 35;
        }
        else if(step == 4)
        {
            Motor_Right(0);
            Motor_Left(7000);
            limit = 60;
        } else if(step == 5)
        {
            Motor_Right(7000);
            Motor_Left(7000);
            let->area = 3;
            PID_init(let);
            UART_Line_Sensor_ClearRxBuffer();
            let->slave.Trans = 0;
            limit = 35;
        } else if(step == 6)
        {
            Line_Trace(let,MODE_BACKWARD);
            limit = 0;
        }
    }
    if(let->color == RED)
    {
        if(let->area == 0)
        {
            Motor_Right(0);
            Motor_Left(0);
            count = 0;
            step = 0;
            let->mode = MODE_SHOOT;
        }
    }
    else if(let->color == YELLO)
    {
        if(let->area == 1)
        {
            Motor_Right(0);
            Motor_Left(0);
            count = 0;
            step = 0;
            let->mode = MODE_SHOOT;
        }
    }
    else if(let->color == BLUE)
    {
        if(let->area == 2)
        {
            Motor_Right(0);
            Motor_Left(0);
            count = 0;
            step = 0;
            let->mode = MODE_SHOOT;
        }
    }
    count++;
    if(limit == count)
    {
        count = 0;
        step++;
        return;
    }
}
コード例 #5
0
ファイル: main.cpp プロジェクト: open-rdc/irc2015
///*
int main(){     //第3回試走会用
    int ball_count = 0;
    bool area_flag = OFF;
    float serch_posi = 0.0;
    float move_dis = 0.0;
    state = init_state;
    
//    color = RED;
    
    while(1){       //while開始
        switch(state){
            case init_state:
                initialize();
                state = free_ball;
                //state = ball_search;
                //state = finish;
                break;
            
            case free_ball:
                Line_Trace(3);
                turn(85.0, FAST);
                wait(0.25);
                servo_throw();
                wait(0.25);
                servo_ini();
                wait(0.25);
                turn(-85.0,FAST);
                Line_Trace(2);
                state = ball_search;
                break;
                
            case ball_search:
                    
                if(move_flag){
                    while(l_Y <= move_dis)
                        Line_Trace();
                    stop();
                    move_flag = OFF;
                }
                else{
                    if(color != NO_BALL){
                        while(l_Y <= serch_posi)
                            Line_Trace();
                        stop();
                    }
                }
            
                serch();
                state = ball_shoot;
                //if((l_time+35) > LIMIT)
                //    state = finish;
                break;
                
            case ball_shoot:
                wait(0.25);
                //color = what_color();
                if(color != NO_BALL){         //色を得られたら
                    //turn(180, FAST);
                    //wait(0.25);
                    if(area_flag){
                        if(color == YELLO)
                            Line_Trace(color);
                        else
                            Line_Trace(color + 1);
                    }
                    else{
                        if(color == YELLO)
                            Line_Trace(color - 1);
                        else
                            Line_Trace(color);
                    }
                    
                    Ball_Shoot();
                    ball_count++;
                    if(ball_count == B_GOAL || (l_time+35) > LIMIT){
                        state = finish;
                        break;
                    }
                    if(color == YELLO)
                        Line_Trace(color - 1);
                    else
                        Line_Trace(color);
                }
                //目標まで行ったかの確認
                state = ball_search;
                               
                if(move_flag)
                    state = area_move;
                break;
                
            case area_move:
                move_dis = 250;
                if(serch_posi >= 1200 - move_dis)      //行き過ぎ防止用
                    move_dis = 0;
                serch_posi += move_dis;
                area_flag = ON;
                state = ball_search;
                break;
            
            case finish:
                fine();
                break;
            
        }//switch終了
    }//while終了
}//main終了