[{"id":"10241dd4-9ae2-4d04-99f8-aa973266d003","type":"image","data":{"images":["https://storage.googleapis.com/copool-prod-bucket/08c5ac23-297b-4b09-bdfd-5c3a2eec3de9-%EC%8A%A4%ED%81%AC%EB%A6%B0%EC%83%B7%202025-08-21%20140127.png","https://storage.googleapis.com/copool-prod-bucket/770f51b4-587a-483f-935b-b939d7729eb8-%EC%8A%A4%ED%81%AC%EB%A6%B0%EC%83%B7%202025-08-21%20140300.png"]}},{"id":"af1563ba-93c3-4b79-8378-30a02deb64ae","type":"code","data":{"code":"#include // 서보(HuemonelabKit)\n#include // TFT SPI\n#include \n#include \n#include // fabsf\n\n// 커스텀 색 (RGB565) — 50% 회색\n#define COLOR_DARKGRAY 0x7BEF\n\n// ── TFT (ST7735 80x160) ─────────────────────────────\n#define TFT_CS 10\n#define TFT_DC 9\n#define TFT_RST 8\nAdafruit_ST7735 tft(TFT_CS, TFT_DC, TFT_RST);\n\n// ── 초음파 HC-SR04 ─────────────────────────────────\nconst int TRIG_PIN = 4; // BLK가 6이면 충돌 피하려고 TRIG=4 사용\nconst int ECHO_PIN = 5;\nconst unsigned long PERIOD_MS = 1000; // 1초마다 갱신\nunsigned long lastMeasureMs = 0;\n\n// ── 거리 표시 영역(부분 갱신) ──────────────────────\nconst int BOX_X = 0, BOX_Y = 0, BOX_W = 160, BOX_H = 22;\nfloat lastDrawnDistance = -999.f; // 마지막으로 화면에 그린 값\n\n// ── 서보 객체 ───────────────────────────────────────\nServoMotor servoGrip; // 1. 집게\nServoMotor servoArm1; // 2. 팔1(어깨)\nServoMotor servoArm2; // 3. 팔2(팔꿈치)\nServoMotor servoBase; // 4. 바닥 회전\n\n// ── 조이스틱 핀 ─────────────────────────────────────\nconst int JOY1_X = A0; // 팔1\nconst int JOY1_Y = A1; // 팔2\nconst int JOY2_X = A2; // 바닥 회전\nconst int JOY2_Y = A3; // 집게\n\n// ── 서보 핀 (TFT와 충돌 피해서 변경) ───────────────\nconst int PIN_GRIP = 7; // 그대로\nconst int PIN_ARM1 = 2; // (원래 8 → 2로 변경)\nconst int PIN_ARM2 = 3; // (원래 9 → 3으로 변경)\nconst int PIN_BASE = A4; // (원래 10 → A4; A5로 바꿔도 됨)\n\n// ── 설정값 ─────────────────────────────────────────\nconst int DEAD_BAND = 20;\nconst int SMOOTH = 4;\n\nint minDeg[4] = { 10, 10, 10, 0}; // Grip, Arm1, Arm2, Base\nint maxDeg[4] = {170, 170, 170, 180};\nbool invert[4] = {false, false, false, false};\n\nint targetDeg[4] = {90, 90, 90, 90};\nint currentDeg[4] = {90, 90, 90, 90};\n\n// ── 유틸(조이스틱 → 각도) ─────────────────────────\nint joyToDeg(int raw, int which) {\n int centered = raw - 512;\n if (abs(centered) < DEAD_BAND) centered = 0;\n long mapped = map(centered, -512, 512, 0, 180);\n if (invert[which]) mapped = 180 - mapped;\n mapped = constrain(mapped, minDeg[which], maxDeg[which]);\n return (int)mapped;\n}\n\nint smoothMove(int current, int target) {\n if (SMOOTH <= 0) return target;\n return current + (target - current) / SMOOTH;\n}\n\n// ── HC-SR04: 거리 1회 측정(cm) ─────────────────────\nfloat readDistanceCM() {\n digitalWrite(TRIG_PIN, LOW); delayMicroseconds(2);\n digitalWrite(TRIG_PIN, HIGH); delayMicroseconds(10);\n digitalWrite(TRIG_PIN, LOW);\n unsigned long dur = pulseIn(ECHO_PIN, HIGH, 30000UL); // ~5m\n if (dur == 0) return -1.0f;\n return (dur * 0.0343f) / 2.0f; // 왕복 → /2 (cm)\n}\n\n// ── 간단 안정화: 3회 평균 ──────────────────────────\nfloat measureAveraged() {\n float s = 0; int n = 0;\n for (int i=0; i<3; i++) {\n float d = readDistanceCM();\n if (d >= 0) { s += d; n++; }\n delay(30); // 잔향 방지 간격\n }\n return (n == 0) ? -1.0f : (s / n);\n}\n\n// ── TFT: 고정 UI 1회 그리기 ────────────────────────\nvoid drawStaticUI() {\n tft.fillScreen(ST77XX_BLACK);\n tft.setTextWrap(false);\n tft.setTextColor(ST77XX_WHITE);\n tft.setTextSize(1);\n tft.setCursor(4, 4);\n tft.println(\"Ultrasonic Distance\");\n tft.drawRect(BOX_X, BOX_Y, BOX_W, BOX_H, COLOR_DARKGRAY);\n}\n\n// ── TFT: 숫자만 부분 갱신(깜빡임 최소화) ───────────\nvoid drawDistance(float d) {\n bool changed = false;\n if (d < 0 && lastDrawnDistance >= 0) changed = true;\n else if (d >= 0 && (fabsf(d - lastDrawnDistance) >= 0.05f)) changed = true;\n if (!changed) return;\n\n tft.fillRect(BOX_X+1, BOX_Y+1, BOX_W-2, BOX_H-2, ST77XX_BLACK);\n tft.setTextSize(2);\n tft.setCursor(6, 2);\n if (d < 0) {\n tft.setTextColor(ST77XX_YELLOW);\n tft.print(\"Timeout\");\n } else {\n tft.setTextColor(ST77XX_WHITE);\n tft.print(d, 1);\n tft.print(\" cm\");\n }\n lastDrawnDistance = (d < 0) ? -1.0f : d;\n}\n\nvoid setup() {\n // ── 서보 ──\n servoGrip.attach(PIN_GRIP);\n servoArm1.attach(PIN_ARM1);\n servoArm2.attach(PIN_ARM2);\n servoBase.attach(PIN_BASE);\n\n pinMode(JOY1_X, INPUT);\n pinMode(JOY1_Y, INPUT);\n pinMode(JOY2_X, INPUT);\n pinMode(JOY2_Y, INPUT);\n\n servoGrip.write(currentDeg[0]);\n servoArm1.write(currentDeg[1]);\n servoArm2.write(currentDeg[2]);\n servoBase.write(currentDeg[3]);\n\n // ── 초음파 ──\n pinMode(TRIG_PIN, OUTPUT);\n pinMode(ECHO_PIN, INPUT);\n digitalWrite(TRIG_PIN, LOW);\n\n // ── TFT ──\n tft.initR(INITR_MINI160x80); // 또는 INITR_160x80\n tft.setRotation(1);\n drawStaticUI();\n drawDistance(-1); // 초기 표시\n}\n\nvoid loop() {\n // 1) 조이스틱 → 서보\n int j1x = analogRead(JOY1_X);\n int j1y = analogRead(JOY1_Y);\n int j2x = analogRead(JOY2_X);\n int j2y = analogRead(JOY2_Y);\n\n targetDeg[1] = joyToDeg(j1x, 1); // Arm1\n targetDeg[2] = joyToDeg(j1y, 2); // Arm2\n targetDeg[3] = joyToDeg(j2x, 3); // Base\n targetDeg[0] = joyToDeg(j2y, 0); // Grip\n\n for (int i = 0; i < 4; i++) currentDeg[i] = smoothMove(currentDeg[i], targetDeg[i]);\n\n servoGrip.write(currentDeg[0]);\n servoArm1.write(currentDeg[1]);\n servoArm2.write(currentDeg[2]);\n servoBase.write(currentDeg[3]);\n\n // 2) 1초마다 거리 측정 → TFT 갱신\n unsigned long now = millis();\n if (now - lastMeasureMs >= PERIOD_MS) {\n lastMeasureMs = now;\n float d = measureAveraged(); // 3회 평균\n drawDistance(d);\n }\n\n delay(15); // 서보 안정화\n}\n","language":"c"}}]

코풀 커뮤니티

의수 보고서 양삼렬