이번 포스팅에서는 모바일 로봇의 모터와 연동하여
색 물체를 따라 움직여 보겠습니다.
우선 색을 먼저 학습시키는 법은 아래 포스팅을 확인해주세요.
Pixy2 색 인식하기▽
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를 장착하여 선을 연결해주었습니다.
아두이노 나노 젤리비 보드와 연결하였고
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' 카테고리의 다른 글
Pixy2 color code 사용하기 (0) | 2020.05.13 |
---|---|
Pixy2 cam 아두이노와 통신하기 (7) | 2020.05.13 |
Pixy2 cam 아두이노 코딩하기 ― 물체 따라서 서보모터 움직이기 (19) | 2020.01.09 |
Pixy2 cam 물체 인식, 색 학습 하는 법 (4) | 2020.01.09 |