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

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <math.h>

Adafruit_PWMServoDriver pca = Adafruit_PWMServoDriver(0x40);

// ── 채널 정의 (집게=5, 손목회전=7, 손목굽힘=9, 팔꿈치=11, 어깨=13, 베이스=15) ──
#define CH_GRIPPER    5
#define CH_WRIST_R    7
#define CH_WRIST_P    9
#define CH_ELBOW     11
#define CH_SHOULDER  13
#define CH_BASE      15

// ── 링크 길이(mm) & 베이스 위험영역 ──
#define L1     130.0   // 어깨 → 팔꿈치
#define L2      65.0   // 팔꿈치 → 손목
#define L3      60.0   // 손목 → 집게 끝
#define R_BASE  70.0   // 디스크 반경 + 안전마진
#define H_SHLDR 40.0   // 어깨축 높이(디스크 윗면 기준, 추정 — 실측시 교체)

// ── 채널별 보정 (pulseMin, pulseMax, safeMin, safeMax, home) ──
struct ServoCal { int pMin, pMax, sMin, sMax, home; };
ServoCal cal[16];

void initCal() {
  for (int i = 0; i < 16; i++) cal[i] = {500, 2500, 0, 180, 90};
  //                               pMin  pMax  sMin sMax home
  cal[CH_GRIPPER]   = {500, 2500,   0,  50,   0};  // 0=열림  50=닫힘
  cal[CH_WRIST_R]   = {500, 2500,  40, 130, 130};  // 40=세로 130=가로
  cal[CH_WRIST_P]   = {500, 2500,  70, 170,  90};  // 170=일자 70=숙임
  cal[CH_ELBOW]     = {400, 2600,   0,  90,  60};  // 0=일자  90=굽힘
  cal[CH_SHOULDER]  = {400, 2600,  20, 140, 120};  // 100=수직 20=아래 140=뒤로젖힘
  cal[CH_BASE]      = {400, 2600,   0, 180,  90};  // 0=왼  90=뒤  180=오른
}

int  curAngle[16];
bool initialized[16];
int  moveDelay = 25;   // 1도당 ms (클수록 느림)
bool ems = false;

// ── 각도 → PCA 틱 ──
int angleToTicks(int ch, int angle) {
  int us = map(angle, 0, 180, cal[ch].pMin, cal[ch].pMax);
  return (int)((long)us * 4096L * 50L / 1000000L);
}

// ── FK: 어깨 기준 집게 끝 좌표 (수직평면) ──
void fkTip(int a13, int a11, int a9, float &gx, float &gz) {
  float ps = radians(a13 - 10);
  float pe = ps - radians(a11);
  float pw = pe - radians(170 - a9);
  float ex = L1 * cos(ps),         ez = L1 * sin(ps);
  float wx = ex + L2 * cos(pe),    wz = ez + L2 * sin(pe);
  gx = wx + L3 * cos(pw);          gz = wz + L3 * sin(pw);
}

// ── 충돌 판정: 집게 끝이 베이스 위험구역(아래+반경 안) 진입 ──
bool tipCollide(int a13, int a11, int a9) {
  float gx, gz;
  fkTip(a13, a11, a9, gx, gz);
  return (gz < -H_SHLDR) && (fabs(gx) < R_BASE);
}

// ── 긴급정지 확인 ──
void checkEMS() {
  if (Serial.available()) {
    String peek = Serial.readStringUntil('\n');
    peek.trim();
    if (peek == "0") { ems = true; Serial.println("!!! EMS !!!"); }
  }
}

// ── 부드러운 이동 (EMS + 팔관절 실시간 충돌가드) ──
void smoothMove(int ch, int target) {
  if (ems) return;
  target = constrain(target, cal[ch].sMin, cal[ch].sMax);

  if (!initialized[ch]) {
    pca.setPWM(ch, 0, angleToTicks(ch, target));
    curAngle[ch] = target; initialized[ch] = true;
    Serial.print("CH"); Serial.print(ch); Serial.print(" 초기 -> "); Serial.println(target);
    return;
  }

  bool armJoint = (ch == CH_SHOULDER || ch == CH_ELBOW || ch == CH_WRIST_P);
  int step = (target > curAngle[ch]) ? 1 : -1;

  for (int a = curAngle[ch]; a != target; a += step) {
    // 팔 관절이면 이동 전 충돌 예측
    if (armJoint) {
      int a13 = (ch == CH_SHOULDER) ? a : curAngle[CH_SHOULDER];
      int a11 = (ch == CH_ELBOW)    ? a : curAngle[CH_ELBOW];
      int a9  = (ch == CH_WRIST_P)  ? a : curAngle[CH_WRIST_P];
      if (tipCollide(a13, a11, a9)) {
        Serial.print("!! 충돌가드 정지 @ CH"); Serial.print(ch);
        Serial.print(" "); Serial.println(a);
        curAngle[ch] = a;
        return;
      }
    }
    pca.setPWM(ch, 0, angleToTicks(ch, a));
    delay(moveDelay);
    checkEMS();
    if (ems) { curAngle[ch] = a; return; }
  }
  pca.setPWM(ch, 0, angleToTicks(ch, target));
  curAngle[ch] = target;
  Serial.print("CH"); Serial.print(ch); Serial.print(" -> "); Serial.println(target);
}

// ── 팔(어깨/팔꿈치/손목) 안전 순서 이동 ──
//    펴는 방향(올림)은 어깨먼저, 접는 방향(내림)은 손목먼저 → 충돌 최소화
void moveArm(int a13, int a11, int a9) {
  bool lowering = (a13 < curAngle[CH_SHOULDER]);  // 어깨 내리는 동작인가
  if (lowering) {                                  // 내릴 땐 끝단부터 접기
    smoothMove(CH_WRIST_P, a9);  if (ems) return;
    smoothMove(CH_ELBOW,   a11); if (ems) return;
    smoothMove(CH_SHOULDER,a13);
  } else {                                         // 올릴 땐 어깨부터 펴기
    smoothMove(CH_SHOULDER,a13); if (ems) return;
    smoothMove(CH_ELBOW,   a11); if (ems) return;
    smoothMove(CH_WRIST_P, a9);
  }
}

// ────────── 자세 라이브러리 (case별) ──────────
// 형식: moveArm(어깨13, 팔꿈치11, 손목9)
void poseHome() {                    // 대기: 위로 컴팩트
  moveArm(120, 60, 90); if (ems) return;
  smoothMove(CH_WRIST_R, 130);
  smoothMove(CH_BASE, 90);
  smoothMove(CH_GRIPPER, 0);
  Serial.println(">> HOME");
}
void poseReady(int baseDir) {        // 준비: 방향 조준 + 팔 들고 집게 열기
  smoothMove(CH_GRIPPER, 0);
  smoothMove(CH_BASE, baseDir);
  moveArm(90, 30, 120);
  Serial.println(">> READY");
}
void posePickDown() {                // 바닥으로 내려 집을 자세 (집게 열린 채)
  smoothMove(CH_GRIPPER, 0);
  moveArm(40, 60, 120);
  Serial.println(">> PICK_DOWN");
}
void doGrip()    { smoothMove(CH_GRIPPER, 50); Serial.println(">> GRIP"); }
void doRelease() { smoothMove(CH_GRIPPER, 0);  Serial.println(">> RELEASE"); }
void poseLift()  { moveArm(120, 40, 120); Serial.println(">> LIFT"); }   // 들어올리기
void poseCarry() {                   // 이동중: 뒤보고 컴팩트(물건 쥔 채)
  smoothMove(CH_BASE, 90);
  moveArm(120, 60, 100);
  Serial.println(">> CARRY");
}
void posePlaceBack() {               // 뒤 팔로워에 내려놓기
  smoothMove(CH_BASE, 90);
  moveArm(80, 50, 100); if (ems) return;
  smoothMove(CH_GRIPPER, 0);
  Serial.println(">> PLACE_BACK");
}

// ── 자동 피킹 시퀀스 (오른쪽 집기 → 뒤 내려놓기) ──
void pickSequence() {
  poseReady(180);    if (ems) return;   // 오른쪽 조준
  posePickDown();    if (ems) return;
  doGrip();          if (ems) return;
  poseLift();        if (ems) return;
  poseCarry();       if (ems) return;   // 뒤로 회전
  posePlaceBack();   if (ems) return;
  poseHome();
  Serial.println(">>> 시퀀스 완료");
}

void setup() {
  Serial.begin(9600);
  initCal();
  pca.begin();
  pca.setPWMFreq(50);
  delay(10);
  for (int i = 0; i < 16; i++) { curAngle[i] = 90; initialized[i] = false; }

  Serial.println("=== 로봇팔 제어 (FK 충돌가드 + EMS) ===");
  Serial.println("채널 직접 : '13 90'");
  Serial.println("자세 명령 : H=홈 RL/RB/RR=준비(좌/뒤/우) P=내리기 G=집기 O=놓기 L=들기 C=이동 B=뒤놓기");
  Serial.println("자동      : K=피킹 시퀀스");
  Serial.println("속도      : 'SP 30'   긴급정지: '0'   해제: 'R'");
}

void loop() {
  if (!Serial.available()) return;
  String line = Serial.readStringUntil('\n');
  line.trim();
  if (line.length() == 0) return;

  // 긴급정지
  if (line == "0") { ems = true; Serial.println("!!! EMS !!! — 'R'로 재개"); return; }

  String up = line; up.toUpperCase();

  if (up == "R")  { ems = false; Serial.println("EMS 해제"); return; }
  if (ems)        { Serial.println("EMS 중 — 'R'로 재개"); return; }

  // 자세 명령
  if (up == "H")  { poseHome();       return; }
  if (up == "RL") { poseReady(0);     return; }  // 왼쪽
  if (up == "RB") { poseReady(90);    return; }  // 뒤
  if (up == "RR") { poseReady(180);   return; }  // 오른쪽
  if (up == "P")  { posePickDown();   return; }
  if (up == "G")  { doGrip();         return; }
  if (up == "O")  { doRelease();      return; }
  if (up == "L")  { poseLift();       return; }
  if (up == "C")  { poseCarry();      return; }
  if (up == "B")  { posePlaceBack();  return; }
  if (up == "K")  { pickSequence();   return; }

  // 속도
  if (up.startsWith("SP")) {
    int v = up.substring(2).toInt();
    if (v >= 5 && v <= 200) { moveDelay = v; Serial.print("속도 -> "); Serial.println(moveDelay); }
    return;
  }

  // 채널 직접 제어 (13 90)
  line.replace(",", " ");
  up = line; up.toUpperCase(); up.replace("CH", ""); up.trim();
  int sp = up.indexOf(' ');
  if (sp < 0) { Serial.println("형식: 13 90"); return; }
  int ch  = up.substring(0, sp).toInt();
  int ang = up.substring(sp + 1).toInt();
  if (ch < 0 || ch > 15) { Serial.println("채널 0~15"); return; }
  smoothMove(ch, ang);
}

전체 케이스 구현 완료

(세부 길이 조정만 하면 될듯)

 

자세 명령어(case 별)

명령	자세	관절값(13,11,9)	용도
H	HOME	120 / 60 / 90	대기 (위로 컴팩트)
RL/RB/RR	READY	90 / 30 / 120	준비 (좌/뒤/우 조준 + 집게 열기)
P	PICK_DOWN	20 / 75 / 120	바닥으로 내려 집을 자세
G	GRIP	집게 50	닫기
O	RELEASE	집게 0	열기
L	LIFT	120 / 40 / 120	들어올리기
C	CARRY	120 / 60 / 100	이동중 (뒤보고 컴팩트)
B	PLACE_BACK	80 / 50 / 100	뒤 팔로워에 내려놓기
K	자동 시퀀스	—	우측집기→뒤놓기 전체 자동

 

+EMS로 터미널에 0 넣으면 정지

 

통신에 사용할 파이썬 버전 코드

"""
R0botArmCase.py
==================
로봇팔 Python 컨트롤러

구조:
    Python- --시리얼--> Arduino(RobotArmCase.ino) --I2C--> PCA9685 --> 서보 6개

아두이노 펌웨어가 저수준 실행(부드러운 이동/충돌가드/EMS/안전한계) 담당
python 파일은 상위 명령을 시리얼로 보내는 컨트롤러

펌웨어가 받는 명령 문자열 (ino loop()와 동일):
    "13 90"  채널 직접 제어
    H        홈
    RL/RB/RR 준비 (좌/뒤/우)
    P        바닥 내리기
    G / O    집기 / 놓기
    L        들기
    C        이동(뒤보고 컴팩트)
    B        뒤 내려놓기
    K        자동 피킹 시퀀스
    SP 30    속도 변경(ms/도)
    0        긴급정지(EMS)
    R        EMS 해제

사용 예:
    from RobotArmCase import RobotArm
    arm = RobotArm("COM12")      # 포트는 환경에 맞게
    arm.home()
    arm.channel(13, 90)
    arm.move_to(130, 0, -90)     # 좌표(mm)로 IK 이동
    arm.pick_sequence()
    arm.close()

의존성:
    pip install pyserial
"""

import math
import time

try:
    import serial  # type: ignore # pyserial
except ImportError:
    serial = None


# ─────────────────────────────────────────────
#  채널 / 링크 / 보정값  (ino와 동일하게 유지)
# ─────────────────────────────────────────────
CH_GRIPPER  = 5
CH_WRIST_R  = 7
CH_WRIST_P  = 9
CH_ELBOW    = 11
CH_SHOULDER = 13
CH_BASE     = 15

L1, L2, L3 = 130.0, 65.0, 60.0   # 어깨-팔꿈치, 팔꿈치-손목, 손목-집게끝 (mm)
R_BASE     = 70.0                # 디스크 반경 + 안전마진
H_SHLDR    = 40.0                # 어깨축 높이(디스크 윗면 기준, 추정)

# 채널별 보정: (pulseMin, pulseMax, safeMin, safeMax, home)
CAL = {
    CH_GRIPPER:  (500, 2500,   0,  50,   0),   # 0=열림  50=닫힘
    CH_WRIST_R:  (500, 2500,  40, 130, 130),   # 40=세로 130=가로
    CH_WRIST_P:  (500, 2500,  70, 170,  90),   # 170=일자 70=숙임
    CH_ELBOW:    (400, 2600,   0,  90,  60),   # 0=일자  90=굽힘
    CH_SHOULDER: (400, 2600,  20, 140, 120),   # 100=수직 20=아래 140=뒤로젖힘
    CH_BASE:     (400, 2600,   0, 180,  90),   # 0=왼 90=뒤 180=오른
}

# 자세 라이브러리 (어깨13, 팔꿈치11, 손목9) — test1.ino 자세값과 동일
POSES = {
    "HOME":       (120, 60,  90),
    "READY":      (90,  30, 120),
    "PICK_DOWN":  (40,  60, 120),
    "LIFT":       (120, 40, 120),
    "CARRY":      (120, 60, 100),
    "PLACE_BACK": (80,  50, 100),
}

# 의미 있는 단일값
GRIP_OPEN, GRIP_CLOSE = 0, 50
BASE_LEFT, BASE_BACK, BASE_RIGHT = 0, 90, 180


def clamp(v, lo, hi):
    return max(lo, min(hi, v))


# ─────────────────────────────────────────────
#  순운동학 / 역기구학 (충돌 사전검사·좌표제어용)
# ─────────────────────────────────────────────
def fk_tip(a13, a11, a9):
    """어깨 기준 집게 끝 좌표 (gx=전방, gz=높이) mm. test1.ino fkTip과 동일."""
    ps = math.radians(a13 - 10)
    pe = ps - math.radians(a11)
    pw = pe - math.radians(170 - a9)
    ex, ez = L1 * math.cos(ps), L1 * math.sin(ps)
    wx, wz = ex + L2 * math.cos(pe), ez + L2 * math.sin(pe)
    gx = wx + L3 * math.cos(pw)
    gz = wz + L3 * math.sin(pw)
    return gx, gz


def tip_collide(a13, a11, a9):
    """집게 끝이 베이스 위험구역 진입하면 True."""
    gx, gz = fk_tip(a13, a11, a9)
    return (gz < -H_SHLDR) and (abs(gx) < R_BASE)


def solve_ik(x, y, z):
    """
    목표 좌표(x=전방, y=좌, z=위; mm, 어깨축 원점) -> (base, shoulder, elbow) 서보각
    손목은 일자(170) 가정. 도달 불가면 None
    """
    wz = z + L3            # 집게 길이 보정(집게가 아래 향한다 가정)
    r  = math.hypot(x, y)
    D2 = r * r + wz * wz
    D  = math.sqrt(D2)
    if D > L1 + L2 or D < abs(L1 - L2):
        return None        # 도달 불가능

    # 팔꿈치 (코사인 법칙)
    cos_e = clamp((D2 - L1*L1 - L2*L2) / (2*L1*L2), -1.0, 1.0)
    theta_elbow = math.acos(cos_e)

    # 어깨
    alpha = math.atan2(wz, r)
    beta  = math.atan2(L2 * math.sin(theta_elbow), L1 + L2 * math.cos(theta_elbow))
    theta_shoulder = alpha - beta

    # 라디안 -> 서보각
    base_deg     = clamp(int(math.degrees(math.atan2(y, x)) + 90), *CAL[CH_BASE][2:4])
    shoulder_deg = clamp(int(math.degrees(theta_shoulder) + 10),   *CAL[CH_SHOULDER][2:4])
    elbow_deg    = clamp(int(math.degrees(theta_elbow)),           *CAL[CH_ELBOW][2:4])
    return base_deg, shoulder_deg, elbow_deg


# ─────────────────────────────────────────────
#  로봇팔 컨트롤러
# ─────────────────────────────────────────────
class RobotArm:
    def __init__(self, port="COM12", baud=9600, timeout=1.0, verbose=True,
                 connect=True):
        self.port = port
        self.baud = baud
        self.verbose = verbose
        self.ser = None
        if connect:
            self.connect(timeout)

    # ── 연결 ──
    def connect(self, timeout=1.0):
        if serial is None:
            raise RuntimeError("pyserial 미설치: pip install pyserial")
        self.ser = serial.Serial(self.port, self.baud, timeout=timeout)
        time.sleep(2.0)          # 아두이노 리셋 대기
        self._drain()
        if self.verbose:
            print(f"[연결] {self.port} @ {self.baud}")

    def close(self):
        if self.ser and self.ser.is_open:
            self.ser.close()
            if self.verbose:
                print("[종료] 시리얼 닫음")

    # ── 저수준 송수신 ──
    def send(self, cmd, wait=0.05):
        """명령 문자열 전송 후 펌웨어 응답 읽어서 반환."""
        if not self.ser or not self.ser.is_open:
            raise RuntimeError("시리얼 미연결")
        self.ser.write((cmd.strip() + "\n").encode("utf-8"))
        time.sleep(wait)
        return self._read_lines()

    def _read_lines(self):
        out = []
        while self.ser.in_waiting:
            line = self.ser.readline().decode("utf-8", "ignore").strip()
            if line:
                out.append(line)
                if self.verbose:
                    print("  <", line)
        return out

    def _drain(self):
        time.sleep(0.1)
        self._read_lines()

    # ── 채널 직접 제어 ──
    def channel(self, ch, angle):
        """단일 채널 각도 (펌웨어가 안전한계/충돌가드 적용)."""
        return self.send(f"{ch} {int(angle)}")

    # ── 자세 명령 (펌웨어 자세 라이브러리 호출 = 안전순서 이동) ──
    def home(self):        return self.send("H", wait=0.1)
    def ready_left(self):  return self.send("RL", wait=0.1)
    def ready_back(self):  return self.send("RB", wait=0.1)
    def ready_right(self): return self.send("RR", wait=0.1)
    def pick_down(self):   return self.send("P", wait=0.1)
    def grip(self):        return self.send("G")
    def release(self):     return self.send("O")
    def lift(self):        return self.send("L", wait=0.1)
    def carry(self):       return self.send("C", wait=0.1)
    def place_back(self):  return self.send("B", wait=0.1)

    def pick_sequence(self):
        """자동 피킹 시퀀스 (펌웨어 K). 완료까지 응답 폴링."""
        self.ser.write(b"K\n")
        return self._poll_until(">>> 시퀀스 완료", timeout=30)

    # ── 속도 / 긴급정지 ──
    def set_speed(self, ms_per_deg):
        return self.send(f"SP {int(ms_per_deg)}")

    def estop(self):
        """긴급정지 — 즉시 전송."""
        if self.ser and self.ser.is_open:
            self.ser.write(b"0\n")
        if self.verbose:
            print("  !! EMS 전송")
        return self._read_lines()

    def resume(self):
        return self.send("R")

    # ── 좌표 기반 이동 (Python IK -> 채널 명령) ──
    def move_to(self, x, y, z, wrist=170):
        """
        목표 좌표(mm)로 이동. IK를 Python에서 풀고 채널 명령으로 전송.
        충돌 사전검사 후 안전순서(어깨->팔꿈치->베이스)로 보냄.
        """
        sol = solve_ik(x, y, z)
        if sol is None:
            print(f"[IK] 도달 불가: ({x},{y},{z})")
            return False
        base, shoulder, elbow = sol
        if tip_collide(shoulder, elbow, wrist):
            print(f"[충돌] 목표 자세가 베이스 위험구역: sh={shoulder} el={elbow}")
            return False
        if self.verbose:
            print(f"[IK] ({x},{y},{z}) -> BASE {base}  SH {shoulder}  EL {elbow}")
        # 안전순서: 올림이면 어깨 먼저, 내림이면 팔꿈치 먼저
        self.channel(CH_SHOULDER, shoulder)
        self.channel(CH_ELBOW, elbow)
        self.channel(CH_WRIST_P, wrist)
        self.channel(CH_BASE, base)
        return True

    # ── 응답 폴링 (시퀀스 완료 대기) ──
    def _poll_until(self, marker, timeout=30):
        out, t0 = [], time.time()
        while time.time() - t0 < timeout:
            if self.ser.in_waiting:
                line = self.ser.readline().decode("utf-8", "ignore").strip()
                if line:
                    out.append(line)
                    if self.verbose:
                        print("  <", line)
                    if marker in line:
                        break
            else:
                time.sleep(0.02)
        return out


# ─────────────────────────────────────────────
#  대화형 콘솔 (시리얼 모니터 대용)
# ─────────────────────────────────────────────
def repl(port="COM12"):
    """터미널에서 펌웨어 명령을 그대로 입력하는 콘솔, 'quit'으로 종료"""
    arm = RobotArm(port)
    print("명령 입력 (H, RR, P, G, O, L, C, B, K, '13 90', SP 30, 0, R) / quit 종료")
    try:
        while True:
            cmd = input("arm> ").strip()
            if cmd.lower() in ("quit", "exit"):
                break
            if not cmd:
                continue
            arm.send(cmd)
    except KeyboardInterrupt:
        print()
    finally:
        arm.estop()
        arm.close()


if __name__ == "__main__":
    import sys
    port = sys.argv[1] if len(sys.argv) > 1 else "COM12"
    repl(port)