융합_로보테크 AI 자율주행 로봇 개발자 과정-26/02/27

추가할 것

IR 리스브 센서 + 리모컨으로 전후방 주차(거리탐지로 벽에 너무 가까워지면 정지)

ㄴ 실제 자동차 키로 주차할 때 사용하는 것처럼

 

 

상단: 초음파 센서 3개, 블루투스 송신 모듈, LED3개(RGY),  LCD, 인체 감지센서

하단: 자율주행 로봇+ 리모컨, IR 리시브 센서로 전후방주차

(인터넷에서 찾은 예제와 로봇 그대로, 통신은 여유 있으면)

 

목적: 물류센터 자율주행 로봇에 장착할 보조 안전 모듈 및 데이터 분석 프로그램

자율주행 로봇이 메인이 아님(완성형 사용)

근접 위험 이벤트를 수집 및 분석하는 데이터 레이어 추가

로봇 대 로봇, 로봇 대 사람, 로봇 대 장애물로 위험 상황 나뉨

 

어디서 위험이 반복적으로 발생하는지에 대한 구조적 분석

ㄴ 분석된 데이터로 로봇의 이동 경로, 회피 효율적으로 개선하는데 도움

 

자율주행 로봇 + 리모컨 조작

 

초음파 탐지 x4 + 인체감지센서(LED, LCD, 부저)

 

GUI + SQL

 

BOARD1(사물 인식)
초음파 4개로 센서데이터값가져오기(여기서 LED로 사물에 가까우면 ON 멀어질 수 록 작아지게) -> 사물인식이된다. -> 통신

BOARD2(위험 감지 인식) 
조도 센서, 
온습도 센서, 
Sound 센서 
데이터값 받아오기 -> 적절한 값을 설정해서 임계값에 돌입할시에 통신

=======================================================
컴퓨터에서 값을 받아와준다. + GUI(PyQt5)
=======================================================

BOARD3(외부 GUI역할) 
컴퓨터에서 받은 통신값을 토대로 LCD패널에 나오게 하고, 
부저 모듈까지 연결해서 소리나게끔 , 
LED3개로 위험 상태 표시

BOARD4(추후에)

 

/*  BOARD 1 — 사물 인식 모듈
 *  Arduino Uno R3
 *
 * 최종 핀 배치 (충돌 없음, PWM 확보)
 * ─────────────────────────────────────────────────────
 *  HC-SR04 전방   TRIG=A0  ECHO=A1
 *  HC-SR04 후방   TRIG=A2  ECHO=A3
 *  HC-SR04 좌측   TRIG=2   ECHO=4
 *  HC-SR04 우측   TRIG=7   ECHO=8
 *
 *  RGB LED R      D11  (PWM~)  + 220Ω → LED R 긴다리
 *  RGB LED G      D10  (PWM~)  + 220Ω → LED G 긴다리
 *  RGB LED B      D6   (PWM~)  + 220Ω → LED B 긴다리
 *  RGB LED GND    공통 음극(짧은다리) → GND
 *
 *  HC-06 TX       D13  (SoftSerial TX → HC-06 RXD)
 *  HC-06 RX       D12  (SoftSerial RX ← HC-06 TXD)
 *
 * 아두이노 우노 PWM 핀: 3, 5, 6, 9, 10, 11
 *   → 여기서 사용: 6(B), 10(G), 11(R)  ← 충돌 없음
 *
 * 동작
 *  - 4방향 거리 100ms마다 측정
 *  - 최솟값(가장 가까운 방향) 기준으로 RGB LED 색상+밝기 변화
 *      < 10cm  : 빨강 깜빡임  (DANGER)
 *      10~30cm : 주황          (CLOSE)
 *      30~60cm : 파랑 (거리비례 dim) (MID)
 *      > 60cm  : 초록 희미    (SAFE)
 *  - 500ms마다 + 등급 변화 시 즉시 블루투스 전송
 *    형식: B1,F:23,B:45,L:12,R:67,MIN:12,LV:CLOSE
 *
 * 라이브러리: SoftwareSerial (내장)
 */

#include <SoftwareSerial.h>

//  핀 정의

// 초음파 센서 TRIG / ECHO 배열 [전, 후, 좌, 우]
const int TRIG[4] = {A0, A2,  2,  7};
const int ECHO[4] = {A1, A3,  4,  8};
const char* DIR[4] = {"F", "B", "L", "R"};

// RGB LED
const int RGB_R = 11;  // PWM
const int RGB_G = 10;  // PWM
const int RGB_B =  6;  // PWM

// 블루투스 (SoftwareSerial)
SoftwareSerial btSerial(12, 13);  // RX=12, TX=13

//  거리 임계값 (cm)
const float D_DANGER = 10.0;   // ~10cm   : DANGER
const float D_CLOSE  = 30.0;   // 10~30cm : CLOSE
const float D_MID    = 60.0;   // 30~60cm : MID
                                // 60cm~   : SAFE

//  위험 등급
enum Level { SAFE, MID, CLOSE_LV, DANGER_LV };
Level currentLevel = SAFE;

//  전역 변수
float dist[4]        = {999, 999, 999, 999};
unsigned long lastBt = 0;
const unsigned long BT_MS = 500;

//  함수 선언
float  measureDist(int trig, int echo);
Level  classify(float d);
void   setRGB(int r, int g, int b);
void   updateRGB(float d);
void   sendBT(float minD, Level lv);
const char* lvStr(Level lv);

//  setup()
void setup() {
  Serial.begin(9600);
  btSerial.begin(9600);

  for (int i = 0; i < 4; i++) {
    pinMode(TRIG[i], OUTPUT);
    pinMode(ECHO[i], INPUT);
  }
  pinMode(RGB_R, OUTPUT);
  pinMode(RGB_G, OUTPUT);
  pinMode(RGB_B, OUTPUT);

  // 시작 점등 (R→G→B 순서로 확인)
  setRGB(255, 0, 0); delay(300);
  setRGB(0, 255, 0); delay(300);
  setRGB(0, 0, 255); delay(300);
  setRGB(0, 0, 0);

  Serial.println("[BOARD1] Ready");
  btSerial.println("B1,READY");
}

//  loop()
void loop() {
  unsigned long now = millis();

  // 4방향 거리 측정
  for (int i = 0; i < 4; i++) {
    dist[i] = measureDist(TRIG[i], ECHO[i]);
  }

  // 최솟값 (가장 가까운 방향)
  float minD = dist[0];
  for (int i = 1; i < 4; i++) {
    if (dist[i] < minD) minD = dist[i];
  }

  // 등급 판별
  Level prev = currentLevel;
  currentLevel = classify(minD);

  // RGB 업데이트
  updateRGB(minD);

  // 등급 변화 시 즉시 전송
  if (currentLevel != prev) {
    sendBT(minD, currentLevel);
    lastBt = now;
  }

  // 정기 전송 500ms
  if (now - lastBt >= BT_MS) {
    lastBt = now;
    sendBT(minD, currentLevel);
  }

  // 시리얼 디버그
  for (int i = 0; i < 4; i++) {
    Serial.print(DIR[i]); Serial.print(":"); Serial.print((int)dist[i]); Serial.print(" ");
  }
  Serial.print("LV:"); Serial.println(lvStr(currentLevel));

  delay(100);
}

//  초음파 거리 측정 (cm)
//  범위 초과 시 999 반환
float measureDist(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  long dur = pulseIn(echo, HIGH, 30000UL);
  if (dur == 0) return 999.0;
  return dur * 0.034f / 2.0f;
}

//  거리 → 위험 등급
Level classify(float d) {
  if (d < D_DANGER) return DANGER_LV;
  if (d < D_CLOSE)  return CLOSE_LV;
  if (d < D_MID)    return MID;
  return SAFE;
}

//  RGB LED 직접 설정 (0~255)
void setRGB(int r, int g, int b) {
  analogWrite(RGB_R, r);
  analogWrite(RGB_G, g);
  analogWrite(RGB_B, b);
}

//  거리에 따른 RGB 자동 제어
//
//  <10cm  빨강 점멸 (위험)
//  10~30  주황 (빨강+초록 혼합, 거리비례)
//  30~60  파랑 dim (멀수록 어두움)
//  >60    초록 희미
void updateRGB(float d) {
  if (d >= 400) {
    setRGB(0, 3, 0);  // 센서 범위 밖 → 거의 꺼짐
    return;
  }

  if (d < D_DANGER) {
    // DANGER: 빨강 점멸 (300ms 주기)
    bool on = (millis() % 300) < 150;
    setRGB(on ? 255 : 60, 0, 0);

  } else if (d < D_CLOSE) {
    // CLOSE: 주황 (빨강 고정 + 초록 비례)
    // 10cm→(255,0,0) 30cm→(255,100,0)
    float t = (d - D_DANGER) / (D_CLOSE - D_DANGER);  // 0~1
    setRGB(255, (int)(100 * t), 0);

  } else if (d < D_MID) {
    // MID: 파랑, 가까울수록 밝게
    // 30cm→180, 60cm→30
    float t = 1.0 - (d - D_CLOSE) / (D_MID - D_CLOSE);  // 1~0
    int b = (int)(30 + 150 * t);
    setRGB(0, 0, b);

  } else {
    // SAFE: 초록 희미
    // 60cm→40, 120cm→5
    float t = 1.0 - min((d - D_MID) / 100.0f, 1.0f);  // 1~0
    int g = (int)(5 + 35 * t);
    setRGB(0, g, 0);
  }
}

//  블루투스 전송
//  B1,F:23,B:45,L:12,R:67,MIN:12,LV:CLOSE
void sendBT(float minD, Level lv) {
  char buf[72];
  snprintf(buf, sizeof(buf),
    "B1,F:%d,B:%d,L:%d,R:%d,MIN:%d,LV:%s",
    (int)dist[0], (int)dist[1],
    (int)dist[2], (int)dist[3],
    (int)minD, lvStr(lv)
  );
  btSerial.println(buf);
  Serial.print("[BT] "); Serial.println(buf);
}

//  Level → 문자열
const char* lvStr(Level lv) {
  switch (lv) {
    case DANGER_LV: return "DANGER";
    case CLOSE_LV:  return "CLOSE";
    case MID:       return "MID";
    default:        return "SAFE";
  }
}

 

파이썬에서 실행 / 블루투스

import serial
import time

PORT = "COM3"      # 본인 포트
BAUDRATE = 9600


def parse_line(raw):
    # [BT] 제거
    raw = raw.replace("[BT] ", "")

    parts = raw.split(",")

    data = {}
    for p in parts:
        if ":" in p:
            key, val = p.split(":", 1)
            data[key.strip()] = val.strip()

    return data


def pretty_print(data):
    f = data.get("F", "-")
    b = data.get("B", "-")
    l = data.get("L", "-")
    r = data.get("R", "-")
    lv = data.get("LV", "-")
    direction = data.get("DIR", "-")

    # 1줄
    line1 = f"F: {f}  B: {b}  L: {l}  R: {r}  |  LV: {lv}"

    # 2줄 (등급별 방향 표시)
    danger = direction if lv == "DANGER" else "-"
    close  = direction if lv == "CLOSE"  else "-"
    mid    = direction if lv == "MID"    else "-"
    safe   = direction if lv == "SAFE"   else "-"

    line2 = f"DANGER: {danger}  CLOSE: {close}  MID: {mid}  SAFE: {safe}"

    print(line1)
    print(line2)
    print("-" * 60)


def main():
    try:
        ser = serial.Serial(PORT, BAUDRATE, timeout=1)
        print(f"[INFO] Connected to {PORT}")
        time.sleep(2)

        while True:
            if ser.in_waiting:
                raw = ser.readline().decode(errors="ignore").strip()

                if raw and "B1" in raw:
                    data = parse_line(raw)
                    pretty_print(data)

    except KeyboardInterrupt:
        print("\n[INFO] 종료")
    except Exception as e:
        print("ERROR:", e)


if __name__ == "__main__":
    main()