Servo Recorder

servoKey.ino

/*  Servo Recorder

The following commands are supported:

  a records the current servo position for 5 seconds
  b releases the servo to record the position
  c plays back the recorded position for 5 seconds
  d calibrates all angles from 0...180

The minimum and maximum pulse width will vary with different servo models.  
If the calibration routine does not work correctly then look up the minimum 
and maximum pulse widths for your servo and use them in the servo.attach 
lines like so:

  servo.attach(9,minimumPulseWidth,maximumPulseWidth);

This code requires a servo hack. Watch this for more info: http://youtu.be/ygX7sQM2gq0

*/

#include <Servo.h>

const int FEEDBACKPIN = A0;
const int SERVOPIN = 3;
const int MINP = 800;
const int MAXP = 2600;
const int FRAMES = 150;  //  30 frames a second for 5 seconds
const int SMOOTH = 50;

Servo servo;
int angle = 90;
int position = 90;
int final = 90;

int trimpot[500];
int posi[FRAMES];

void setup(){
  Serial.begin(9600);
}

void loop(){
  if ( Serial.available()) {
    char ch = Serial.read();
    switch(ch) {
      
      //  record
      case 'a':
        servo.detach();    
        Serial.println("Recording positions");
        for( int i=0; i<FRAMES; i++){
          posi[i]=0;
          //delay for 33ms, but and average value every 3ms.
          for( int j=0; j<11; j++){
            posi[i]=posi[i]+analogRead(FEEDBACKPIN);
            delay(3);
          }
          posi[i]=posi[i]/11;
        }
        Serial.println("Done.");
        break;
      //  end of record
        
      //  release
      case 'b':
        Serial.println("Stop");
        servo.detach();    
        break;
      //  end of release
      
      //  play
      case 'c':
        servo.attach(SERVOPIN,MINP,MAXP);  
        Serial.println("Playing positions");
        for( int i=0; i<FRAMES; i++){
          final=posi[i];      
          angle = trimpot[final];
          servo.write(angle);
          delay(33);
        }
        Serial.println("Done.");
        break;
      //  end of play
        
      //  calibrate
      case 'd' :   
        Serial.println("Calibrating");
        servo.attach(SERVOPIN,MINP,MAXP);
        servo.write(1);
        delay(1000);  // wait 1 second for servo to reach the position   
        for( int i=1; i<=180; i++ ){
          servo.write(i);
          delay(50);
          angle=0;
          for( int j=0; j<SMOOTH; j++){
            angle=angle+analogRead(FEEDBACKPIN);
            delay(5);
          }
          angle=angle/SMOOTH;
          Serial.print(angle);
          trimpot[angle] = i;
          Serial.print(", ");
        }
        int a=1;
        for( int i=0; i<500; i++ ){
          if ( trimpot[i] == 0 )
            trimpot[i]=a;
          else
            a=trimpot[i];
        }
        Serial.println();
        Serial.println("Done.");
        break;  
      //  end of calibrate
      
    }
  }
}