Robot arm record and playback on an Arduino Uno

// Robot arm record and playback: teach the arm a routine with four knobs,
// save each pose to EEPROM with a button, then play the routine back.
//
//   arm      BASE pin 3, SHOULDER pin 5, ELBOW pin 6, GRIP pin 9
//            VCC and GND from a separate 5 V supply, GND joined to the Uno's
//   knobs    base A0, shoulder A1, elbow A2, claw A3
//   buttons  RECORD on pin 2, PLAY on pin 4, each to GND
//
// HOW TO USE IT
//   - Turn the knobs: the arm follows them.
//   - Press RECORD to save the pose the arm is in. Up to 20 poses.
//   - Press PLAY: the arm glides through the saved poses, over and over.
//     Press PLAY again to stop and go back to the knobs.
//   - Hold RECORD for two seconds to forget every pose.
//
// WHY EEPROM. A variable lives in RAM and is gone when the board resets or
// loses power. The Uno's ATmega328P also has 1024 bytes of EEPROM, which
// keeps its bytes with the power off, so a taught routine is still there
// tomorrow. The layout here:
//
//   byte 0             how many poses are saved
//   bytes 1 to 4       pose 1: base, shoulder, elbow, claw, one byte each
//   bytes 5 to 8       pose 2, and so on
//
// A brand new chip reads 255 in every byte, so a count over 20 means
// "nothing saved yet". EEPROM.update() only writes a byte that changed,
// because each byte is good for about 100,000 writes.

#include <Servo.h>
#include <EEPROM.h>

Servo base, shoulder, elbow, claw;
Servo *joints[4] = {&base, &shoulder, &elbow, &claw};
const int SERVO_PINS[4] = {3, 5, 6, 9};
const int KNOBS[4] = {A0, A1, A2, A3};

// How far each joint may go: the claw is shut at 20 and wide open at 120.
const int LOW_END[4] = {0, 40, 40, 20};
const int HIGH_END[4] = {180, 160, 160, 120};

const int RECORD = 2;
const int PLAY = 4;

const int MAX_POSES = 20;
const int COUNT_ADDRESS = 0;

int pose[4] = {90, 90, 90, 70};  // where the arm is now

int savedCount() {
  int count = EEPROM.read(COUNT_ADDRESS);
  return count > MAX_POSES ? 0 : count;  // 255 on a fresh chip
}

int poseAddress(int index) { return 1 + index * 4; }

void moveTo(const int target[4]) {
  for (int j = 0; j < 4; j++) {
    pose[j] = target[j];
    joints[j]->write(pose[j]);
  }
}

void printPose(const char *what, int number) {
  Serial.print(what);
  Serial.print(' ');
  Serial.print(number);
  Serial.print(":  base ");
  Serial.print(pose[0]);
  Serial.print("  shoulder ");
  Serial.print(pose[1]);
  Serial.print("  elbow ");
  Serial.print(pose[2]);
  Serial.print("  claw ");
  Serial.println(pose[3]);
}

void savePose() {
  int count = savedCount();
  if (count == MAX_POSES) {
    Serial.println("memory full: hold RECORD for 2 s to clear it");
    return;
  }
  for (int j = 0; j < 4; j++) EEPROM.update(poseAddress(count) + j, pose[j]);
  EEPROM.update(COUNT_ADDRESS, count + 1);
  printPose("saved pose", count + 1);
}

// A button press: true once per press, after the contacts stop bouncing.
bool pressed(int pin) {
  if (digitalRead(pin) == HIGH) return false;
  delay(20);
  return digitalRead(pin) == LOW;
}

// Wait for a button to be let go, and say how long it was held, in ms.
unsigned long heldFor(int pin) {
  unsigned long start = millis();
  while (digitalRead(pin) == LOW) delay(5);
  return millis() - start;
}

// Glide from where the arm is to a pose, in 40 small steps of 20 ms.
// Returns false if PLAY was pressed on the way, to stop playback.
bool glideTo(const int target[4]) {
  int from[4] = {pose[0], pose[1], pose[2], pose[3]};
  for (int step = 1; step <= 40; step++) {
    int now[4];
    for (int j = 0; j < 4; j++) now[j] = from[j] + (target[j] - from[j]) * step / 40;
    moveTo(now);
    delay(20);
    if (pressed(PLAY)) {
      heldFor(PLAY);
      return false;
    }
  }
  return true;
}

void playBack() {
  int count = savedCount();
  if (count == 0) {
    Serial.println("nothing saved yet: press RECORD first");
    return;
  }
  Serial.print("playing ");
  Serial.print(count);
  Serial.println(" poses: press PLAY to stop");
  while (true) {
    for (int i = 0; i < count; i++) {
      int target[4];
      for (int j = 0; j < 4; j++) target[j] = EEPROM.read(poseAddress(i) + j);
      if (!glideTo(target)) {
        Serial.println("stopped: the knobs have the arm again");
        return;
      }
      printPose("pose", i + 1);
      delay(300);
    }
  }
}

void setup() {
  pinMode(RECORD, INPUT_PULLUP);
  pinMode(PLAY, INPUT_PULLUP);
  for (int j = 0; j < 4; j++) {
    joints[j]->attach(SERVO_PINS[j]);
    joints[j]->write(pose[j]);
  }
  Serial.begin(115200);
  Serial.print("record and playback arm: ");
  Serial.print(savedCount());
  Serial.println(" poses in EEPROM");
}

void loop() {
  // The knobs drive the arm.
  int target[4];
  for (int j = 0; j < 4; j++) {
    target[j] = map(analogRead(KNOBS[j]), 0, 1023, LOW_END[j], HIGH_END[j]);
  }
  moveTo(target);

  if (pressed(RECORD)) {
    if (heldFor(RECORD) >= 2000) {
      EEPROM.update(COUNT_ADDRESS, 0);
      Serial.println("cleared every pose");
    } else {
      savePose();
    }
  }
  if (pressed(PLAY)) {
    heldFor(PLAY);
    playBack();
  }
  delay(20);
}