초음파 센서와 적외선 센서를 이용하 장애물 감지 (인피니언 TC275)

이제 초음파 센서와 적외선 센서를 이용하여 장애물 감지를 수행해 보도록 하겠습니다.

차량 앞에 센서는 아래와 같이 달았습니다.

적외선은 양쪽 장애물을 탐지하는데 사용합니다.

초음파는 전방 장애물을 살피는데 사용합니다.

 

이전 글에서 적외선 센서를 넣어서 장애물을 탐지하는 동작을 수행하였습니다.

그때, 정 가운데 장애물을 인지하지 못하여 자동차가 멈추어 서는 현상이 발견되었습니다.

이제 전방에 초음파 센서를 달아서, 전방의 장애물도 탐지하여 

장애물 회피가 가능하도록 설계를 해보도록 하겠습니다.

코드는 간단하게 아래와 같습니다.

void Unit_WirelessControl(void)
{
    uint8_t u8nuWirelessCmd = 0u;
    SensorAdcRaw* pstSensorAdcRaw = MidAdc_GetAdcGroup0SenseRaw();
    static uint8_t ucWarningFlag = 0u;
    static uint8_t ucWarningUltraFlag = 0u;
    static uint8_t ucFirstDetectionFlag = 0u;
    static uint8_t ucFirstUltraDetectionFlag = 0u;
    float32_t fDuty =0.35f;      

    if(pstSensorAdcRaw->InfRaySense1_Raw >= 2200u || pstSensorAdcRaw->InfRaySense2_Raw >= 2200u)
    {
        if(ucFirstDetectionFlag == 0u)
        {
            if(pstSensorAdcRaw->InfRaySense1_Raw > pstSensorAdcRaw->InfRaySense2_Raw) 
            {
                u8nuWirelessCmd = 'a'; /*TurnLeft*/
            }
            else
            {
                u8nuWirelessCmd = 'd'; /*TurnRight*/
            }
            ucFirstDetectionFlag = 1u;
            ucWarningFlag = 1u;
        }
    }
    else if(fUltraDistance < 5.0f) /*Distance < 5Cm*/
    {
        if(ucFirstUltraDetectionFlag == 0u)
        {
            u8nuWirelessCmd = 'x'; /*Reverse*/
            ucFirstUltraDetectionFlag = 1u;
            ucWarningUltraFlag = 1u;
        }
    }
    else
    {    
        if(ucWarningFlag == 1u)
        {
            gu32nuAscRxData = 'w';
            ucWarningFlag = 0u;
            ucFirstDetectionFlag = 0u;
        }

        if(ucWarningUltraFlag == 1u)
        {
            gu32nuAscRxData = 'w';
            ucWarningUltraFlag = 0u;
            ucFirstUltraDetectionFlag = 0u;
        }
        
        u8nuWirelessCmd = (uint8_t)gu32nuAscRxData;
    }
      
    if(u8nuWirelessCmd == 'w')    /*Forward*/
    {
        Unit_MotorFrontDirectionCtl(MOTOR_FWD);
        Unit_MotorRearDirectionCtl(MOTOR_FWD);
    }
    else if(u8nuWirelessCmd == 'd') /*TurnRight*/
    {
        Unit_MotorFrontDirectionCtl(MOTOR_TURN_RIGHT);    
        Unit_MotorRearDirectionCtl(MOTOR_TURN_RIGHT);
    }
    else if(u8nuWirelessCmd == 'a') /*TurnLeft*/
    {
        Unit_MotorFrontDirectionCtl(MOTOR_TURN_LEFT);    
        Unit_MotorRearDirectionCtl(MOTOR_TURN_LEFT);
    }
    else if(u8nuWirelessCmd == 'x') /*Reverse*/
    {
        Unit_MotorFrontDirectionCtl(MOTOR_REVERSE);    
        Unit_MotorRearDirectionCtl(MOTOR_REVERSE);
    }    
    else if(u8nuWirelessCmd == 's') /*Stop*/
    {
        Unit_MotorFrontDirectionCtl(MOTOR_STOP);    
        Unit_MotorRearDirectionCtl(MOTOR_STOP);
    }
    else
    {
        /*No Code*/
    }  

    /*Setting Duty*/ 
    DrvGtmPwmTest(fDuty,fDuty,fDuty,fDuty);
}

코드가 정리정돈이 안되었지만, 간단하게 설명 드리면

1) 먼저 양쪽 장애물을 탐지하여 좌회전, 우회전을 수행합니다.

    if(pstSensorAdcRaw->InfRaySense1_Raw >= 2200u || pstSensorAdcRaw->InfRaySense2_Raw >= 2200u)
    {
        if(ucFirstDetectionFlag == 0u)
        {
            if(pstSensorAdcRaw->InfRaySense1_Raw > pstSensorAdcRaw->InfRaySense2_Raw) 
            {
                u8nuWirelessCmd = 'a'; /*TurnLeft*/
            }
            else
            {
                u8nuWirelessCmd = 'd'; /*TurnRight*/
            }
            ucFirstDetectionFlag = 1u;
            ucWarningFlag = 1u;
        }
    }

2) 그런데 만일 양쪽 장애물이 탐지되지 않았지만 전방에 장애물이 탐지되어

자동차와 거리가 5cm 이하기 된다면

그때 이를 확인하고 후진을 수행하도록 합니다.

    else if(fUltraDistance < 5.0f) /*Distance < 5Cm*/
    {
        if(ucFirstUltraDetectionFlag == 0u)
        {
            u8nuWirelessCmd = 'x'; /*Reverse*/
            ucFirstUltraDetectionFlag = 1u;
            ucWarningUltraFlag = 1u;
        }
    }

3) 그리고, 장애물과의 거리가 떨어졌다면

다시 직진을 통해서 앞으로 나아갑니다.

    else
    {    
        if(ucWarningFlag == 1u)
        {
            gu32nuAscRxData = 'w';
            ucWarningFlag = 0u;
            ucFirstDetectionFlag = 0u;
        }

        if(ucWarningUltraFlag == 1u)
        {
            gu32nuAscRxData = 'w';
            ucWarningUltraFlag = 0u;
            ucFirstUltraDetectionFlag = 0u;
        }
        
        u8nuWirelessCmd = (uint8_t)gu32nuAscRxData;
    }

이렇게 설계를 하였을때,

아래와 같이 동작을 하는 것을 확인할수 있습니다.

 

이제 passive Buzzer를 자동차에 한번 달아보면 좋을 것 같습니다.