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

To Do List(ROBOT ARM)

로봇팔 완성 작업 순서

1단계: 전원 시스템 구축
18650 4개 충전 상태 확인 (각 셀 4.0V 이상)
홀더에 18650 3개 끼우기 (극성 확인)
홀더 → 벅컨버터 IN+/IN- 연결
멀티미터로 벅컨버터 OUT 측정하며 트리머 돌려 6.5V 정확히 맞추기
CC 트리머 8A 근처로 (있으면)

2단계: PCA9685 + Arduino 배선
벅컨버터 OUT+ → PCA9685 V+
벅컨버터 OUT- → PCA9685 GND (V+ 옆)
Arduino 5V → PCA9685 VCC (로직 전원)
Arduino GND → PCA9685 GND (반드시 공통)
Arduino A4 → SDA, A5 → SCL
(옵션) 220uF 콘덴서를 V+/GND에 병렬

3단계: 서보 6개 장착 & 채널 식별
서보 6개를 PCA9685 채널에 꽂기 (위치는 임의로)
Arduino에 채널 식별 코드 업로드
시리얼로 0~15 차례로 보내며 어떤 채널이 어느 관절인지 매핑
매핑표 작성 (베이스/어깨/팔꿈치/손목굽힘/손목회전/그리퍼)
35kg 서보 3개는 베이스/어깨/팔꿈치에 배치 확인 (재꽂기 필요 시)

4단계: 영점 잡기 (서보 혼 결합)
모든 서보를 코드로 90도 중립으로 보냄
이 상태에서 서보 혼(horn)을 결합 → 가동 범위가 양쪽 균등하게
각 관절 자체적으로 한계 확인 (CH? 60, CH? 120 같은 작은 각도로)

5단계: 관절별 안전 한계 측정
한 관절씩 천천히 양 끝 한계 찾기 (5도씩 증가)
부딪힘/걸림/소리 나는 직전 각도 기록
관절별 MIN_ANGLE / MAX_ANGLE / HOME 정리
안전 한계 포함된 본 코드 작성·업로드

6단계: 피킹 동작 설계
홈 자세에서 시작
한 단계씩 명령 보내며 바닥 피킹 자세 찾기
베이스 회전 → 어깨 숙임 → 팔꿈치 펴기 → 손목 굽힘
각 단계 각도값 기록
pickFromFloor() 함수에 값 반영, K 명령으로 자동 실행 테스트

7단계: 안정화 & 차체 통합
반복 동작 테스트 (10회 정도)
떨림/걸림/오차 보정
로봇팔을 TurtleBot3 상부에 마운트
차체 진동/주행 시 팔 자세 유지 확인

8단계: ROS2 연동 (전체 시스템 통합 단계)
Arduino ↔ ROS2 시리얼/MQTT 브리지
ROS2 토픽으로 피킹 명령 받기
카메라 → 객체 검출 → 좌표 → 로봇팔 명령 흐름

 

FTS330M 모터 테스트

모터 방향 확인

 

부하X 기준 PULSE 값 찾기

MIN 400, MAX 2600이 안정적

 

채널 별 테스트 코드

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

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define PULSE_MIN_US 400
#define PULSE_MAX_US 2600
#define ANGLE_MAX    180

int curAngle[16];

int angleToTicks(int angle) {
  int us = map(angle, 0, ANGLE_MAX, PULSE_MIN_US, PULSE_MAX_US);
  return (int)((long)us * 4096L * 50L / 1000000L);
}

void moveSmooth(int ch, int target, int stepMs = 20) {
  target = constrain(target, 0, ANGLE_MAX);
  int cur = curAngle[ch];
  int step = (target > cur) ? 1 : -1;
  for (int a = cur; a != target; a += step) {
    pwm.setPWM(ch, 0, angleToTicks(a));
    delay(stepMs);
  }
  pwm.setPWM(ch, 0, angleToTicks(target));
  curAngle[ch] = target;
}

void setup() {
  Serial.begin(9600);
  pwm.begin();
  pwm.setPWMFreq(50);
  delay(500);
  for (int i = 0; i < 16; i++) curAngle[i] = 90;

  Serial.println("=== Servo Control ===");
  Serial.println("Input: <channel> <angle>");
  Serial.println("  Examples:");
  Serial.println("    10 90     (CH10 to 90 deg)");
  Serial.println("    CH10 90   (with prefix)");
  Serial.println("    10,90     (with comma)");
  Serial.println("  Range: channel 0~15, angle 0~180");
}

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

    // CH 접두사 제거
    if (s.startsWith("CH") || s.startsWith("ch")) s = s.substring(2);

    // 공백 또는 쉼표로 분리
    int sep = s.indexOf(' ');
    if (sep < 0) sep = s.indexOf(',');
    if (sep < 0) {
      Serial.println("Format: <ch> <angle>  (ex: 10 90)");
      return;
    }

    int ch    = s.substring(0, sep).toInt();
    int angle = s.substring(sep + 1).toInt();

    if (ch < 0 || ch > 15) {
      Serial.println("Channel must be 0~15");
      return;
    }
    if (angle < 0 || angle > ANGLE_MAX) {
      Serial.println("Angle must be 0~180");
      return;
    }

    Serial.print("CH"); Serial.print(ch);
    Serial.print(" -> "); Serial.print(angle); Serial.println(" deg");
    moveSmooth(ch, angle);
    Serial.print("  done. Current = "); Serial.println(curAngle[ch]);
  }
}

 

 

채널	부위	모터	펄스(us)	안전범위	home	의미 / 방향
5	그리퍼(집게)	MG996R	500–2500	0 ~ 50	0	0=열림, 50=닫힘
7	손목 회전(roll)	MG996R	500–2500	40 ~ 130	130	40=세로(y)잡기, 130=가로(x)잡기
9	손목 굽힘(pitch)	MG996R	500–2500	70 ~ 170	170	170=일자 뻗기, 70=숙이기
11	팔꿈치(elbow)	FT330M 35kg	400–2600	0 ~ 90	0	0=일자 뻗기, 90=숙이기
13	어깨(shoulder)	FT330M 35kg	400–2600	20 ~ 100	100	100=수직 들기, 20=아래로 (오차 있음)
15	베이스 회전	FT330M 35kg	400–2600	0 ~ 180	90	0=왼쪽, 90=뒤, 180=오른쪽 (전면 도달 불가)

 

전원/하드웨어

항목	값
배터리	18650 3.7V × 3 (3S 직렬)
홀더 출력	약 11.1~12.6V
벅 컨버터 출력	6.493V (→ PCA9685 V+)
펄스 주파수	50Hz
GND	벅 / PCA9685 / 아두이노 공통

 

4단계까지 거의 완료( 모든 관절이 굽혔을 때 서로 충돌하는 범위 테스트 해야 함)

 

각 부위별 길이 측정해서 충돌할 각도 구해서 안전 범위 정하기

ㄴ현재 자 없어서 길이 측정 불가 -> 3D 프린터 모델에서 길이 검색해보기

ㄴㄴ25, 58, 65, 120, 130 정도로 측정됨(오차 클 것으로 예상)

 

대략적 그림