//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