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

로봇팔 각도 초기값 찾기

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

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVO_FREQ 50

// ── 채널 ──
#define CH_GRIPPER 12
#define CH_WRIST   13
#define CH_MIDDLE  14
#define CH_BASE    15

// ── 각도 범위 ──
#define ANGLE_MIN 0
#define ANGLE_MAX 270

// ── 홈 자세 (수직 곧게 핀 상태) ──
#define HOME_GRIPPER 90
#define HOME_WRIST   210
#define HOME_MIDDLE  220
#define HOME_BASE    100

// ── 현재 각도 ──
int curGripper = HOME_GRIPPER;
int curWrist   = HOME_WRIST;
int curMiddle  = HOME_MIDDLE;
int curBase    = HOME_BASE;

int angleToTicks(int angle) {
  int us = map(angle, 0, 180, 500, 2400);
  return (int)((long)us * 4096L * SERVO_FREQ / 1000000L);
}

void smoothMove(int ch, int &cur, int target, int delayMs = 20) {
  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 printMenu() {
  Serial.println("\n=== Gripper Test ===");
  Serial.println("  G<angle>  집게 각도 (예: G30, G60, G90, G120)");
  Serial.println("  Range: 0 ~ 270");
  Serial.println("Other joints fixed at home position.");
}

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

  // 다른 관절은 홈 자세 고정
  pwm.setPWM(CH_WRIST,   0, angleToTicks(HOME_WRIST));
  pwm.setPWM(CH_MIDDLE,  0, angleToTicks(HOME_MIDDLE));
  pwm.setPWM(CH_BASE,    0, angleToTicks(HOME_BASE));

  // 집게 시작 위치
  pwm.setPWM(CH_GRIPPER, 0, angleToTicks(curGripper));

  printMenu();
  Serial.print("Gripper start angle: ");
  Serial.println(curGripper);
}

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();

    if (c == 'G' || c == 'g') {
      smoothMove(CH_GRIPPER, curGripper, angle);
      Serial.print("Gripper -> ");
      Serial.println(curGripper);
    } else {
      Serial.println("Use G<angle> only.");
    }
  }
}

 
팔 움직임 구현 성공
ㄴ3D 프린팅할 때 팔을 U자로 굽는게 아닌 Z자로 굽게 잘못 설계해서 하드웨어적으로 불가능함
ㄴ코드는 문제가 없으니 된다치고, 나머지 동작 완성하기 + 3D팔 프린팅 설계 다시
 
완성 코드

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

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVO_FREQ 50

// ── 채널 ──
#define CH_GRIPPER 12
#define CH_WRIST   13
#define CH_MIDDLE  14
#define CH_BASE    15

// ── 각도 범위 ──
#define ANGLE_MIN 0
#define ANGLE_MAX 270

// ── 집게 위치 ──
#define GRIP_OPEN  270
#define GRIP_CLOSE 190

// ── 홈 자세 ──
// B110 정면, M220 수직, W210 수직, G 열림
#define HOME_GRIPPER GRIP_OPEN
#define HOME_WRIST   210
#define HOME_MIDDLE  220
#define HOME_BASE    110

// ── 피킹 동작 각도 ──
#define PICK_BASE         110   // 정면 유지
#define PICK_MIDDLE_DOWN  150   // 중간관절 숙이기
#define PICK_WRIST_DOWN   150   // 윗관절 추가 숙이기
#define PICK_LIFT_MIDDLE  200   // 들어올림 (홈보다 살짝 숙인 상태)

// ── 현재 각도 ──
int curGripper = HOME_GRIPPER;
int curWrist   = HOME_WRIST;
int curMiddle  = HOME_MIDDLE;
int curBase    = HOME_BASE;

int angleToTicks(int angle) {
  int us = map(angle, 0, 180, 500, 2400);
  return (int)((long)us * 4096L * SERVO_FREQ / 1000000L);
}

void smoothMove(int ch, int &cur, int target, int delayMs = 20) {
  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 pickFromFloor() {
  Serial.println(">> 1. 집게 열기 확인");
  smoothMove(CH_GRIPPER, curGripper, GRIP_OPEN);
  delay(300);

  Serial.println(">> 2. 정면 회전 확인");
  smoothMove(CH_BASE, curBase, PICK_BASE);
  delay(300);

  Serial.println(">> 3. 중간관절 숙이기");
  smoothMove(CH_MIDDLE, curMiddle, PICK_MIDDLE_DOWN);
  delay(300);

  Serial.println(">> 4. 윗관절 미세 조정");
  smoothMove(CH_WRIST, curWrist, PICK_WRIST_DOWN);
  delay(500);

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

  Serial.println(">> 6. 들어올리기");
  smoothMove(CH_MIDDLE, curMiddle, PICK_LIFT_MIDDLE);
  delay(300);

  Serial.println(">> 7. 홈 복귀");
  smoothMove(CH_WRIST,  curWrist,  HOME_WRIST);
  smoothMove(CH_MIDDLE, curMiddle, HOME_MIDDLE);
  Serial.println(">> 완료 (집게는 닫힌 상태)");
}

void homePose() {
  smoothMove(CH_GRIPPER, curGripper, HOME_GRIPPER);
  smoothMove(CH_WRIST,   curWrist,   HOME_WRIST);
  smoothMove(CH_MIDDLE,  curMiddle,  HOME_MIDDLE);
  smoothMove(CH_BASE,    curBase,    HOME_BASE);
  Serial.println(">> HOME");
}

void printStatus() {
  Serial.print("  G:"); Serial.print(curGripper);
  Serial.print("  W:"); Serial.print(curWrist);
  Serial.print("  M:"); Serial.print(curMiddle);
  Serial.print("  B:"); Serial.println(curBase);
}

void printMenu() {
  Serial.println("\n=== Arm Pick Test ===");
  Serial.println("  G<angle>  집게 (190 닫힘 ~ 270 벌림)");
  Serial.println("  W<angle>  CH13 윗관절 (210 수직, 260 숙임, 150 뒤로)");
  Serial.println("  M<angle>  CH14 중간관절 (220 수직, 150 숙임)");
  Serial.println("  B<angle>  CH15 아래관절 (110 정면, 50 시계, 150 반시계)");
  Serial.println("  H         홈 위치");
  Serial.println("  P         바닥 피킹 시퀀스");
}

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

  pwm.setPWM(CH_GRIPPER, 0, angleToTicks(curGripper));
  pwm.setPWM(CH_WRIST,   0, angleToTicks(curWrist));
  pwm.setPWM(CH_MIDDLE,  0, angleToTicks(curMiddle));
  pwm.setPWM(CH_BASE,    0, angleToTicks(curBase));

  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 'G': case 'g': smoothMove(CH_GRIPPER, curGripper, angle); break;
      case 'W': case 'w': smoothMove(CH_WRIST,   curWrist,   angle); break;
      case 'M': case 'm': smoothMove(CH_MIDDLE,  curMiddle,  angle); break;
      case 'B': case 'b': smoothMove(CH_BASE,    curBase,    angle); break;
      case 'H': case 'h': homePose(); break;
      case 'P': case 'p': pickFromFloor(); break;
      default: Serial.println("?"); return;
    }
    printStatus();
  }
}

 

---

나머지 부품 아직 없어서 작업 이어서 불가