[P]uffer Fish.Fab

//biodyn_mechanics_sketch - Smoothing Created 22 April 2007
//Servo 360 Hack Code developed by Luis Quinones + Michael Mccune on July 16, 2010
//Servo 360 Hack Code written by Luis Quinones on July 18, 2010
//SERVO SETUPS/////////////////////////////////////////////
//#include  
//Servo myservo1;
//VARIABLES//////////////////////////////////////////////////////

const int numReadings = 1;  // number of readings to smooth out (average)
int readings[numReadings];  //reading from the analog input
int index = 0;  //index of the current reading
int total = 0;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Sensor[s]//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const int buttonPin = 53;  //pin which contains button
int sensorA = 0;  // analog pin used to connect the potentiometer
int sensorB = 1;  // analog pin used to connect the potentiometer
int sensorC = 2;  // analog pin used to connect the potentiometer
int sensorD = 3;  // analog pin used to connect the potentiometer
int sensorE = 4;  // analog pin used to connect the potentiometer
int sensorF = 5;  // analog pin used to connect the potentiometer
int sensorG = 6;  // analog pin used to connect the potentiometer
int sensorH = 7;  // analog pin used to connect the potentiometer
int sensorI = 8;  // analog pin used to connect the potentiometer
int sensorJ = 9;  // analog pin used to connect the potentiometer
int sensorK = 10;  // analog pin used to connect the potentiometer
int sensorL = 11;  // analog pin used to connect the potentiometer
int sensorM = 12;  // analog pin used to connect the potentiometer
int sensorN = 13;  // analog pin used to connect the potentiometer
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//STARTING POSITIONS//
int currentPos = 0;  //average of sample variables set to 0
int avrgPos = 0; 
int constrPos = 0;
int pastPos = 0;  //set pastPos to 0
int buttonState = 0; //set button state to off
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//SERVO PINS//
int servoPin1 = 23;
int servoPin2 = 24;
int servoPin3 = 25;
int servoPin4 = 26;
int servoPin5 = 27;
int servoPin6 = 28;
int servoPin7 = 29;
int servoPin8 = 30;
int servoPin9 = 31;
int servoPin10 = 32;
int servoPin11 = 33;
int servoPin12 = 34;
int servoPin13 = 35;
//int servoPin14 = 15;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//SETUP//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup() 
{ 
  Serial.begin(9600);   //initialize serial communication with computer:
 pinMode(servoPin1,OUTPUT);  //set the pin as OUTPUT
 pinMode(servoPin2,OUTPUT);  //set the pin as OUTPUT
 pinMode(servoPin3,OUTPUT);  //set the pin as OUTPUT
 pinMode(servoPin4,OUTPUT);  //set the pin as OUTPUT
 pinMode(servoPin5,OUTPUT);  //set the pin as OUTPUT
 pinMode(servoPin6,OUTPUT);  //set the pin as OUTPUT
 pinMode(servoPin7,OUTPUT);  //set the pin as OUTPUT
 pinMode(servoPin8,OUTPUT);  //set the pin as OUTPUT
 pinMode(servoPin9,OUTPUT);  //set the pin as OUTPUT
 pinMode(servoPin10,OUTPUT);  //set the pin as OUTPUT
 pinMode(servoPin11,OUTPUT);  //set the pin as OUTPUT
 pinMode(servoPin12,OUTPUT);  //set the pin as OUTPUT
 pinMode(servoPin13,OUTPUT);  //set the pin as OUTPUT
 //pinMode(servoPin14,OUTPUT);  //set the pin as OUTPUT
    pinMode(buttonPin,INPUT);  //set button as input
  for(int thisReading = 0; thisReading < numReadings; thisReading++)
    readings[thisReading] = 0;  //initialize all the readings to 0:
} 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Loops//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop()
{ 
    buttonState = digitalRead(buttonPin); 
    if (buttonState == LOW){
      Serial.print("TURN SWITCH TO START SYSTEM_: ");  //setting label to inform what information is being printed
      Serial.print("n");
    }
    else{
     // int direction1 = 0;  
     // digitalWrite(servoPin1,LOW);  //setting initial state of servo as OFF
      total = total - readings[index];  //substract the last reading
      readings[index] = analogRead(sensorI + sensorA + sensorC + sensorB + sensorD - sensorE + sensorF);  //read from the sensor  
      total = total + readings[index];  //add the reading to the total
      index = index + 1;  //advance to the next position in the array
      if(index >= numReadings)
        index = 0;
      avrgPos = total/numReadings;  //averaging out sampling values
      constrPos = constrain(avrgPos,285,620);  //constrain incoming values
      currentPos = map(avrgPos, 285, 620, 0, 1000);  // scale analog sensor values to use it with the servo.
      /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
      int direction1 = currentPos - pastPos;  //subtracting the current position from the past position, this will set the direction.
      // @ this point pastPos = 0 in order to have a positive starting point in direction1
      float distScalar = 1; //scales the amount of travel by the servo - mostly optimized @ 1:41 am with value of 0.5
      //higher value makes the servo travel more revolutions
      float y = ((abs(direction1 * distScalar)));  //scaling down the incoming direction1 values(add absolute value to ensure
      //we always have a positive starting value
      int rndY = round(y)+1;  //rounding off float value to use as UpperBound of the loop (add one to create a higher value)
      /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
      for(int i = 0; i <= rndY; i++){  
        if(direction1 < 4 && direction1 > -4){  //setting condition to keep range of 4 to -4 at a moving value of 0 to make servo stop moving
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 1 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin1,LOW);  //setting initial state of servo as OFF
          delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin1,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 2 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin2,LOW);  //setting initial state of servo as OFF
          delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin2,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 3 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin3,LOW);  //setting initial state of servo as OFF
          delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin3,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 4 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin4,LOW);  //setting initial state of servo as OFF
          delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin4,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 5 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin5,LOW);  //setting initial state of servo as OFF
          delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin5,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 6 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin6,LOW);  //setting initial state of servo as OFF
          delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin6,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 7 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin7,LOW);  //setting initial state of servo as OFF
          delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin7,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 8 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin8,LOW);  //setting initial state of servo as OFF
          delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin8,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 9 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin9,LOW);  //setting initial state of servo as OFF
          delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin9,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 10 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin10,LOW);  //setting initial state of servo as OFF
          delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin10,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 11 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin11,LOW);  //setting initial state of servo as OFF
          delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin11,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 12 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin12,LOW);  //setting initial state of servo as OFF
          delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin12,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 13 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin13,LOW);  //setting initial state of servo as OFF
          delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin13,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 14 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //digitalWrite(servoPin14,LOW);  //setting initial state of servo as OFF
          //delayMicroseconds(direction1);  //pausing the program for amount of time ( delay = current Position - past Position
          //digitalWrite(servoPin14,LOW);  //keeping the end state of the servo as OFF
          //delay(50); 
        }
        else{
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 1 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin1,HIGH);  //setting initial state of servo as ON
          delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin1,LOW);  //keeping the end state of the servo as OFF
          //delay(15);
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 2 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin2,HIGH);  //setting initial state of servo as ON
          delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin2,LOW);  //keeping the end state of the servo as OFF
          //delay(15);
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 3 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin3,HIGH);  //setting initial state of servo as ON
          delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin3,LOW);  //keeping the end state of the servo as OFF
          //delay(15);
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 4 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin4,HIGH);  //setting initial state of servo as ON
          delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin4,LOW);  //keeping the end state of the servo as OFF
          //delay(15);
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 5 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin5,HIGH);  //setting initial state of servo as ON
          delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin5,LOW);  //keeping the end state of the servo as OFF
          //delay(15);
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 6 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin6,HIGH);  //setting initial state of servo as ON
          delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin6,LOW);  //keeping the end state of the servo as OFF
          //delay(15);
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 7 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin7,HIGH);  //setting initial state of servo as ON
          delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin7,LOW);  //keeping the end state of the servo as OFF
          //delay(15);
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 8 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin8,HIGH);  //setting initial state of servo as ON
          delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin8,LOW);  //keeping the end state of the servo as OFF
          //delay(15)
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 9 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin9,HIGH);  //setting initial state of servo as ON
          delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin9,LOW);  //keeping the end state of the servo as OFF
          //delay(15);
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 10 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin10,HIGH);  //setting initial state of servo as ON
          delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin10,LOW);  //keeping the end state of the servo as OFF
          //delay(15);
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 11 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin11,HIGH);  //setting initial state of servo as ON
          delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin11,LOW);  //keeping the end state of the servo as OFF
          //delay(15);
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 12 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin12,HIGH);  //setting initial state of servo as ON
          delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin12,LOW);  //keeping the end state of the servo as OFF
          //delay(15)
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 13 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          digitalWrite(servoPin13,HIGH);  //setting initial state of servo as ON
          delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          digitalWrite(servoPin13,LOW);  //keeping the end state of the servo as OFF
          //delay(15);
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //SERVO - 14 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          //digitalWrite(servoPin14,HIGH);  //setting initial state of servo as ON
          //delayMicroseconds(direction1*-1);  //pausing the program for amount of time ( delay = current Position - past Position
          //digitalWrite(servoPin14,LOW);  //keeping the end state of the servo as OFF
          //delay(15);
         
        }
        Serial.print("ServoPositionValue: ");  //setting label to inform what information is being printed
        Serial.print(direction1);  //direction1 = current Position - past Position
        Serial.print("n");
        //delay(25);
      }
      pastPos = currentPos;  //feeding the pastPosition w/ the currentPos
    }  
  }
// delay(15);  // waits for the servo to get there 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
  analogValue = analogRead(analogPin);      // read the analog input
 pulse = map(analogValue,0,1023,minPulse,maxPulse);    // convert the analog value
 // to a range between minPulse
 // and maxPulse.
 // pulse the servo again if rhe refresh time (20 ms) have passed:
 if (millis() - lastPulse >= refreshTime) {
 digitalWrite(servoPin, HIGH);   // Turn the motor on
 delayMicroseconds(pulse);       // Length of the pulse sets the motor position
 digitalWrite(servoPin, LOW);    // Turn the motor off
 lastPulse = millis();           // save the time of the last pulse
 */
Update