PSD를 사용한 측정 및 거리 출력 실습

1. [심화 실습 예제] PSD를 사용한 측정 및 거리 출력 실습 예시 설명

 

블랙 베리는 전방 좌측과 전방 우측, 측방 좌측과 측방 우측에 각각 1개씩 총 4개의 PSD 센서를 가지고 있으며, 각 센서의 핀번호는 A0, A2, A1, A3입니다.

센서별 핀 번호, 센서별/관측회차별 아날로그값, 센서별 아날로그 관측값 평균 등은 배열을 이용하면 간단히 표현합니다.

변수에 대해 초기화한 후 센서별로 관측을 5회 실시한 후 관측값의 평균을 구합니다. 관측값 평균을 입력하면 실제 거리를 구하는 함수를 이용해 실제 거리를 추정한 후, 시리얼 모니터로 0.5초에 한 번씩 출력하도록 합니다.

장애물까지의 거리(6cm 미만 제외)를 변화시키며 관측한 아날로그값(데이터를 모으기 위해 네 개의 센서에서 측정된 값을 시리얼 모니터에 출력하는 프로그램이 필요할 수 있음)을 이용하면 각 센서별로 아날로그 출력값에 대한 실제 거리값 그래프를 그릴 수 있고, 그래프로부터 변인간 관계를 파악하여 함수화 할 수 있습니다. 엑셀과 같은 스프레드시트 프로그램을 이용해 분산형 그래프를 그린 후, 그래프로부터 추세선을 그리고 수식을 차트에 표시하도록 하는 방식으로 함수를 추정합니다. 추세선을 그리는 함수는 일차 함수, 다항식, 지수함수 등을 사용할 수 있습니다.

 

아래의 그림처럼 엑셀시트에 측정한 거리 데이터를 입력한 후 [추천차트] 아이콘을 선택하고 확인 버튼을 누릅니다.

 

 

 

아래의 그림처럼 차트가 추가됩니다.

 

 

 

차트의 추세선을 선택하고 마우스 오른쪽을 클릭한 후에 "추세선 추가" 메뉴를 선택합니다.

 

 

 

오른쪽에 추세선 서식이 출력되고 추세선 옵션에서 "거듭제곱"을 클릭하신 후에 "수식을 차트에 표시"를 체크해 주시면 차트 내부의 아래의 그림과 같이 추세선을 반영한 수식이 출력됩니다.

 

 

 

 

• 거리 함수(y=f(x))를 구할 때에는 x는 아날로그 관측값, y는 실제 거리가 되어야 하므로 추세선을 그리기 위해 그래프를 그릴 때 유의해야 합니다. 그래프는 분산형 그래프를 사용합니다.
• 추세선을 함수화 할 때 가급적 실제 데이터와 유사한 함수를 찾아야 합니다. 실습에서 사용하는 PSD 센서의 경우 거듭제곱 형태의 함수로 근사할 것을 추천합니다.
• 시리얼 출력은 너무 자주하면 관측값을 읽기 어렵고, 출력 회수가 작을 때에는 거리 변화를 신속히 파악하기 어렵기 때문에 적절한 시간 간격을 설정해야 합니다.

 

아래는 PSD를 이용한 거리측정 실습코드입니다.

 

////////////// [실습] 블랙 베리의 거리 감각은 진화 중.
////////////// 4개의 PSD 센서에서 측정한 아날로그 측정값의 5회 평균을 이용해,장애물
////////////// 까지의 실제 거리([cm])를 구한 후 0.5초 간격으로 Serial 모니터로 출력 

//////////////  거리 측정 관련 선언
// PIN_NUM는 전방 좌측, 전방 우측, 측방 좌측, 측방 우측
// PSD 센서의 연결핀 정보를 담고 있는 배열
const int PIN_NUM[] = { A0, A2, A1, A3 }; 

//////////////  메인 프로그램

void setup() {
  Serial.begin( 115200 ); // 시리얼 모니터 초기화 (baudrate)
}

void loop() {
  // 각 센서의 센서 값, 평균 값, 거리 값을 저장할 배열을 생성 및 초기화
  static int analogReadValue[5][4] = {0,}; // 배열을 0으로 채움
  static float analogReadValueAverage[4] = {0.0,};
  static float distanceValue[4] = {0.0,};

  for (int n=0; n<5; n++)  // 평균낼 횟수만큼 반복
  {
    for (int i=0; i<4; i++)  // 4개의 센서 값을 각각 읽어옴.
    {
      analogReadValue[n][i] = analogRead(PIN_NUM[i]); // PIN_NUM에 해당하는센서
                                                      // 값을 읽어옴.
    }
  }

  for (int i=0; i<4; i++)  //각 센서별 n회(여기선 5회) 측정값에 대해 평균을 냄.
  {
    analogReadValueAverage[i] = (analogReadValue[0][i]
                                 + analogReadValue[1][i]
                                 + analogReadValue[2][i]
                                 + analogReadValue[3][i]
                                 + analogReadValue[4][i])/5;
                                 
    // 센서별 인덱스와 아날로그 측정값 평균을 입력으로 psd2Distance 함수에서 처리하여,
    // 실제 거리([cm])를 distanceValue 배열에 저장
    distanceValue[i] = psd2Distance(i, analogReadValueAverage[i]);
  }

  // 아날로그 측정값 평균 출력( "[Analog Value(fl, fr, sl, sr)]: (a, b, c, d)" )
  Serial.print("[Analog Value(fl, fr, sl, sr)]: (");
  for (int i=0; i<4; i++)
  {
    Serial.print(analogReadValueAverage[i]);
    if (i<3){
      Serial.print(", ");
    }
    else{
      Serial.println(") ");
    }
  }
 
  // 아날로그 측정값 평균을 이용한 실제 거리값 출력
  // ( "[Distance(fl, fr, sl, sr)]: (a cm, b cm, c cm, d cm)" )
  Serial.print( "[Distance(fl, fr, sl, sr)] : (" );
  for (int i=0; i<4; i++)
  { 
    Serial.print( distanceValue[i], 1 );  //소수점 첫째자리까지 출력 
    if (i<3){ 
      Serial.print( " cm, " );
    }
    else {
      Serial.println( "cm )" );
    }
  }
  delay(500); 
}


// PSD 센서 인덱스와 아날로그 측정값 평균을 입력으로 장애물까지의
// 실제 거리([cm])를 출력하는 함수
// 각 센서의 실제 거리 값은 1cm 단위로 거리를 변화시켜가며 측정한 아날로그 신호를 이용해,
// 아날로그 신호에 대한 거리값 그래프를 그린후 거듭제곱 형태의 추세 함수를 추정하여 구함.
float psd2Distance(int x, float d) 
{
  float distance;                     // 아날로그 관측값 (5회) 평균을 이용해
                                      // 산출한 실제 거리([cm])
  
  if (x==0){
    distance = 13768*pow(d, -1.183);   // 전방 좌측 거리 추세 함수
  }
  else if (x==1){
    distance = 15911*pow(d, -1.2);     // 전방 우측 거리 추세 함수
  }
  else if (x==2){
    distance = 37406*pow(d, -1.333);    // 측방 좌측 거리 추세 함수
  }
  else if (x==3){
    distance = 44932*pow(d, -1.361);     // 측방 우측 거리 추세 함수
  }
  else {
    distance = 0.0;
  }
  return distance;
}

 

 

 

 

1.4개의 PSD센서를 사용하기 위한 PSD 센서들의 핀번호를 배열에 저장합니다.

 

2. setup에서는 시리얼 모니터에 거리 값을 출력하기 위해 시리얼 통신을 초기화 합니다.

 

loop 에서는 아래의 동작들을 반복합니다.

 

3.각 센서의 센서 값과 평균 값, 거리 값을 저장할 배열을 선언하고 초기화 합니다.

 

4.4번씩 5번 반복하는 이중 for문에서는 4개의 PSD 센서에서 5번씩 센서 값을 읽어 analogReadValue 배열에 저장합니다.

 

 

 

loop 에서는 아래의 동작들을 반복합니다.

5.4번 반복하는 for 문에서 각 PSD 센서별로 저장해 둔 5개의 센서 값의 평균을 계산해 analogReadValueAverage 배열에 저장하고 PSD 센서 값을 거리 값으로 변환해주는 psd2Distance 함수에 현재 계산중인 PSD센서 핀 번호의 PIN_NUM 배열 상의 인덱스와 센서 평균 값을 전달하고 반환된 거리 값을 거리 값 배열에 저장합니다.

 

6.각 PSD 센서에서 측정된 아날로그 값의 평균을 시리얼 모니터에 출력합니다.

 

7.각 PSD 센서에서 측정된 거리 값을 시리얼 모니터에 출력합니다.

 

8. 출력된 값이 너무 빨리 지나가면 값을 파악하기 어려우므로 적절한 시간동안 대기합니다.

 

 

 

PIN_NUM 배열에 저장된 PSD 센서 핀 번호의 인덱스 x와 센서 값 d를 받아 PSD 센서 값을 거리 값으로 변환해주는 함수 psd2Distance는 아래와 같이 동작합니다.

9.반환할 거리 값을 저장하는 distance 변수를 선언합니다.

 

10.PSD 센서에 따라 약간씩 다른 식을 적용하여 거리를 계산하기 위해 x 값에 따라 분기, d 값을 사용해 거리 값을 계산한 후 distance 변수에 저장합니다.
(식은 MobileManipulator-Blackberry-Lab-02-01-PSD-Value-Monitor 를 사용해 거리별로 데이터를 모으고, 엑셀에서 추세선을 사용해 구하였음)

 

11.계산된 거리 값을 반환합니다.

 

 

 

2. SharpIR 사용

먼저 SharpIR 라이브러리를 설치합니다.

툴 -> 라이브러리 관리 메뉴를 선택합니다.

검색창에서 "sharpri"을 입력합니다.

아래의 그림과 같이 관련 라이브러리 리스트가 출력됩니다. 여기에서 "SharpIR"의 "설치" 버튼을 클릭합니다.

 

 

 

 

 

 

설치가 완료되면 라이브러리 매니저를 닫고 라이브러리 테스트 예제를 입력하고 컴파일을 수행합니다.

 

/*
  [비교 실습] 4개의 Sharp(PSD) 센서 거리 측정 비교
  - 방법 1: 기존 코드의 추세함수(psd2Distance) + 아날로그값 5회 평균
  - 방법 2: SharpIR 라이브러리(getDistance) 사용 (센서별 5회 평균)

  출력(0.5초마다):
  [Analog Avg(fl, fr, sl, sr)] : (...)
  [Dist Trend (cm)]            : (... cm)
  [Dist SharpIR (cm)]          : (... cm)

  주의:
  1) SharpIR 라이브러리는 "센서 모델"에 따라 변환이 달라집니다.
     아래 MODEL들은 예시입니다. 본인 센서 모델에 맞게 꼭 바꾸세요.
  2) SharpIR는 cm를 int로 반환하는 경우가 많아 소수점은 안 나올 수 있습니다.
*/

#include <Arduino.h>
#include <SharpIR.h>

// 전방 좌측, 전방 우측, 측방 좌측, 측방 우측
const int PIN_NUM[4] = { A0, A2, A1, A3 };

// SharpIR 객체 4개 (각 센서가 모두 같은 모델이라고 가정)
// 만약 모델이 서로 다르면 센서별로 다르게 지정해야 합니다.
SharpIR irFL(SharpIR::GP2Y0A21YK0F, A0);
SharpIR irFR(SharpIR::GP2Y0A21YK0F, A2);
SharpIR irSL(SharpIR::GP2Y0A21YK0F, A1);
SharpIR irSR(SharpIR::GP2Y0A21YK0F, A3);

// 배열로 다루기 쉽게 포인터 배열 구성
SharpIR* irSensor[4] = { &irFL, &irFR, &irSL, &irSR };

// ===== 방법 1: 첫번째 소스의 추세함수 =====
float psd2Distance(int x, float d)
{
  float distance = 0.0;

  // d가 0이면 pow(d, 음수)에서 문제가 나므로 예외 처리
  if (d <= 0.0) return 0.0;

  if (x == 0) {
    distance = 13768 * pow(d, -1.183);   // 전방 좌측
  }
  else if (x == 1) {
    distance = 15911 * pow(d, -1.2);     // 전방 우측
  }
  else if (x == 2) {
    distance = 37406 * pow(d, -1.333);   // 측방 좌측
  }
  else if (x == 3) {
    distance = 44932 * pow(d, -1.361);   // 측방 우측
  }
  else {
    distance = 0.0;
  }
  return distance;
}

// 아날로그값 5회 평균
float readAnalogAverage5(int pin)
{
  long sum = 0;
  for (int n = 0; n < 5; n++) {
    sum += analogRead(pin);
    delay(2); // 너무 빠르게 연속 측정하는 것을 약간 완화(선택)
  }
  return (float)sum / 5.0;
}

// SharpIR 거리 5회 평균 (getDistance()는 보통 int cm 반환)
float readSharpIRDistanceAverage5(SharpIR* sensor)
{
  long sum = 0;
  for (int n = 0; n < 5; n++) {
    sum += sensor->getDistance(); // cm
    delay(2);
  }
  return (float)sum / 5.0;
}

void setup()
{
  Serial.begin(115200);
}

void loop()
{
  float analogAvg[4] = {0, 0, 0, 0};
  float distTrend[4] = {0, 0, 0, 0};
  float distSharp[4] = {0, 0, 0, 0};

  // 1) 아날로그 평균(5회) 읽기 + 추세함수 거리 계산
  for (int i = 0; i < 4; i++) {
    analogAvg[i] = readAnalogAverage5(PIN_NUM[i]);
    distTrend[i] = psd2Distance(i, analogAvg[i]);
  }

  // 2) SharpIR 거리(5회) 평균
  for (int i = 0; i < 4; i++) {
    distSharp[i] = readSharpIRDistanceAverage5(irSensor[i]);
  }

  // 출력 1) 아날로그 평균
  Serial.print("[Analog Avg(fl, fr, sl, sr)] : (");
  for (int i = 0; i < 4; i++) {
    Serial.print(analogAvg[i], 1);
    if (i < 3) Serial.print(", ");
    else       Serial.println(")");
  }

  // 출력 2) 추세함수 거리
  Serial.print("[Dist Trend (cm)]            : (");
  for (int i = 0; i < 4; i++) {
    Serial.print(distTrend[i], 1);
    if (i < 3) Serial.print(" cm, ");
    else       Serial.println(" cm)");
  }

  // 출력 3) SharpIR 거리
  Serial.print("[Dist SharpIR (cm)]          : (");
  for (int i = 0; i < 4; i++) {
    Serial.print(distSharp[i], 1);
    if (i < 3) Serial.print(" cm, ");
    else       Serial.println(" cm)");
  }

  Serial.println("--------------------------------------------------");

  delay(500);
}

 

 

실행결과에서 블랙배리에서 사용하는 방식과 SharpIR 파이브러리가 제공하는 출력 결과를 비교해보세요. SharpIR의 라이브러리는 교정 값이 고정되어 있지만 강의에서 제안한 방법은 블랙배리 보드에 적합하게 사용자가 추세선 함수를 정의할 수 있기 때문에 거리 측정 정확도를 더 정밀하게 할 수 있습니다.