recordreplay

#include <LinkedList.h>
#include <Servo.h>

// potentiometer pins
#define BASE_POT_PIN A0
#define SHOULDER_POT_PIN A2
#define SHOULDER2_POT_PIN A3
#define ELBOW_POT_PIN A1

// servo pins
#define BASE_SERVO_PIN 6
#define SHOULDER_SERVO_PIN 8
#define SHOULDER2_SERVO_PIN 7
#define ELBOW_SERVO_PIN 9

// motion constants
#define MOTION_DELAY 15

// servo objects
Servo base_servo, shoulder_servo, shoulder2_servo, elbow_servo;

// history of servo positions
LinkedList<int> *base_history = new LinkedList<int>();
LinkedList<int> *shoulder_history = new LinkedList<int>();
LinkedList<int> *shoulder2_history = new LinkedList<int>();
LinkedList<int> *elbow_history = new LinkedList<int>();

/*
 * Records servo motion and inserts into a history.
 * Assumes servo has been detached already.
 */
void record(int potPin, int potMin, int potMax, Servo servo, LinkedList<int>& history) {
  int reading = map(analogRead(potPin), potMin, potMax, 0, 180);
  history.add(reading);
  delay(10);
  return;
}

/*
 * Replays servo motion based on history.
 * Assumes servo has been attached to a pin already.
 */
void play(Servo servo, LinkedList<int>& history) {
  int value = history.shift();
  servo.write(value);
  Serial.println(value);
  delay(MOTION_DELAY);
  return;
}

/*
 * Detaches all servos.
 */
void detachAll() {
  base_servo.detach();
  shoulder_servo.detach();
  shoulder2_servo.detach();
  elbow_servo.detach();
  delay(100);
}

/*
 * Attaches all servos.
 */
void attachAll() {
  base_servo.attach(BASE_SERVO_PIN);
  shoulder_servo.attach(SHOULDER_SERVO_PIN);
  shoulder2_servo.attach(SHOULDER2_SERVO_PIN);
  elbow_servo.attach(ELBOW_SERVO_PIN);
  delay(100);
}

void setup() {
  Serial.begin(9600);
  attachAll();
  Serial.println("---   START   ---");
}

void loop() {

  // record all servo motions
  detachAll();
  Serial.println("---   NOW RECORDING ---");
  while(Serial.available() == 0) {
    record(BASE_POT_PIN, 84, 415, base_servo, *base_history);
    record(SHOULDER_POT_PIN, 55, 401, shoulder_servo, *shoulder_history);
    record(SHOULDER2_POT_PIN, 55, 401, shoulder2_servo, *shoulder2_history);
    record(ELBOW_POT_PIN, 68, 373, elbow_servo, *elbow_history);
  }

  attachAll();
  Serial.println("---   NOW PLAYING   ---");
  while(base_history->size() > 0) {
    Serial.println(base_history->size());
    play(base_servo, *base_history);
    play(shoulder_servo, *shoulder_history);
    play(shoulder2_servo, *shoulder2_history);
    play(elbow_servo, *elbow_history);
  }

  Serial.println("---   FINISH   ---");
  while(1==1) {
    Serial.println("---   DONE   ---");
  }