Login▼/회원가입BASIC4MCU | 질문게시판 | 라이다센서를 통한 장애물 인식
페이지 정보
작성자 cold 작성일2021-08-15 16:31 조회1,769회 댓글0건첨부파일
본문
안녕하세요
픽시캠을 통해서 색깔인식을 하여 사람을 따라오는 링거대를 만들고 있습니다.
그런데 라이다 센서를 통해 장애물을 인식하여 장애물을 만나면 멈ㅊ추게 하는 코드를 작성하였는데
설정해준 감지범위(300mm)내에 무언가가 들어오면 바로 멈추게끔 코드를 설정했다 생각했는데 자꾸만 5~10초뒤에 멈추게 됩니다.
무엇이 잘 못된 것인지 알고 싶습니다.
아래는 저희가 작성한 코드 입니다.
#define CHECK_PERIOD 500
#include <YDLidar.h>
#include <Pixy2.h>
// This is the main Pixy object
Pixy2 pixy;
float angle_distance[360];
YDLidar lidar;
bool isScanning = true;
unsigned long prev_millis;
void setup()
{
pixy.init();
pinMode(2, OUTPUT); // Motor 1 방향 설정 핀
pinMode(4, OUTPUT); // Motor 2 방향 설정 핀
pinMode(6, OUTPUT); // Motor 3 방향 설정 핀
pinMode(8, OUTPUT); // Motor 4 방향 설정 핀
pinMode(17, OUTPUT);
digitalWrite(17, LOW);
delay(1000);
analogWrite(3, 0); // Motor 1 속도조절 (0~255)
analogWrite(5, 0); // Motor 2 속도조절 (0~255)
analogWrite(7, 0); // Motor 3 방향설정 (0~255)
analogWrite(9, 0); // Motor 4 방향설정 (0~255)
Serial.begin(115200); // 시리얼 모니터와의 통신
Serial.println("LIDAR START");
lidar.begin(Serial2, 115200); // 라이다와는 Serial2 와 통신
Serial.println("#####START#####");
isScanning = true;
delay(1000);
digitalWrite(17, HIGH);
}
void loop()
{
int j;
// grab blocks!
pixy.ccc.getBlocks();
float distance = lidar.getCurrentScanPoint().distance; //distance value in mm unit
float angle = lidar.getCurrentScanPoint().angle; //anglue value in degree
byte quality = lidar.getCurrentScanPoint().quality; //quality of the current measurement
bool startBit = lidar.getCurrentScanPoint().startBit;
if (pixy.ccc.numBlocks)
{
Serial.print("Detected ");
Serial.println(pixy.ccc.numBlocks);
for (j=0; j<pixy.ccc.numBlocks; j++)
{
if(isScanning)
{
if(lidar.waitScanDot() == RESULT_OK)
{
if(distance > 300)//장애물x
{
if (pixy.ccc.blocks[j].m_x < 80) //왼쪽에 있을 때
{
digitalWrite(2, HIGH);
analogWrite(3, 0);
digitalWrite(4, LOW);
analogWrite(5, 180);
digitalWrite(6, HIGH);
analogWrite(7, 150);
digitalWrite(8, HIGH);
analogWrite(9, 0);
delay(150);
}
else if (pixy.ccc.blocks[j].m_x > 200)//오른쪽
{
digitalWrite(2, LOW);
analogWrite(3, 150);
digitalWrite(4, HIGH);
analogWrite(5, 0);
digitalWrite(6, HIGH);
analogWrite(7, 0);
digitalWrite(8, HIGH);
analogWrite(9, 150);
delay(150);
}
else //중간
{
if(pixy.ccc.blocks[j].m_width<50)//멀 때
{
digitalWrite(2, LOW);
analogWrite(3, 180);
digitalWrite(4, LOW);
analogWrite(5, 195);
digitalWrite(6, LOW);
analogWrite(7, 180);
digitalWrite(8, LOW);
analogWrite(9, 165);
delay(500);
}
else if(pixy.ccc.blocks[j].m_width>100 && pixy.ccc.blocks[j].m_width<150)//가까움
{
digitalWrite(2, LOW);
analogWrite(3, 110);
digitalWrite(4, LOW);
analogWrite(5, 135);
digitalWrite(6, LOW);
analogWrite(7, 140);
digitalWrite(8, LOW);
analogWrite(9, 95);
delay(500);
}
else if(pixy.ccc.blocks[j].m_width>150)//넘 가까우ㅁ
{
digitalWrite(2, HIGH);
analogWrite(3, 100);
digitalWrite(4, HIGH);
analogWrite(5, 135);
digitalWrite(6, HIGH);
analogWrite(7, 130);
digitalWrite(8, HIGH);
analogWrite(9, 95);
delay(300);
}
else//중간거리
{
digitalWrite(2, LOW);
analogWrite(3, 160);
digitalWrite(4, LOW);
analogWrite(5, 175);
digitalWrite(6, LOW);
analogWrite(7, 160);
digitalWrite(8, LOW);
analogWrite(9, 145);
delay(500);
}
}
}
else if(distance <= 300)
{
digitalWrite(2, LOW); // Motor 1 방향설정 (HIGH 또는 LOW)
analogWrite(3, 0); // Motor 1 속도조절 (0~255)
digitalWrite(4, LOW); // Motor 2 방향설정 (HIGH 또는 LOW)
analogWrite(5, 0); // Motor 2 속도조절 (0~255)
digitalWrite(6, LOW); // Motor 3 방향설정 (HIGH 또는 LOW)
analogWrite(7, 0); // Motor 3 방향설정 (0~255)
digitalWrite(8, LOW); // Motor 4 방향설정 (HIGH 또는 LOW)
analogWrite(9, 0); // Motor 4 방향설정 (0~255)
}
}
}
}
}
else
{
digitalWrite(2, LOW); // Motor 1 방향설정 (HIGH 또는 LOW)
analogWrite(3, 0); // Motor 1 속도조절 (0~255)
digitalWrite(4, LOW); // Motor 2 방향설정 (HIGH 또는 LOW)
analogWrite(5, 0); // Motor 2 속도조절 (0~255)
digitalWrite(6, LOW); // Motor 3 방향설정 (HIGH 또는 LOW)
analogWrite(7, 0); // Motor 3 방향설정 (0~255)
digitalWrite(8, LOW); // Motor 4 방향설정 (HIGH 또는 LOW)
analogWrite(9, 0); // Motor 4 방향설정 (0~255)
}
}
그리고 첨부파일 저희가 참고한 코드입니다. 이 코드를 바탕으로 코드를 변경해서 짜보았습니다.
- BASIC4MCU 작성글 SNS에 공유하기
댓글 0
조회수 1,769등록된 댓글이 없습니다.