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

 

로봇팔 받침용 터틀봇 판 + 팔로워 차량 1대 하드웨어 완성 및 테스트

 

로봇팔 모터 잘못 구매(기존 MG996R -> 서보 모터 레드캣 레이싱 헥스플라이 25KG 4개)

ㄴ가장 빠른 배송이 6월 10일 이후

 

로봇팔 아두이노 테스트용 코드 완성

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

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// 풀스윙용 펄스 범위 (MG996R 확장)
#define PULSE_MIN_US  500
#define PULSE_MAX_US  2500
#define ANGLE_MAX     270

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

int curAngle[16];

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("=== Channel Angle Control (0~270) ===");
  Serial.println("Format: CH<num> <angle>");
  Serial.println("Example: CH10 200");
  Serial.println("Range: ch 0~15, angle 0~270");
}

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

    if (s.startsWith("CH") || s.startsWith("ch")) {
      s = s.substring(2);
    }

    int spaceIdx = s.indexOf(' ');
    if (spaceIdx < 0) {
      Serial.println("Format error. Example: CH10 200");
      return;
    }

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

    if (ch < 0 || ch > 15) {
      Serial.println("Invalid channel (0~15)");
      return;
    }
    if (angle < 0 || angle > ANGLE_MAX) {
      Serial.print("Invalid angle (0~"); Serial.print(ANGLE_MAX); Serial.println(")");
      return;
    }

    Serial.print("CH"); Serial.print(ch);
    Serial.print(" -> "); Serial.print(angle); Serial.println(" deg");

    moveSmooth(ch, angle);

    Serial.println("  done");
  }
}

 

테스트 해봐야 하는 새 코드

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

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVO_FREQ 50

// ── 채널 매핑 (베이스 → 그리퍼) ──
// 식별 코드로 확인 후 필요 시 수정
#define CH_BASE         0   // J1 베이스 회전
#define CH_SHOULDER     1   // J2 어깨 (토크 최대)
#define CH_ELBOW        2   // J3 팔꿈치
#define CH_WRIST_PITCH  3   // J4 손목 굽힘
#define CH_WRIST_ROLL   4   // J5 손목 회전
#define CH_GRIPPER      5   // J6 집게

// ── MG996R 표준 사양 ──
#define ANGLE_MIN     0
#define ANGLE_MAX     180
#define PULSE_MIN_US  1000   // 0도
#define PULSE_MAX_US  2000   // 180도

// ── 집게 위치 (실제 집게 장착 후 튜닝 필요) ──
#define GRIP_OPEN   60
#define GRIP_CLOSE  120

// ── 홈 자세 (모두 90도 = 중립) ──
#define HOME_BASE          90
#define HOME_SHOULDER      90
#define HOME_ELBOW         90
#define HOME_WRIST_PITCH   90
#define HOME_WRIST_ROLL    90
#define HOME_GRIPPER       GRIP_OPEN

// ── 현재 각도 ──
int curBase        = HOME_BASE;
int curShoulder    = HOME_SHOULDER;
int curElbow       = HOME_ELBOW;
int curWristPitch  = HOME_WRIST_PITCH;
int curWristRoll   = HOME_WRIST_ROLL;
int curGripper     = HOME_GRIPPER;

// ── 각도 → 펄스 변환 (ticks) ──
int angleToTicks(int angle) {
  int us = map(angle, 0, 180, PULSE_MIN_US, PULSE_MAX_US);
  return (int)((long)us * 4096L * SERVO_FREQ / 1000000L);
}

void smoothMove(int ch, int &cur, int target, int delayMs = 15) {
  target = constrain(target, ANGLE_MIN, ANGLE_MAX);
  int step = (target > cur) ? 1 : -1;
  for (int a = cur; a != target; a += step) {
    pwm.setPWM(ch, 0, angleToTicks(a));
    delay(delayMs);
  }
  pwm.setPWM(ch, 0, angleToTicks(target));
  cur = target;
}

// ── 홈 자세 ──
void homePose() {
  smoothMove(CH_GRIPPER,     curGripper,    HOME_GRIPPER);
  smoothMove(CH_WRIST_ROLL,  curWristRoll,  HOME_WRIST_ROLL);
  smoothMove(CH_WRIST_PITCH, curWristPitch, HOME_WRIST_PITCH);
  smoothMove(CH_ELBOW,       curElbow,      HOME_ELBOW);
  smoothMove(CH_SHOULDER,    curShoulder,   HOME_SHOULDER);
  smoothMove(CH_BASE,        curBase,       HOME_BASE);
  Serial.println(">> HOME");
}

// ── 바닥 피킹 (각도값은 실제 측정 후 튜닝) ──
void pickFromFloor() {
  Serial.println(">> 1. 집게 열기");
  smoothMove(CH_GRIPPER, curGripper, GRIP_OPEN);
  delay(300);

  Serial.println(">> 2. 어깨 숙이기");
  smoothMove(CH_SHOULDER, curShoulder, 45);
  delay(300);

  Serial.println(">> 3. 팔꿈치 펴기");
  smoothMove(CH_ELBOW, curElbow, 120);
  delay(300);

  Serial.println(">> 4. 손목 굽혀서 바닥 향함");
  smoothMove(CH_WRIST_PITCH, curWristPitch, 60);
  delay(500);

  Serial.println(">> 5. 집게 닫기");
  smoothMove(CH_GRIPPER, curGripper, GRIP_CLOSE);
  delay(500);

  Serial.println(">> 6. 들어올리기");
  smoothMove(CH_SHOULDER, curShoulder, 90);
  smoothMove(CH_ELBOW, curElbow, 90);
  delay(300);

  Serial.println(">> 7. 홈 복귀");
  smoothMove(CH_WRIST_PITCH, curWristPitch, HOME_WRIST_PITCH);
  Serial.println(">> 완료 (집게는 닫힌 상태)");
}

void printStatus() {
  Serial.print("  B:");  Serial.print(curBase);
  Serial.print(" S:");   Serial.print(curShoulder);
  Serial.print(" E:");   Serial.print(curElbow);
  Serial.print(" WP:");  Serial.print(curWristPitch);
  Serial.print(" WR:");  Serial.print(curWristRoll);
  Serial.print(" G:");   Serial.println(curGripper);
}

void printMenu() {
  Serial.println("\n=== 6DOF Arm (MG996R) ===");
  Serial.println("  B<n>  베이스 회전 (CH0)");
  Serial.println("  S<n>  어깨 (CH1)");
  Serial.println("  E<n>  팔꿈치 (CH2)");
  Serial.println("  P<n>  손목 굽힘 (CH3)");
  Serial.println("  R<n>  손목 회전 (CH4)");
  Serial.println("  G<n>  집게 (CH5)");
  Serial.println("  H     홈 위치");
  Serial.println("  K     바닥 피킹");
  Serial.println("  Range: 0 ~ 180");
}

void setup() {
  Serial.begin(9600);
  pwm.begin();
  pwm.setPWMFreq(SERVO_FREQ);
  delay(500);

  pwm.setPWM(CH_BASE,         0, angleToTicks(curBase));
  pwm.setPWM(CH_SHOULDER,     0, angleToTicks(curShoulder));
  pwm.setPWM(CH_ELBOW,        0, angleToTicks(curElbow));
  pwm.setPWM(CH_WRIST_PITCH,  0, angleToTicks(curWristPitch));
  pwm.setPWM(CH_WRIST_ROLL,   0, angleToTicks(curWristRoll));
  pwm.setPWM(CH_GRIPPER,      0, angleToTicks(curGripper));

  printMenu();
  printStatus();
}

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

    char c = cmd.charAt(0);
    int angle = cmd.substring(1).toInt();

    switch (c) {
      case 'B': case 'b': smoothMove(CH_BASE,        curBase,       angle); break;
      case 'S': case 's': smoothMove(CH_SHOULDER,    curShoulder,   angle); break;
      case 'E': case 'e': smoothMove(CH_ELBOW,       curElbow,      angle); break;
      case 'P': case 'p': smoothMove(CH_WRIST_PITCH, curWristPitch, angle); break;
      case 'R': case 'r': smoothMove(CH_WRIST_ROLL,  curWristRoll,  angle); break;
      case 'G': case 'g': smoothMove(CH_GRIPPER,     curGripper,    angle); break;
      case 'H': case 'h': homePose(); break;
      case 'K': case 'k': pickFromFloor(); break;
      default: Serial.println("?"); return;
    }
    printStatus();
  }
}