ذراع روبوتية بأربعة محاور

#include <Servo.h>

Servo base, shoulder, elbow, gripper;
const int POT[4] = {A0, A1, A2, A3};
const int BTN = 2;

int pose[60][4];
int poseCount = 0;
bool replaying = false;

void writeAll(int a, int b, int c, int d) {
  base.write(a); shoulder.write(b); elbow.write(c); gripper.write(d);
}

void setup() {
  base.attach(3); shoulder.attach(5); elbow.attach(6); gripper.attach(9);
  pinMode(BTN, INPUT_PULLUP);
  Serial.begin(9600);
}

void loop() {
  if (replaying) {
    for (int i = 0; i < poseCount; i++) {
      writeAll(pose[i][0], pose[i][1], pose[i][2], pose[i][3]);
      delay(400);
    }
    if (digitalRead(BTN) == LOW) { replaying = false; delay(500); }
    return;
  }

  int a = map(analogRead(POT[0]), 0, 1023, 0, 180);
  int b = map(analogRead(POT[1]), 0, 1023, 15, 165);
  int c = map(analogRead(POT[2]), 0, 1023, 15, 165);
  int d = map(analogRead(POT[3]), 0, 1023, 10, 90);
  writeAll(a, b, c, d);

  if (digitalRead(BTN) == LOW) {
    delay(50);
    unsigned long t = millis();
    while (digitalRead(BTN) == LOW) {}
    if (millis() - t > 800) {          // long press -> replay
      replaying = poseCount > 0;
    } else if (poseCount < 60) {       // short press -> record pose
      pose[poseCount][0] = a; pose[poseCount][1] = b;
      pose[poseCount][2] = c; pose[poseCount][3] = d;
      poseCount++;
      Serial.print("Saved pose "); Serial.println(poseCount);
    }
    delay(200);
  }
  delay(20);
}