아두이노에서 이미지 센서를 이용한 색상 트래킹 - 팬틸트 구현 Pan Tilt

Pixy 영상 센서 개봉기를 최근 올렸었는데요. 그 때는 그냥 Pixy 모니터링 프로그램으로 테스트를 한 번 해보았었죠. 오늘은 아두이노에서 영상 센서의 값을 받아오는 것과 팬틸트를 구현하는 것을 해볼려고 합니다.

저렇게만 케이블을 연결하고서 먼저 카메라 앞에 있는 녹색을 인식시키도록 할겁니다.

영상을 보면 알 수 있습니다. 버튼을 길게 눌러서 녹색등이 켜졌을때, 녹색을 가까이 가져가고, LED가 진한 녹색일때, 한 번더 버튼을 누르면 됩니다.^^ 그러면 일단 녹색 인식 교육은 끝~

아두이노와 연결시켰습니다. 뭐.. Pixy 보드는 두 개의 서보모터를 연결할 수 있는 단자를 이미 가지고 있어서 바로 팬틸트 구현이 가능합니다. 그러나~ 전 뭐 제가 직접 해보고 싶었습니다.ㅋㅋㅋ

예전에 구해놓은 모터 쉴드도 있어서 서보 모터 두 개를 그저 편하게 연결했습니다. 그리고 Pixy와 아두이노는 ISP 단자를 통해 연결됩니다.

아무리 봐도 저 틸트(tilt)를 구현하는 부분은 참 막 만드는 아이디어인데 저렴한 것 같기도하고.. 또 한 편으로 좋은 방식인듯도 합니다.ㅠㅠ.

뭐 아무튼 저렇게 연결했습니다. Pixy의 아두이노 설치 페이지를 가면, 아두이노용 라이브러리를 받을 수 있습니다. 요즘 아두이노는 zip 파일 형태의 라이브러리르 바로 인식하니 참 편합니다.^^

그리고 Pixy의 Hello World를 실행해보고, 소스코드도 보면 라이브러리 사용법 정도는 바로 알 수 있습니다.

그게 실행된 화면입니다. 아까 녹색이 네번째라서 sig는 4라고 표기가 되구요. x, y 좌표와 검출된 사각형 영역의 높이와 너비가 반환됩니다. 딱~ 알면되는 정보 딱 그만큼을 주네요^^

#include <SPI.h>  
#include <Pixy.h>
#include <Servo.h>

Pixy pixy;
Servo pan_servo, tilt_servo;

float target_pan, target_tilt, value_pan, value_tilt;
float alpha = 0.1;

이제 헬로월드 예제를 바탕으로 저도 팬틸트를 구현하려고 합니다. ㅋㅋ 먼저, 필요 라이브러리를 include하고, pixy를 준비하고, 코드에서 필요한 정보를 위해 몇몇 변수를 선언하고, 또 초기화도 했습니다.

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.print("Starting...\n");

  pan_servo.attach(6);
  tilt_servo.attach(5);
  pixy.init();

  delay(1000);

  target_pan = 90.;
  target_tilt = 50.;

  value_pan = target_pan;
  value_tilt = target_tilt;
  
  pan_servo.write(target_pan);
  tilt_servo.write(target_tilt);

  delay(1000);
}

setup() 함수에서는 서보모터 두 개를 각각 pan, tilt로 해서 지정해주고, 초기각도도 지정해주었습니다. pixy 카메라는 여기서 init()으로 초기화를 합니다.

void loop() {   
  static int i = 0;
  uint16_t blocks;

  blocks = pixy.getBlocks();

loop() 함수의 초반에는 별거 없이 pixy에서 검출된 영역을 가져옵니다. getBlocks() 함수를 사용하는 거구요. 그리고 검출된 결과가 있으면 blocks 변수에 검출된 내용이 저장됩니다.

  if (blocks) {
    i++;

    if (i%2==0) {
      int j=0;
      target_pan = pixy.blocks[j].x + pixy.blocks[j].width/2;
      target_pan = map(target_pan, 0, 300, 150, 30);

      target_tilt = pixy.blocks[j].y + pixy.blocks[j].height/2;
      target_tilt = map(target_tilt, 0, 200, 10, 100);

너무 자주 검출 내용을 가져다 보면 문제가 있다고 합니다. 시리얼 통신으로 데이터를 보내지 않아서 Hello World보다 좀 더 줄였습니다. 초당 50번 결과가 나오는데, 두 번 걸러 한 번만 그 결과를 사용하도록 합니다. 그리고 위 두 개의 같은 종류의 코드는 아두이노의 map 함수를 이용해서 현재 검출된 좌표를 서보모터에 보낼 지령으로 환산하는 코드가 같이 있습니다. 서보모터는 제어 코드가 들어갈게 없어서 저렇게 방향과 크기를 고려해서 매핑만 잘해주면 됩니다.

      value_pan = alpha * target_pan + (1-alpha) * value_pan;
      value_tilt = alpha * target_tilt + (1-alpha) * value_tilt;
      
      pan_servo.write(value_pan);
      tilt_servo.write(value_tilt);
    }    
  }
}

그래도. 바로 그 결과를 서보모터 지령으로 보내면, 서보모터가 엄청 떨게 됩니다. 덜덜덜~~~ 그래서 뭐 자세한 차단주파수의 개념을 쓸건 아니니 정확간 간격의 시간도 필요없이 그저 개념상 1차 저역통과필터(LPF)를 적용했습니다. 거기서 사용한 것이 alpha라는 변수로, 0에 가까울 수록 둔감하고, 느려지고, 1에 가까울 수록 빠르고 시끄럽(^^)습니다.^^

#include <SPI.h>  
#include <Pixy.h>
#include <Servo.h>

Pixy pixy;
Servo pan_servo, tilt_servo;

float target_pan, target_tilt, value_pan, value_tilt;
float alpha = 0.1;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.print("Starting...\n");

  pan_servo.attach(6);
  tilt_servo.attach(5);
  pixy.init();

  delay(1000);

  target_pan = 90.;
  target_tilt = 50.;

  value_pan = target_pan;
  value_tilt = target_tilt;
  
  pan_servo.write(target_pan);
  tilt_servo.write(target_tilt);

  delay(1000);
}

void loop() {   
  static int i = 0;
  uint16_t blocks;
  char buf[32]; 

  blocks = pixy.getBlocks();

  if (blocks) {
    i++;

    if (i%2==0) {
      int j=0;
      target_pan = pixy.blocks[j].x + pixy.blocks[j].width/2;
      target_pan = map(target_pan, 0, 300, 150, 30);

      target_tilt = pixy.blocks[j].y + pixy.blocks[j].height/2;
      target_tilt = map(target_tilt, 0, 200, 10, 100);

      value_pan = alpha * target_pan + (1-alpha) * value_pan;
      value_tilt = alpha * target_tilt + (1-alpha) * value_tilt;
      
      pan_servo.write(value_pan);
      tilt_servo.write(value_tilt);
    }    
  }
}

이제 영상을 보면 됩니다. 크크크크...

뭐.. 혼자 구동하고 영상 찍고해야해서 좀 허접하지만. 뭐 아무튼.. 나쁘진 않다~로 마무리 하겠습니다. ㅋㅋㅋ