diff --git a/definitions.h b/definitions.h
deleted file mode 100644
index d378089..0000000
--- a/definitions.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#ifndef DEFINITION_H_
-#define DEFINITION_H_
-
-//Vacuum Pressure Sensor: HX710B
-#define PIN_PRESSURE1_OUT 20
-#define PIN_PRESSURE2_OUT 21
-#define PIN_PRESSURE3_OUT 22
-#define PIN_PRESSURE_SCK 15
-
-#define PIN_INPUTARM1 0
-#define PIN_INPUTARM2 1
-#define PIN_INPUTARM3 2
-#define PIN_INPUTAUX 3
-
-//Pins Connected to ULN2803
-#define PIN_OUT1 6 //PWM
-#define PIN_OUT2 7 //PWM
-#define PIN_OUT3 8 //PWM
-#define PIN_OUT4 9 //PWM
-#define PIN_OUT5 16 //Digital
-#define PIN_OUT6 17 //Digital
-#define PIN_OUT7 18 //Digital
-#define PIN_OUT8 19 //Digital
-
-#define PIN_BUTTON 23
-
-#define PIN_PUMP1 PIN_OUT1
-#define PIN_PUMP2 PIN_OUT3
-
-#define PIN_VALVE1 PIN_OUT5
-#define PIN_VALVE2 PIN_OUT7
-
-
-
-#endif
\ No newline at end of file
diff --git a/HX710B.cpp b/prosthesis_controller/HX710B.cpp
similarity index 100%
rename from HX710B.cpp
rename to prosthesis_controller/HX710B.cpp
diff --git a/HX710B.h b/prosthesis_controller/HX710B.h
similarity index 100%
rename from HX710B.h
rename to prosthesis_controller/HX710B.h
diff --git a/prosthesis_controller/definitions.h b/prosthesis_controller/definitions.h
new file mode 100644
index 0000000..762fd8d
--- /dev/null
+++ b/prosthesis_controller/definitions.h
@@ -0,0 +1,37 @@
+#ifndef DEFINITION_H_
+#define DEFINITION_H_
+
+//Vacuum Pressure Sensor: HX710B
+#define PIN_PRESSURE1_OUT 20
+#define PIN_PRESSURE2_OUT 21
+#define PIN_PRESSURE3_OUT 22
+#define PIN_PRESSURE_SCK 15
+
+#define PIN_INPUTARM1 0
+#define PIN_INPUTARM2 1
+#define PIN_INPUTARM3 2
+#define PIN_INPUTAUX 3
+
+//Pins Connected to ULN2803
+#define PIN_OUT1 19 //Digital
+#define PIN_OUT2 18 //Digital
+#define PIN_OUT3 17 //Digital
+#define PIN_OUT4 16 //Digital
+#define PIN_OUT5 6 //PWM
+#define PIN_OUT6 7 //PWM
+#define PIN_OUT7 8 //PWM
+#define PIN_OUT8 9 //PWM
+
+#define PIN_BUTTON 23
+
+#define PIN_PUMP1 PIN_OUT5
+#define PIN_PUMP2 PIN_OUT7
+
+#define PIN_VALVE1 PIN_OUT1
+#define PIN_VALVE2 PIN_OUT3
+
+#define PIN_VIBRATION PIN_OUT2
+
+
+
+#endif
\ No newline at end of file
diff --git a/prosthesis_controller.ino b/prosthesis_controller/prosthesis_controller.ino
similarity index 87%
rename from prosthesis_controller.ino
rename to prosthesis_controller/prosthesis_controller.ino
index a549c46..286da12 100644
--- a/prosthesis_controller.ino
+++ b/prosthesis_controller/prosthesis_controller.ino
@@ -5,6 +5,9 @@
 void setup() {
   Serial.begin(115200);
 
+  pinMode(PIN_VIBRATION,OUTPUT);
+  analogWrite(PIN_VIBRATION,0);
+
   pinMode(PIN_BUTTON,INPUT_PULLDOWN);
 
   initVacuum();
@@ -12,6 +15,8 @@ void setup() {
 
   pinMode(PIN_INPUTARM1,INPUT);
 
+  pinMode(PIN_INPUTAUX,INPUT);
+
   initServos();
 }
 
diff --git a/servo.cpp b/prosthesis_controller/servo.cpp
similarity index 92%
rename from servo.cpp
rename to prosthesis_controller/servo.cpp
index 4aee0f3..4a6909f 100644
--- a/servo.cpp
+++ b/prosthesis_controller/servo.cpp
@@ -103,13 +103,17 @@ void loopServos(unsigned long millis) {
   servos[0].angle=sin(millis/1000.0/1.0)*20;
   servos[1].angle=cos(millis/1000.0/1.0)*20;
   servos[2].angle=sin(millis/1000.0/1.0)*20;
-
+  static uint8_t servoUpdateI=0; //only one servo per function call
   static unsigned long last_servo_update=0;
-  if (millis-last_servo_update>SERVO_UPDATE_INTERVAL){
+  if (millis-last_servo_update>SERVO_UPDATE_INTERVAL/SERVO_COUNT){
     last_servo_update=millis;
-    for (uint8_t i=0;i<SERVO_COUNT;i++) {
-      dxl.setGoalPosition(servos[i].id, servos[i].offset_angle+servos[i].angle*servos[i].orientation, UNIT_DEGREE);
-    }
+    //for (uint8_t i=0;i<SERVO_COUNT;i++) {
+      //dxl.setGoalPosition(servos[i].id, servos[i].offset_angle+servos[i].angle*servos[i].orientation, UNIT_DEGREE);
+    //}
+    
+    dxl.setGoalPosition(servos[servoUpdateI].id, servos[servoUpdateI].offset_angle+servos[servoUpdateI].angle*servos[servoUpdateI].orientation, UNIT_DEGREE);
+    servoUpdateI++;
+    servoUpdateI%=SERVO_COUNT;
   }
   
 }
diff --git a/servo.h b/prosthesis_controller/servo.h
similarity index 100%
rename from servo.h
rename to prosthesis_controller/servo.h
diff --git a/vacuum.cpp b/prosthesis_controller/vacuum.cpp
similarity index 100%
rename from vacuum.cpp
rename to prosthesis_controller/vacuum.cpp
diff --git a/vacuum.h b/prosthesis_controller/vacuum.h
similarity index 100%
rename from vacuum.h
rename to prosthesis_controller/vacuum.h