Guest User

recordreplay

a guest
Aug 4th, 2016
78
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

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×