Pixy2 cam 아두이노로 색 물체 따라 움직이는 모바일 로봇 만들기!

 

이번 포스팅에서는 모바일 로봇의 모터와 연동하여

색 물체를 따라 움직여 보겠습니다.

 

우선 색을 먼저 학습시키는 법은 아래 포스팅을 확인해주세요.

Pixy2 색 인식하기▽

Pixy2 cam 물체 인식, 색 학습 하는 법

 

 

Pixy2 공식사이트에서는 Zumo 로봇을 이용하였는데요,

예제 코드도 Zumo 로봇 코딩한 것을 활용하였습니다. 

 

Zumo 모바일 로봇▽

https://www.pololu.com/product/2509

가이드 https://docs.pixycam.com/wiki/doku.php?id=wiki:v2:pixy_zumo_howto

 

---

 

Pixy2 cam 모바일 로봇 장착

---

 

저는 아두이노 나노 보드를 사용하는

젤리비 마카롱 로봇을 사용하였습니다.^^

로봇젤리비 마카롱▽

https://smartstore.naver.com/robotnmore/products/4410641703

 

모바일 로봇에 Pixy를 장착하여 선을 연결해주었습니다.

 

Pixy2 cam 모바일 로봇에 장착

선 연결

아두이노 나노 젤리비 보드와 연결하였고

Pix2 cam에 따로 들어가는 전압이 6V-10V 필요하여 선을 따로 빼 7.2V 배터리에 연결하였습니다.

 

---

 

Pixy2 아두이노 코딩

---

 

 

예제 코드 Pixy2 > ccc_zumo_chase가 있습니다.

이 코드를 활용하였습니다.

#include <Pixy2.h>
#include <PIDLoop.h>
#include <ZumoMotors.h>

// this limits how fast Zumo travels forward (400 is max possible for Zumo)
#define MAX_TRANSLATE_VELOCITY  250

Pixy2 pixy;
ZumoMotors motors;

PIDLoop panLoop(350, 0, 600, true);
PIDLoop tiltLoop(500, 0, 700, true);
PIDLoop rotateLoop(300, 600, 300, false);
PIDLoop translateLoop(400, 800, 300, false);

void setup()
{
  Serial.begin(115200);
  Serial.print("Starting...\n");
  
  // initialize motor objects
  motors.setLeftSpeed(0);
  motors.setRightSpeed(0);
  
  // need to initialize pixy object
  pixy.init();
  // user color connected components program
  pixy.changeProg("color_connected_components");
}

// Take the biggest block (blocks[0]) that's been around for at least 30 frames (1/2 second)
// and return its index, otherwise return -1
int16_t acquireBlock()
{
  if (pixy.ccc.numBlocks && pixy.ccc.blocks[0].m_age>30)
    return pixy.ccc.blocks[0].m_index;

  return -1;
}

// Find the block with the given index.  In other words, find the same object in the current
// frame -- not the biggest object, but he object we've locked onto in acquireBlock()
// If it's not in the current frame, return NULL
Block *trackBlock(uint8_t index)
{
  uint8_t i;

  for (i=0; i<pixy.ccc.numBlocks; i++)
  {
    if (index==pixy.ccc.blocks[i].m_index)
      return &pixy.ccc.blocks[i];
  }

  return NULL;
}


void loop()
{  
  static int16_t index = -1;
  int32_t panOffset, tiltOffset, headingOffset, left, right;
  Block *block=NULL;
  
  pixy.ccc.getBlocks();

  if (index==-1) // search....
  {
    Serial.println("Searching for block...");
    index = acquireBlock();
    if (index>=0)
      Serial.println("Found block!");
 }
  // If we've found a block, find it, track it
  if (index>=0)
     block = trackBlock(index);

  // If we're able to track it, move motors
  if (block)
  {
    // calculate pan and tilt errors
    panOffset = (int32_t)pixy.frameWidth/2 - (int32_t)block->m_x;
    tiltOffset = (int32_t)block->m_y - (int32_t)pixy.frameHeight/2;  

    // calculate how to move pan and tilt servos
    panLoop.update(panOffset);
    tiltLoop.update(tiltOffset);

    // move servos
    pixy.setServos(panLoop.m_command, tiltLoop.m_command);

    // calculate translate and rotate errors
    panOffset += panLoop.m_command - PIXY_RCS_CENTER_POS;
    tiltOffset += tiltLoop.m_command - PIXY_RCS_CENTER_POS - PIXY_RCS_CENTER_POS/2 + PIXY_RCS_CENTER_POS/8;

    rotateLoop.update(panOffset);
    translateLoop.update(-tiltOffset);

    // keep translation velocity below maximum
    if (translateLoop.m_command>MAX_TRANSLATE_VELOCITY)
      translateLoop.m_command = MAX_TRANSLATE_VELOCITY;

    // calculate left and right wheel velocities based on rotation and translation velocities
    left = -rotateLoop.m_command + translateLoop.m_command;
    right = rotateLoop.m_command + translateLoop.m_command;

    // set wheel velocities
    motors.setLeftSpeed(left);
    motors.setRightSpeed(right);

    // print the block we're tracking -- wait until end of loop to reduce latency
    block->print();
  }  
  else // no object detected, stop motors, go into search state
  {
    rotateLoop.reset();
    translateLoop.reset();
    motors.setLeftSpeed(0);
    motors.setRightSpeed(0);
    index = -1; // set search state
  }
}

 

 

Zumo 로봇을 사용하지 않기 때문에 ZumoMotors 대신 모터제어 함수를 따로 만들어주었습니다.

 

#define RIGHT_WHEEL 6
#define LEFT_WHEEL 5
#define RIGHT_DIR 8
#define LEFT_DIR 7

void DriveInit()
{
  pinMode(RIGHT_DIR, OUTPUT);
  pinMode(LEFT_DIR, OUTPUT);
  pinMode(RIGHT_WHEEL, OUTPUT);
  pinMode(LEFT_WHEEL, OUTPUT);
}
void Stop()
{
  analogWrite(LEFT_WHEEL, 0);
  analogWrite(RIGHT_WHEEL, 0);
}
void Drive(int32_t left, int32_t right) {

  digitalWrite(LEFT_DIR, (left > 0) ? 1 : 0);
  digitalWrite(RIGHT_DIR, (right > 0) ? 0 : 1);
  analogWrite(LEFT_WHEEL, abs(left));
  analogWrite(RIGHT_WHEEL, abs(right));

}

 

  pixy.ccc.getBlocks();

구문을 이용하여 서치 후

 

// If we're able to track it, move motors
  if (block)
  {

    // calculate pan and tilt errors
    panOffset = (int32_t)pixy.frameWidth / 2 - (int32_t)block->m_x;
    tiltOffset = (int32_t)block->m_y - (int32_t)pixy.frameHeight / 2;


    // calculate how to move pan and tilt servos
    panLoop.update(panOffset);
    tiltLoop.update(tiltOffset);

    // move servos
    pixy.setServos(panLoop.m_command, tiltLoop.m_command);

    // calculate translate and rotate errors
    panOffset += panLoop.m_command - PIXY_RCS_CENTER_POS;
    tiltOffset += tiltLoop.m_command - PIXY_RCS_CENTER_POS - PIXY_RCS_CENTER_POS / 2 + PIXY_RCS_CENTER_POS / 8;

    rotateLoop.update(panOffset);
    translateLoop.update(-tiltOffset);

    // keep translation velocity below maximum
    if (translateLoop.m_command > MAX_TRANSLATE_VELOCITY)
      translateLoop.m_command = MAX_TRANSLATE_VELOCITY;

    // calculate left and right wheel velocities based on rotation and translation velocities
    left = -rotateLoop.m_command + translateLoop.m_command;
    right = rotateLoop.m_command + translateLoop.m_command;


    left = left / 4; 
    right = right / 4; 
    if (left > 120) left = 120;
    if (right > 120) right = 120;


    if (mm_signature == 2) { 
      if (block->m_height > 150 && block->m_height < 200) {
        Stop();
      }
      else if (block->m_height >= 200) {
        Drive(-70, -70);
        delay(500);

      }
      else {
        Drive(left, right);
      }
    }
    else if (mm_signature == 4) { 
      if (block->m_height > 70 && block->m_height < 122) {
        Stop();
      }
      else if (block->m_height >= 122) {
        Drive(-70, -70);
        delay(500);
      }
      else {
        Drive(left, right);
      }
    }


  }
  else // no object detected, stop motors, go into search state
  {
    rotateLoop.reset();
    translateLoop.reset();
    Stop();
    index = -1; // set search state
  }

 

 

 

left = left / 4; 
right = right / 4; 
if (left > 120) left = 120;
if (right > 120) right = 120;

계산된 왼쪽, 오른쪽 모터 값을 제가가진 모바일 로봇의 속도와 맞게 조정해주었습니다.

 

Block *trackBlock(uint8_t index)
{
  uint8_t i;

  for (i = 0; i < pixy.ccc.numBlocks; i++)
  {
    if (index == pixy.ccc.blocks[i].m_index) {
      mm_signature = pixy.ccc.blocks[i].m_signature;
      return &pixy.ccc.blocks[i];
    }
  }

  return NULL;
}

 처음 색을 인식하면 그 색의 번호를 mm_signature에 저장하였습니다.

loop문에서 block->m_signature 을 이용하면 번호가 바로 나오지만, 생각 외로 진행과정에서 0으로 변하여 처음부터 저장하였습니다..

 

 

if (mm_signature == 2) { 
      if (block->m_height > 150 && block->m_height < 200) {
        Stop();
      }
      else if (block->m_height >= 200) {
        Drive(-70, -70);
        delay(500);

      }
      else {
        Drive(left, right);
      }
    }
 else if (mm_signature == 4) { 
      if (block->m_height > 70 && block->m_height < 122) {
        Stop();
      }
      else if (block->m_height >= 122) {
        Drive(-70, -70);
        delay(500);
      }
      else {
        Drive(left, right);
      }
    }

 

색 번호에 따라 다르게 코딩하여 움직여주는 모습입니다.

색 번호에 따라 한것은 크기(m_height 멤버변수를 이용하여 높이를 사용했음)따라서

가까이오면 멈추거나 뒤로 가기 위해서 입니다.

 

제가 가진 모바일 로봇에 맞춰 예제 코드를 이것저것 고친 후

아두이노로 Pixy2 cam과 모터 연동에 성공했습니다. ^^

 

만약 물체 체이스가 잘 안된다면 PixyMon으로

색을 잘 인식할 수 있도록 튜닝해주세요! 맨 위의 링크된 포스팅에서 참고해주세요.

 

 

 

 

 

▼활용 영상

https://www.youtube.com/watch?v=nFo0f1bkdCM&t=9s

 

 

아두이노 API 멤버함수, 멤버 변수들은 아래 링크를 확인해주세요.

 

Pixy2 아두이노 코딩▽

Pixy2 cam 아두이노 코딩하기 ― 아두이노로 물체 따라서 서보모터 움직이기