/*
 Controlling a servo position using a potentiometer (variable resistor)
 by Michal Rinott <http://people.interaction-ivrea.it/m.rinott>

 modified on 8 Nov 2013
 by Scott Fitzgerald
 http://www.arduino.cc/en/Tutorial/Knob
*/
#include <Servo.h>

Servo myservo1;
Servo myservo2;

int switch1;
float x = 0.05;
int pot1;
int pot2;
int pause;
int spd;
int val1;
int val2;
float switch1Smoothed;
float switch1Prev;

void setup() {

  myservo1.attach(9);  // upper servos
  myservo2.attach(10);  // lower servos

 //Serial.begin(115200);

}

void loop() {

  int pot1 = analogRead(A0);
  spd = pot1*0.08;
  if (spd <= 5) {
    spd = 5;
  }
  int pot2 = analogRead(A3);
  pause = pot2*0.2;

  switch1 = 500;

  for (int i = 0; i <= pause; i++) {

  switch1Smoothed = (switch1 * x) + (switch1Prev * (1-x));
  switch1Prev = switch1Smoothed;

  val1 = switch1Prev;            // reads the value of the potentiometer (value between 0 and 1023)
  val1 = map(val1, 0, 1023, 0, 180);     // scale it to use it with the servo (value between 0 and 180)
  val2 = switch1Prev;
  val2 = map(val2, 0, 1023, 180,0);
  myservo1.write(val1);                  // sets the servo position according to the scaled value 
  myservo2.write(val2);

  delay(spd);
  }

  for (int j = 0; j <= pause; j++) {

  switch1 = 0;

  switch1Smoothed = (switch1 * x) + (switch1Prev * (1-x));
  switch1Prev = switch1Smoothed;

  val1 = switch1Prev;            // reads the value of the potentiometer (value between 0 and 1023)
  val1 = map(val1, 0, 1023, 0, 180);     // scale it to use it with the servo (value between 0 and 180)
  val2 = switch1Prev;
  val2 = map(val2, 0, 1023, 180, 0);
  myservo1.write(val1);                  // sets the servo position according to the scaled value 
  myservo2.write(val2);

  delay(spd);
  }


}