SHARE
TWEET

recordreplay

a guest Aug 4th, 2016 51 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <LinkedList.h>
  2. #include <Servo.h>
  3.  
  4. // potentiometer pins
  5. #define BASE_POT_PIN A0
  6. #define SHOULDER_POT_PIN A2
  7. #define SHOULDER2_POT_PIN A3
  8. #define ELBOW_POT_PIN A1
  9.  
  10. // servo pins
  11. #define BASE_SERVO_PIN 6
  12. #define SHOULDER_SERVO_PIN 8
  13. #define SHOULDER2_SERVO_PIN 7
  14. #define ELBOW_SERVO_PIN 9
  15.  
  16. // motion constants
  17. #define MOTION_DELAY 15
  18.  
  19. // servo objects
  20. Servo base_servo, shoulder_servo, shoulder2_servo, elbow_servo;
  21.  
  22. // history of servo positions
  23. LinkedList<int> *base_history = new LinkedList<int>();
  24. LinkedList<int> *shoulder_history = new LinkedList<int>();
  25. LinkedList<int> *shoulder2_history = new LinkedList<int>();
  26. LinkedList<int> *elbow_history = new LinkedList<int>();
  27.  
  28. /*
  29.  * Records servo motion and inserts into a history.
  30.  * Assumes servo has been detached already.
  31.  */
  32. void record(int potPin, int potMin, int potMax, Servo servo, LinkedList<int>& history) {
  33.   int reading = map(analogRead(potPin), potMin, potMax, 0, 180);
  34.   history.add(reading);
  35.   delay(10);
  36.   return;
  37. }
  38.  
  39. /*
  40.  * Replays servo motion based on history.
  41.  * Assumes servo has been attached to a pin already.
  42.  */
  43. void play(Servo servo, LinkedList<int>& history) {
  44.   int value = history.shift();
  45.   servo.write(value);
  46.   Serial.println(value);
  47.   delay(MOTION_DELAY);
  48.   return;
  49. }
  50.  
  51. /*
  52.  * Detaches all servos.
  53.  */
  54. void detachAll() {
  55.   base_servo.detach();
  56.   shoulder_servo.detach();
  57.   shoulder2_servo.detach();
  58.   elbow_servo.detach();
  59.   delay(100);
  60. }
  61.  
  62. /*
  63.  * Attaches all servos.
  64.  */
  65. void attachAll() {
  66.   base_servo.attach(BASE_SERVO_PIN);
  67.   shoulder_servo.attach(SHOULDER_SERVO_PIN);
  68.   shoulder2_servo.attach(SHOULDER2_SERVO_PIN);
  69.   elbow_servo.attach(ELBOW_SERVO_PIN);
  70.   delay(100);
  71. }
  72.  
  73. void setup() {
  74.   Serial.begin(9600);
  75.   attachAll();
  76.   Serial.println("---   START   ---");
  77. }
  78.  
  79. void loop() {
  80.  
  81.   // record all servo motions
  82.   detachAll();
  83.   Serial.println("---   NOW RECORDING ---");
  84.   while(Serial.available() == 0) {
  85.     record(BASE_POT_PIN, 84, 415, base_servo, *base_history);
  86.     record(SHOULDER_POT_PIN, 55, 401, shoulder_servo, *shoulder_history);
  87.     record(SHOULDER2_POT_PIN, 55, 401, shoulder2_servo, *shoulder2_history);
  88.     record(ELBOW_POT_PIN, 68, 373, elbow_servo, *elbow_history);
  89.   }
  90.  
  91.   attachAll();
  92.   Serial.println("---   NOW PLAYING   ---");
  93.   while(base_history->size() > 0) {
  94.     Serial.println(base_history->size());
  95.     play(base_servo, *base_history);
  96.     play(shoulder_servo, *shoulder_history);
  97.     play(shoulder2_servo, *shoulder2_history);
  98.     play(elbow_servo, *elbow_history);
  99.   }
  100.  
  101.   Serial.println("---   FINISH   ---");
  102.   while(1==1) {
  103.     Serial.println("---   DONE   ---");
  104.   }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top