#include <Servo.h>

int pot1 = 500;
int pot2 = 500;
int pot3 = 500;
int pot4 = 500;
int pot5 = 500;
int pot6 = 500;

float pot1Scaled;
float pot2Scaled;
float pot3Scaled;
float pot4Scaled;
float pot4aScaled;
float pot5Scaled;
float pot6Scaled;

float pot1Smoothed = 500;
float pot2Smoothed = 500;
float pot3Smoothed = 500;
float pot4Smoothed = 500;
float pot5Smoothed = 500;
float pot6Smoothed = 500;

float pot1SmoothedPrev = 500;
float pot2SmoothedPrev = 500;
float pot3SmoothedPrev = 500;
float pot4SmoothedPrev = 500;
float pot5SmoothedPrev = 500;
float pot6SmoothedPrev = 500;

unsigned long currentMillis;
long previousMillis = 0;    // set up timers
long interval = 5;        // time constant for timer

int stepFlag = 0;
long previousStepMillis = 0;

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;

int lval1c;
int lval2c;
int lval3c;
int lval4c;
int lval5c;
int lval6c;

void setup() {

  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
  pinMode(A4, INPUT);
  pinMode(A5, INPUT);

  Serial.begin(115200);

  servo1.attach(12);
  servo2.attach(11);
  servo3.attach(10);
  
  servo4.attach(9);
  servo5.attach(8);
  servo6.attach(7);
  
  /*
  servo1.writeMicroseconds(1500);
  servo2.writeMicroseconds(1500);
  servo3.writeMicroseconds(1500);
  
  servo4.writeMicroseconds(1500);
  servo5.writeMicroseconds(1500);
  servo6.writeMicroseconds(1500);
  */

}

void loop() {

  currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {  // start 5ms timed loop  
      previousMillis = currentMillis;

      // step sequencer

      if (stepFlag == 0 && currentMillis - previousStepMillis > 1000) {
       
        pot1 = 1000;
        pot2 = 0;
        pot3 = 1000;
        pot4 = 0;
        pot5 = 0;
        pot6 = 0;
        
        stepFlag = 1;           
        previousStepMillis = currentMillis;
      }

      else if (stepFlag == 1 && currentMillis - previousStepMillis > 1000) {
        
        pot1 = 1000;
        pot2 = 0;
        pot3 = 0;
        pot4 = 1000;
        pot5 = 0;
        pot6 = 0;
        
        stepFlag = 2;           
        previousStepMillis = currentMillis;
      }

      else if (stepFlag == 2 && currentMillis - previousStepMillis > 1000) {
        
        pot1 = 1000;
        pot2 = 1000;
        pot3 = 1000;
        pot4 = 0;
        pot5 = 0;
        pot6 = 0;
        
        stepFlag = 3;           
        previousStepMillis = currentMillis;
      }

      if (stepFlag == 3 && currentMillis - previousStepMillis > 1000) {
        
        pot1 = 1000;
        pot2 = 1000;
        pot3 = 1000;
        pot4 = 1000;
        pot5 = 0;
        pot6 = 0;
       
        stepFlag = 4;           
        previousStepMillis = currentMillis;
      }

      if (stepFlag == 4 && currentMillis - previousStepMillis > 1000) {
        
        pot1 = 1000;
        pot2 = 1000;
        pot3 = 1000;
        pot4 = 1000;
        pot5 = 1000;
        pot6 = 0;
        
        stepFlag = 5;           
        previousStepMillis = currentMillis;
      }
      if (stepFlag == 5 && currentMillis - previousStepMillis > 1000) {
        
        pot1 = 0;
        pot2 = 0;
        pot3 = 0;
        pot4 = 0;
        pot5 = 0;
        pot6 = 0;
        
        stepFlag = 1;           
        previousStepMillis = currentMillis;
      }

      // end of step sequencer

      
    
      // scale all pots for the servo microseconds range
      
      pot1Scaled = (pot1 - 500) + 1000;
      pot2Scaled = (pot2 - 500) + 1000;
      pot3Scaled = (pot3 - 500) + 1000;
      pot4Scaled = (pot4 - 500) + 1000;
      pot5Scaled = (pot5 - 500) + 1000;
      pot6Scaled = (pot6 - 500) + 1000;
    
     
      // smooth pots
      
      pot1Smoothed = (pot1Scaled * 0.05) + (pot1SmoothedPrev * 0.95);
      pot2Smoothed = (pot2Scaled * 0.01) + (pot2SmoothedPrev * 0.99);
      pot3Smoothed = (pot3Scaled * 0.01) + (pot3SmoothedPrev * 0.99);
      pot4Smoothed = (pot4Scaled * 0.01) + (pot4SmoothedPrev * 0.99);
      pot5Smoothed = (pot5Scaled * 0.01) + (pot5SmoothedPrev * 0.99);
      pot6Smoothed = (pot6Scaled * 0.01) + (pot6SmoothedPrev * 0.99);  
    
      // bookmark previous values
    
      pot1SmoothedPrev = pot1Smoothed;
      pot2SmoothedPrev = pot2Smoothed;
      pot3SmoothedPrev = pot3Smoothed;
      pot4SmoothedPrev = pot4Smoothed;
      pot5SmoothedPrev = pot5Smoothed;
      pot6SmoothedPrev = pot6Smoothed;
  
      
      lval1c = pot1Smoothed;
      lval2c = pot2Smoothed;
      lval3c = pot3Smoothed;
      lval4c = pot4Smoothed;
      lval5c = pot5Smoothed;
      lval6c = pot6Smoothed;
           
          lval1c = map(lval1c, 500, 1500, 90, 120);
          lval2c = map(lval2c, 500, 1500, 90, 120);
          lval3c = map(lval3c, 500, 1500, 90, 120);
          lval4c = map(lval4c, 500, 1500, 90, 120);
          lval5c = map(lval5c, 500, 1500, 90, 120);
          lval6c = map(lval6c, 500, 1500, 90, 120);
 
 } // end of timed loop
      servo1.write(lval1c);
      servo2.write(lval2c);
      servo3.write(lval3c);
      servo4.write(lval4c);      
      servo5.write(lval5c);
      servo6.write(lval6c); 


      Serial.print(pot1Smoothed);
      Serial.print(" , ");
      Serial.print(lval2c);
      Serial.print(" , ");
      Serial.print(lval3c);
      Serial.print(" , ");
      Serial.print(lval4c);
      Serial.print(" , ");
      Serial.print(lval5c);
      Serial.print(" , ");
      Serial.println(lval6c);
} // end if main loop