/*
 * Two servos with two potentiometer control
 * Theory and Practice of Tangible User Interfaces
 * October 11 2007
 *
 */

int servoPin1 = 7;      // Control pin for servo motor 1
int servoPin2 = 8;      // Control pin for servo motor 2
int potPin1   = 0;      // select the input pin for the potentiometer 1
int potPin2   = 1;      // select the input pin for the potentiometer 2

int pulseWidth1 = 0;    // Amount to pulse the servo 1
int pulseWidth2 = 0;    // Amount to pulse the servo 2
long lastPulse1 = 0;    // the time in millisecs of the last pulse of servo 1
long lastPulse2 = 0;    // the time in millisecs of the last pulse of servo 2
int val_1;              // variable used to store data from potentiometer 1
int val_2;              // variable used to store data from potentiometer 2

int refreshTime = 20;   // the time in millisecs needed in between pulses
int minPulse = 500;    // minimum pulse width

void setup() {
  pinMode(servoPin1, OUTPUT);  // Set servo pin 1 as an output pin
  pinMode(servoPin2, OUTPUT);  // Set servo pin 2 as an output pin
  pulseWidth1 = minPulse;      // Set the motor position to the minimum
  pulseWidth2 = minPulse;      // Set the motor position to the minimum
  Serial.begin(9600);          // connect to the serial port
  Serial.println("Two servos with two potentiometers ready");
}

void loop() {
  val_1 = analogRead(potPin1);    // read the value from the sensor 1, between 0 - 1024
  val_2 = analogRead(potPin2);    // read the value from the sensor 2, between 0 - 1024
    
  if (val_1 > 0 && val_1 <= 999 ) {
    pulseWidth1 = val_1*2 + minPulse;  // convert angle to microseconds
  }
  updateServo1();   // update servo 1 position

  if (val_2 > 0 && val_2 <= 999 ) {
    pulseWidth2 = val_2*2 + minPulse;  // convert angle to microseconds
  }
  updateServo2();   // update servo 2 position
}

void updateServo1() {
  if (millis() - lastPulse1 >= refreshTime) {
    digitalWrite(servoPin1, HIGH); 
    delayMicroseconds(pulseWidth1);  
    digitalWrite(servoPin1, LOW);   
    lastPulse1 = millis();          
  }
}

void updateServo2() {
  if (millis() - lastPulse2 >= refreshTime) {
    digitalWrite(servoPin2, HIGH);  
    delayMicroseconds(pulseWidth2);  
    digitalWrite(servoPin2, LOW);   
    lastPulse2 = millis(); 
  }
}