Smart Sensor & Actuator

🖥️SW Engineer

Smart Sensor & Actuator

앞동네JIHOON 2022. 11. 11. 01:29

1. 최단경로 패치획득게임

1-1. 4X5 또는 5X4 격자구조를 라인 트레이싱하여 도착점까지 모든 정점을 탐색한 후 각 패치의 위치를 디스플레이에 출력한다.1-2. 빨간 정점은 +1점, 파란 정점은 -1점으로 계산하여 가장 많은 점수를 획득하며 최단경로로 출발점까지 복귀하고 점수를 디스플레이에 출력한다.

Step 1. 이동하기

void go(){
    val = 5;

    //if c2 is on color turn left
    if(getColorName(c2) != white)
    {
        setMotorSpeed(lm,nMotorSpeedSetting - val);
        setMotorSpeed(rm,nMotorSpeedSetting + val);
    }
    //if c2 is on white turn right
    else
    {
        setMotorSpeed(lm,nMotorSpeedSetting + val);
        setMotorSpeed(rm,(nMotorSpeedSetting - val)/2);
    }

Theory : 전진할 때는 흰색 바탕에서는 오른쪽으로 꺽고, 간선 위에서는 왼쪽으로 꺽게 하여 항상 간선의 왼쪽을 따라 하나의 센서를 가지고 이동하도록 하였다.
Problem : 정점을 인식하는 과정에서 흰색 위에 있는 경우 정점의 색을 인식하지 않고 지나가버리는 문제가 발생하였다.
Solution : 흰색 바탕을 밟았을 경우를 간선을 밟았을 때보다 더 많이 회전하도록 하여 흰색위에 위치하는 시간을 줄였다.

    // vertex on right side && row ==0 or row is odd number or 0
    if((row==0 || row %2 == 1) && getColorName(c3) !=6){
        int check=0;
        //get c2 on the line
        while(getColorName(c2)==6){
            setMotorSpeed(lm,(nMotorSpeedSetting + val)/2);
            setMotorSpeed(rm,(nMotorSpeedSetting - val)/8);
            sleep(80);
        }
        //while c2 is not white turn right for color
        while(getColorName(c2)!=6 && check < 2){
            setMotorSpeed(lm,(nMotorSpeedSetting + val)/2);
            setMotorSpeed(rm,(nMotorSpeedSetting - val)/8);
            sleep(120);
            //?
            if(getColorName(c2) == red) 
            {
                if (row%2 == 0) S[row][count] = 1;
                else S[row][4-count] = 1;
                playSound(soundBeepBeep); 
                check++;
                sleep(50);
            }
            else if (getColorName(c2) == blue){
                if (row%2 == 0) S[row][count] = -1;
                else S[row][4-count] = -1;
                playTone(220,20);
                check++;
                sleep(50);
            }
            else{
                if (row%2 == 0) S[row][count] = 0;
                else S[row][4-count] = 0;
                playTone(440,20);
                check++;
                sleep(50);
            }
        }
        while(getColorName(c3) !=6){
            //if on color turn left
            if(getColorName(c2) != white)
            {
                setMotorSpeed(lm,nMotorSpeedSetting - val);
                setMotorSpeed(rm,nMotorSpeedSetting + val);
            }
            //if on white turn right
            else
            {
                setMotorSpeed(lm,nMotorSpeedSetting + val);
                setMotorSpeed(rm,nMotorSpeedSetting - val);
            }
        }
        vertex=1;
    }

Theory : row==0 이거나 row가 홀수인 경우에는 오른쪽 센서를 기준으로 흰색이 아닌 경우 정점이라고 판단하고 가운데 센서가 정점의 색을 인식하도록 하였다.
Problem : 노란색은 간선의 기본색인데 이를 파란색으로 오인식하는 문제가 발생하였다.그래서 항상 모든 정점을 파란색이나 빨간색으로 인식하는 오류가 발생하였다.
Solution : check라는 변수를 통해 첫번째 색인식은 오류로 판단하고 두 번째 인식한 색을 정점의 색으로 판단하게 하여 오류를 제거하였다.

위에 나온 정점의 색을 오류없이 인식하고 안정적으로 지나가게 하기 위해 정말 많은 밤을 샜다. 정점을 인식하면 오른쪽으로 일정 시간 회전 후 색을 인식하게 하는 방법,,,정점을 인식하면 멈춘 후 천천히 이동하여 색을 인식해보는 방법,,,등등 정말 할 수 있는 모든 방법을 시도했지만 row의 끝에서나 정점의 색이 노란색인 경우 등등 일정 부분에서의 오류를 줄일 수는 있었지만 이 방법만큼 맵 전체에서 깔끔한 동시에 거의 99.999%의 정확도와 이 후에도 안정적으로 나갈 수 있는 방법은 없었다. 이 방법을 성공적으로 완성시킨 것이 1등을 할 수 있었던 결정적인 요소이자 이 수업의 최종 목적이 아닌가 개인적으로 생각한다!! 😊

// ...row가 짝수인 경우
    else vertex = 0;

    if(vertex ==1){
        count=count+1;
    }
}

row가 짝수인 경우도 동일하게 처리한 후 마무리하였다.

Step 2. 회전하기

void turnLeft()
{
    setMotorSpeed(lm, 40);
    setMotorSpeed(rm, 40);
    sleep(200);
    while( getColorName(c1) != 4)
    {
        setMotorSpeed(lm, -nMotorSpeedSetting * 4/10);
        setMotorSpeed(rm, nMotorSpeedSetting * 4/10);
        sleep(10);
    }
    while( getColorName(c2) != 4)
    {
        setMotorSpeed(lm, -nMotorSpeedSetting * 4/10);
        setMotorSpeed(rm, nMotorSpeedSetting * 4/10);
    }
    while(getColorName(c2) == 4){
        setMotorSpeed(lm, -nMotorSpeedSetting * 4/10);
        setMotorSpeed(rm, nMotorSpeedSetting * 4/10);
    }
    setMotorSpeed(lm, 0);
    setMotorSpeed(rm, 0);
    sleep(100);
}

void turnRight()
{
    setMotorSpeed(lm, 40);
    setMotorSpeed(rm, 40);
    sleep(200);
    while( getColorName(c3) != 4)
    {
        setMotorSpeed(lm, nMotorSpeedSetting * 4/10);
        setMotorSpeed(rm, -nMotorSpeedSetting * 4/10);
    }
    while( getColorName(c2) != 4)
    {
        setMotorSpeed(lm, nMotorSpeedSetting * 4/10);
        setMotorSpeed(rm, -nMotorSpeedSetting * 4/10);
    }
    while(getColorName(c2) == 4){
        setMotorSpeed(lm, nMotorSpeedSetting * 4/10);
        setMotorSpeed(rm, -nMotorSpeedSetting * 4/10);
    }
    while(getColorName(c2)!=4)
    {
        setMotorSpeed(rm,nMotorSpeedSetting * 4/10);
        setMotorSpeed(lm,-nMotorSpeedSetting * 4/10);
    }
    setMotorSpeed(lm, 0);
    setMotorSpeed(rm, 0);
    sleep(100);
}

Theory : 회전하려고 하는 방향에 있는 센서가 간선을 인식할 때까지 회전한 후 가운데 센서가 간선을 인식할 때까지 회전하고 다시 가운데 센서가 간선을 통과해 흰색을 인식할 때까지 회전하도록 하여 90회전을 한다.
Problem : 왼쪽으로 회전하는 경우에는 문제가 없었지만 오른쪽으로 회전하는 경우 간선의 오른쪽으로 벗어나 흰색을 인식한 상태에서 회전이 마무리되어서 이 직진할 때 간선의 왼쪽을 기준으로 만들었기 때문에 엉뚱한 곳으로 빠져나가게 된다.
Solution : 오른쪽 회전은 회전이 끝난 후 다시 간선의 노란색을 인식할 때까지 왼쪽으로 회전하도록 하여 이 후 직진에도 문제가 없도록 하였다.

Step 3. 탐색하기

void completeSearch()
{
    int checksize = 0;
   while(true)
   {
        go();
        if(row == 0 && count == 4 && checksize == 0)
        {
            {
                stopMotor();
                sleep(100);
                setMotorSpeed(lm, 20);
                setMotorSpeed(rm, 20);
                sleep(400);
                stopMotor();
                sleep(100);
            }
            if(getColorName(c2) != white)
            {
                c = 4;
                r = 3;
                setMotorSpeed(lm, -20);
                setMotorSpeed(rm, -20);
                sleep(200);
                stopMotor();
                sleep(100);
            }
            else
            {
                c = 3;
                r = 4;
                setMotorSpeed(lm, -20);
                setMotorSpeed(rm, -20);
                sleep(300);
                stopMotor();
                sleep(100);
            }    
            checksize++;
        }

맵이 5X4인지 4X5인지 첫 줄의 4번째 정점에서 다음 간선의 유무를 통해 판단했다.

        if(count == c+1)
        {
            if(row == r) return;
            if(row%2 ==0)
            {
                turnRight();
            }
            else
            {
                turnLeft();
            }

            count=0;
            row=row+1;

            stopMotor();
            sleep(500);


            if(row%2 == 0)
            {
                while(getColorName(c1)== 6) {
                    //if on color turn left
                    if(getColorName(c2) != white)
                    {
                        setMotorSpeed(lm,(nMotorSpeedSetting - val)/2);
                        setMotorSpeed(rm,(nMotorSpeedSetting + val)/2);
                    }
                    //if on white turn right
                    else
                    {
                        setMotorSpeed(lm,(nMotorSpeedSetting + val));
                        setMotorSpeed(rm,(nMotorSpeedSetting - val)/2);
                    }
                }
                go();
                turnLeft();
            }
            else
            {
                while(getColorName(c3)== 6) {
                    //if on color turn left
                    if(getColorName(c2) != white)
                    {
                        setMotorSpeed(lm,(nMotorSpeedSetting - val)/2);
                        setMotorSpeed(rm,(nMotorSpeedSetting + val)/2);
                    }
                    //if on white turn right
                    else
                    {
                        setMotorSpeed(lm,(nMotorSpeedSetting + val));
                        setMotorSpeed(rm,(nMotorSpeedSetting - val)/2);
                    }
                }
                go();
                turnRight();
            }
        }
    }
}

row의 끝에 도달한 경우 회전하고 row를 늘린 후 count=0로 초기화하고 나서 go함수를 통해 다음 row의 첫 정점의 색을 판단하고 회전 하는 방식으로 모든 정점을 탐색해 나갔다. 탐색 후에는 2차원 배열에 모든 정점의 색을 저장한 후 다시 점수로 바꾸어 저장하였다.

Step 4. 최단 경로, 최대 점수로 복귀하기

for(int i = 0; i<r+1; i++){
        for ( int j = 0; j<c+1 ;j++){
            if(i==0 && j==0) dt[i][j]=S[i][j];
            else if(i==0) dt[i][j] = dt[i][j-1]+S[i][j];
            else if(j==0) dt[i][j] = dt[i-1][j]+S[i][j];
            else dt[i][j]= max(dt[i-1][j],dt[i][j-1])+S[i][j];
        }
    }
    sleep(500);
    int score = dt[r][c];    

    while(r != 0 || c!= 0){
        if(r==0) goLeft();
        else if(c ==0) goUp();
        else if(dt[r-1][c] > dt[r][c-1]) goUp();
        else goLeft();
    }

Theory : 다이나믹 프로그래밍을 이용하여 도착점~출발점까지 모든 정점의 점수를 그 직전 정점의 점수와 합한 것중 최대 점수만 저장하였다. 그리고 최대 점수만 따라서 출발점으로 이동하도록 하였다.

시연영상‼️
https://youtu.be/OUp2KLjicu0 👉🏻 1번문제 45
https://youtu.be/CMb2eE66qq4 👉🏻 1번문제 54

저작자표시 (새창열림)