diff --git a/prosthesis_controller/SimplexNoise.cpp b/prosthesis_controller/SimplexNoise.cpp
new file mode 100644
index 0000000..63c4110
--- /dev/null
+++ b/prosthesis_controller/SimplexNoise.cpp
@@ -0,0 +1,114 @@
+/*
+===============================================================================
+Ported the SimplexNoise algorithm from the C++ versions mentioned below 
+to a reuseable Arduino Library.
+
+By Jordan Shaw / http://jordanshaw.com / 2017-02
+
+A C++ port of a speed-improved simplex noise algorithm for 2D in Java.
+Based on example code by Stefan Gustavson (stegu@itn.liu.se).
+Optimisations by Peter Eastman (peastman@drizzle.stanford.edu).
+Better rank ordering method by Stefan Gustavson in 2012.
+C++ port and minor type and algorithm changes by Josh Koch (jdk1337@gmail.com).
+This could be speeded up even further, but it's useful as it is.
+Version 2012-04-12
+The original Java code was placed in the public domain by its original author,
+Stefan Gustavson. You may use it as you see fit,
+but attribution is appreciated.
+
+Original gist url: https://gist.github.com/Slipyx/2372043
+
+===============================================================================
+*/
+
+#include <math.h>
+#include "Arduino.h"
+#include "SimplexNoise.h"
+
+// Private static member definitions
+const double SimplexNoise::F2 = 0.5 * (sqrt( 3.0 ) - 1.0);
+const double SimplexNoise::G2 = (3.0 - sqrt( 3.0 )) / 6.0;
+const uint8_t SimplexNoise::p[256] = {
+    151,160,137,91,90,15,131,13,201,95,96,53,194,233,7,225,140,36,103,30,69,
+    142,8,99,37,240,21,10,23,190,6,148,247,120,234,75,0,26,197,62,94,252,219,
+    203,117,35,11,32,57,177,33,88,237,149,56,87,174,20,125,136,171,168,68,175,
+    74,165,71,134,139,48,27,166,77,146,158,231,83,111,229,122,60,211,133,230,
+    220,105,92,41,55,46,245,40,244,102,143,54,65,25,63,161,1,216,80,73,209,76,
+    132,187,208,89,18,169,200,196,135,130,116,188,159,86,164,100,109,198,173,
+    186,3,64,52,217,226,250,124,123,5,202,38,147,118,126,255,82,85,212,207,206,
+    59,227,47,16,58,17,182,189,28,42,223,183,170,213,119,248,152,2,44,154,163,
+    70,221,153,101,155,167,43,172,9,129,22,39,253,19,98,108,110,79,113,224,232,
+    178,185,112,104,218,246,97,228,251,34,242,193,238,210,144,12,191,179,162,
+    241,81,51,145,235,249,14,239,107,49,192,214,31,181,199,106,157,184,84,204,
+    176,115,121,50,45,127,4,150,254,138,236,205,93,222,114,67,29,24,72,243,141,
+    128,195,78,66,215,61,156,180
+};
+const Grad SimplexNoise::grad3[12] = {
+    Grad(1,1,0),Grad(-1,1,0),Grad(1,-1,0),Grad(-1,-1,0),Grad(1,0,1),
+    Grad(-1,0,1),Grad(1,0,-1),Grad(-1,0,-1),Grad(0,1,1),Grad(0,-1,1),
+    Grad(0,1,-1),Grad(0,-1,-1)
+};
+uint8_t SimplexNoise::perm[512] = {0};
+uint8_t SimplexNoise::permMod12[512] = {0};
+
+// Initialize permutaion arrays
+void SimplexNoise::init() {
+    for ( uint16_t i = 0; i < 512; ++i ) {
+        perm[i] = p[i & 255];
+        permMod12[i] = static_cast<uint8_t>(perm[i] % 12);
+    }
+    delete[] &p; // lol what
+}
+
+// Fast floor
+int32_t SimplexNoise::fastFloor( double x ) {
+    int32_t xi = static_cast<int32_t>(x);
+    return x < xi ? xi - 1 : xi;
+}
+
+double SimplexNoise::dot( const Grad& g, double x, double y ) {
+    return g.x * x + g.y * y;
+}
+
+// 2D SimplexNoise noise
+double SimplexNoise::noise( double xin, double yin ) {
+    double s = (xin + yin) * F2;
+    int32_t i = fastFloor( xin + s );
+    int32_t j = fastFloor( yin + s );
+    double t = (i + j) * G2;
+    double x0 = xin - (i - t);
+    double y0 = yin - (j - t);
+    uint8_t i1 = 0, j1 = 1;
+    if ( x0 > y0 ) {
+        i1 = 1;
+        j1 = 0;
+    }
+    double x1 = x0 - i1 + G2;
+    double y1 = y0 - j1 + G2;
+    double x2 = x0 - 1.0 + 2.0 * G2;
+    double y2 = y0 - 1.0 + 2.0 * G2;
+    uint8_t ii = i & 255;
+    uint8_t jj = j & 255;
+    uint8_t gi0 = permMod12[ii + perm[jj]];
+    uint8_t gi1 = permMod12[ii + i1 + perm[jj + j1]];
+    uint8_t gi2 = permMod12[ii + 1 + perm[jj + 1]];
+    double n0 = 0.0;
+    double t0 = 0.5 - x0 * x0 - y0 * y0;
+    if ( t0 >= 0.0 ) {
+        t0 *= t0;
+        n0 = t0 * t0 * dot( grad3[gi0], x0, y0 );
+    }
+    double n1 = 0.0;
+    double t1 = 0.5 - x1 * x1 - y1 * y1;
+    if ( t1 >= 0.0 ) {
+        t1 *= t1;
+        n1 = t1 * t1 * dot( grad3[gi1], x1, y1 );
+    }
+    double n2 = 0.0;
+    double t2 = 0.5 - x2 * x2 - y2 * y2;
+    if ( t2 >= 0.0 ) {
+        t2 *= t2;
+        n2 = t2 * t2 * dot( grad3[gi2], x2, y2 );
+    }
+    return 70.0 * (n0 + n1 + n2);
+}
\ No newline at end of file
diff --git a/prosthesis_controller/SimplexNoise.h b/prosthesis_controller/SimplexNoise.h
new file mode 100644
index 0000000..4ed97af
--- /dev/null
+++ b/prosthesis_controller/SimplexNoise.h
@@ -0,0 +1,54 @@
+/*
+===============================================================================
+Ported the SimplexNoise algorithm from the C++ versions mentioned below 
+to a reuseable Arduino Library.
+
+By Jordan Shaw / http://jordanshaw.com / 2017-02
+
+A C++ port of a speed-improved simplex noise algorithm for 2D in Java.
+Based on example code by Stefan Gustavson (stegu@itn.liu.se).
+Optimisations by Peter Eastman (peastman@drizzle.stanford.edu).
+Better rank ordering method by Stefan Gustavson in 2012.
+C++ port and minor type and algorithm changes by Josh Koch (jdk1337@gmail.com).
+This could be speeded up even further, but it's useful as it is.
+Version 2012-04-12
+The original Java code was placed in the public domain by its original author,
+Stefan Gustavson. You may use it as you see fit,
+but attribution is appreciated.
+
+Original gist url: https://gist.github.com/Slipyx/2372043
+
+===============================================================================
+*/
+
+#ifndef SimplexNoise_h
+#define SimplexNoise_h
+
+#include <math.h>
+#include "Arduino.h"
+
+class Grad {
+public:
+    Grad( int8_t x, int8_t y, int8_t z ) : x(x), y(y), z(z) {}
+    int8_t x, y, z;
+};
+
+class SimplexNoise {
+public:
+    // Initialize permutation arrays
+    static void init();
+    // 2D simplex noise
+    static double noise( double xin, double yin );
+
+private:
+    static int32_t fastFloor( double x );
+    static double dot( const Grad& g, double x, double y );
+    static const double F2;
+    static const double G2;
+    static const Grad grad3[12];
+    static const uint8_t p[256];
+    static uint8_t perm[512];
+    static uint8_t permMod12[512];
+};
+
+#endif
\ No newline at end of file
diff --git a/prosthesis_controller/behaviour.cpp b/prosthesis_controller/behaviour.cpp
new file mode 100644
index 0000000..f8c55bc
--- /dev/null
+++ b/prosthesis_controller/behaviour.cpp
@@ -0,0 +1,186 @@
+#include "Arduino.h"
+#include "behaviour.h"
+
+SimplexNoise sn;
+
+
+Mood mood;
+
+float matrix_weights_main[SERVO_COUNT_MAIN][WEIGHT_COUNT]; //mapping outputs, sensors/moods
+float matrix_weights_peripheralL[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT]; 
+float matrix_weights_peripheralR[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT]; 
+
+void updateMatrixWeights(unsigned long millis, bool body_present,bool contact_main){
+  mood.loneliness=1.0;
+  
+  static unsigned long last_weights_update=0;
+  if (millis-last_weights_update>WEIGHT_UPDATE_INTERVAL) {
+    last_weights_update=millis;
+
+    static float contact_main_persist=0;
+    if (contact_main) {
+      contact_main_persist=1.0;
+    }else{
+      if (contact_main_persist>0) {
+        contact_main_persist-=0.2*WEIGHT_UPDATE_INTERVAL/1000.0;
+      }else{
+        contact_main_persist=0;
+      }
+    }
+    
+    //0=pitch
+    //1=roll
+    
+
+    //float map_mode=constrain(mapfloat(mood.wakefulness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise
+    //float map_mode=constrain(mapfloat(mood.shakiness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise
+
+    /*float map_mode_noise=constrain(mapfloat(mood.wakefulness,0,0.2, 0,1),0,1);
+    float map_mode_rollpitch=constrain(mapfloat(mood.shakiness,0,0.5, 0,1),0,1);
+    float map_mode_slither=0;
+    float map_mode_sleeping=mood.loneliness;*/
+
+
+    bool state_bellyup=false;
+    bool state_map_mode_detached=false;
+    bool state_map_mode_attached=false;
+
+    float map_mode_rollpitch=0;
+    float map_mode_slither=0;
+    float map_mode_sleeping=0;
+
+
+    float contact_main_smooth=constrain(mapfloat(contact_main_persist,0.0,0.3, 0.0,1.0), 0.0,1.0);
+    float map_mode_noise=1.0-contact_main_smooth; //idle movement
+    float map_mode_scared=0.0+contact_main_smooth; //contracted position
+
+
+    
+    
+
+
+    float map_sum=map_mode_noise+map_mode_scared;
+    map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0
+    map_mode_scared/=map_sum;
+
+    /*
+    Serial.print(map_mode_noise); Serial.print(",");
+    Serial.print(map_mode_rollpitch); Serial.print(",");
+    Serial.print(map_mode_slither); Serial.print(",");
+    Serial.print(map_mode_sleeping); Serial.println();
+    */
+    
+    //Mode Noise
+    matrix_weights_main[0][W_NOISE]=30.0 *map_mode_noise;
+    matrix_weights_main[0][W_NOISESLOW]=0.0 *map_mode_noise;
+    
+    matrix_weights_main[1][W_NOISE]=30.0 *map_mode_noise;
+    matrix_weights_main[1][W_NOISESLOW]=20.0 *map_mode_noise;
+    
+    matrix_weights_main[2][W_NOISE]=20.0 *map_mode_noise;
+    matrix_weights_main[2][W_NOISESLOW]=30.0 *map_mode_noise;
+    
+    matrix_weights_main[0][W_LOOKDIRECTION]=30* map_mode_scared; 
+    matrix_weights_main[1][W_LOOKDIRECTION]=30.0* map_mode_scared;  
+    matrix_weights_main[2][W_LOOKDIRECTION]=30.0* map_mode_scared; 
+
+    
+    //Mode Roll Pitch
+    matrix_weights_main[0][W_ROLL]=1.0* map_mode_rollpitch; 
+    matrix_weights_main[1][W_PITCH]=1.0* map_mode_rollpitch;  
+    matrix_weights_main[2][W_ROLL]=1.0* map_mode_rollpitch; 
+
+    //Mode Slither
+    matrix_weights_main[0][W_COS]=30 *map_mode_slither;
+    matrix_weights_main[1][W_SIN]=30 *map_mode_slither;
+    matrix_weights_main[2][W_COS]=-30 *map_mode_slither;
+    
+    //Mode Sleeping
+    matrix_weights_main[0][W_NOISESLOW]=20 *map_mode_sleeping;
+    matrix_weights_main[1][W_NOISESLOW]=20 *map_mode_sleeping;
+    matrix_weights_main[2][W_NOISESLOW]=30 *map_mode_sleeping;
+
+    /*
+    float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.1, 0,1),0,1);
+    for(int i = 0; i < SERVO_COUNT ; i++){
+      matrix_weights[i][W_PITCH]*=sleeping;
+      matrix_weights[i][W_ROLL]*=sleeping;
+      matrix_weights[i][W_NOISE]*=sleeping;
+      matrix_weights[i][W_NOISESLOW]*=constrain(sleeping,0.1,1);
+    }
+    */
+  }
+}
+
+
+void updateServosByWeights(unsigned long millis){
+  static unsigned long last_servoweights_update=0;
+  if (millis-last_servoweights_update>SERVOWEIGHT_UPDATE_INTERVAL) 
+  {
+    last_servoweights_update=millis;
+    static unsigned long millis_add;
+    #define MILLISADD_MAX 100
+    millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX);
+
+    float pitch=0;
+    float roll=0;
+
+    float vsin=sin((millis()+millis_add)/1000.0);
+    float vcos=cos((millis()+millis_add)/1000.0);
+    
+    for (uint8_t i=0;i<SERVO_COUNT;i++) {
+      uint8_t servoPosInMatrix=0;
+
+      
+      float pnoise=sn.noise((millis()+millis_add)/5000.0,i*10); //simplexnoise -1 to 1
+      float pnoiseslow=sn.noise((millis()+millis_add)/50000.0,i*10); //simplexnoise -1 to 1
+      
+
+      float angle=0;
+
+      float lookdirection=(servos[i].angle>=0)?1:-1; //if angle positive, 1, else 0
+      
+      
+      
+
+      if (i<SERVO_COUNT_MAIN) { //Main
+        /*if (i==0){
+          Serial.print("servoangle:");Serial.println(servos[i].angle);
+          Serial.print("lookdirection:");Serial.println(lookdirection);
+          Serial.print("weight:");Serial.println(matrix_weights_main[i][W_LOOKDIRECTION]);
+
+
+                    
+        }*/
+
+        servoPosInMatrix=i;
+        angle=
+          pitch*matrix_weights_main[servoPosInMatrix][W_PITCH]
+          +roll*matrix_weights_main[servoPosInMatrix][W_ROLL]
+          +pnoise*matrix_weights_main[servoPosInMatrix][W_NOISE]
+          +pnoiseslow*matrix_weights_main[servoPosInMatrix][W_NOISESLOW]
+          +vsin*matrix_weights_main[servoPosInMatrix][W_SIN]
+          +vcos*matrix_weights_main[servoPosInMatrix][W_COS]
+          +lookdirection*matrix_weights_main[servoPosInMatrix][W_LOOKDIRECTION];
+        
+      }else if(i<SERVO_COUNT_MAIN+SERVO_COUNT_PERIPHERAL) { //Peripheral Left
+        servoPosInMatrix=i-3;
+      }else if(i<SERVO_COUNT_MAIN+2*SERVO_COUNT_PERIPHERAL) { //Peripheral Right
+        servoPosInMatrix=i-2*3;
+
+      }
+      
+      servos[i].angle=angle;
+      
+    }
+  }
+  
+}
+
+
+
+float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
+{
+  return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
+}
+
diff --git a/prosthesis_controller/behaviour.h b/prosthesis_controller/behaviour.h
new file mode 100644
index 0000000..0003abb
--- /dev/null
+++ b/prosthesis_controller/behaviour.h
@@ -0,0 +1,43 @@
+#ifndef BEHAVIOUR_H_
+#define BEHAVIOUR_H_
+
+
+#include "SimplexNoise.h" //Library from https://github.com/jshaw/SimplexNoise
+#include "definitions.h"
+#include "servo.h"
+
+extern dxl_servo servos[SERVO_COUNT];
+
+
+
+#define SERVO_COUNT_MAIN 3
+#define SERVO_COUNT_PERIPHERAL 3
+
+#define WEIGHT_COUNT 7 //enter number of weights here for array size
+
+
+#define WEIGHT_UPDATE_INTERVAL 100
+#define SERVOWEIGHT_UPDATE_INTERVAL SERVO_UPDATE_INTERVAL
+
+
+#define W_PITCH 0
+#define W_ROLL 1
+#define W_NOISE 2
+#define W_NOISESLOW 3
+#define W_SIN 4
+#define W_COS 5
+#define W_LOOKDIRECTION 6
+
+
+struct Mood{
+  float shakiness;
+  float wakefulness;
+  float loneliness;
+};
+extern Mood mood;
+
+void updateMatrixWeights(unsigned long millis,bool body_present, bool contact_main);
+void updateServosByWeights(unsigned long millis);
+float mapfloat(float x, float in_min, float in_max, float out_min, float out_max);
+
+#endif
\ No newline at end of file
diff --git a/prosthesis_controller/bodytemp.cpp b/prosthesis_controller/bodytemp.cpp
new file mode 100644
index 0000000..837ec07
--- /dev/null
+++ b/prosthesis_controller/bodytemp.cpp
@@ -0,0 +1,57 @@
+#include "Arduino.h"
+#include "bodytemp.h"
+
+Adafruit_MLX90614 mlx = Adafruit_MLX90614();
+
+double bodytemp_ambient=0;
+double bodytemp_object=0;
+
+bool body_present=false;
+
+const double bodytemp_activate_temperature=30; //°C min object temperature to detect human
+const double bodytemp_activate_mindiff_ambient=3; //°C object temperature has to be at least this amount above ambient
+const double bodytemp_deactivate_maxdiff_ambient=2; //°C maximum difference between object and ambient temperature to not detect human anymore
+const unsigned long bodytemp_deacitvate_mintime=1000; //minimum time in ms between activate and deactivate
+
+void initBodytemp() {
+  Wire.begin(PIN_SDA,PIN_SCL);
+  if (!mlx.begin(0x5A,&Wire)) {
+    Serial.println("Error connecting to MLX sensor. Check wiring.");
+    while (1);
+  };
+}
+
+void printBodytempDebug() {
+  Serial.print("min:20,max:40,");
+  Serial.print("Ambient:");Serial.print(bodytemp_ambient);
+  Serial.print(",Object:");Serial.print(bodytemp_object);
+  Serial.print(",Presence:");Serial.print(body_present?39:21);
+
+  
+}
+
+bool checkBodypresence(unsigned long millis) {
+  static unsigned long last_read_bodytemp=0;
+  if (millis-last_read_bodytemp > BODYTEMP_READ_MIN_INTERVAL)
+  {
+    last_read_bodytemp = millis;
+
+    bodytemp_ambient = mlx.readAmbientTempC();
+    bodytemp_object = mlx.readObjectTempC();
+
+    static unsigned long last_change_bodypresent=0;
+    if (millis-last_change_bodypresent > bodytemp_deacitvate_mintime) {}
+    if (!body_present) { //is currently not present
+      if (bodytemp_object >= bodytemp_activate_temperature && (bodytemp_object-bodytemp_ambient) >= bodytemp_activate_mindiff_ambient) {
+        body_present=true;
+        last_change_bodypresent=millis;
+      }
+    }else{ //was present last time
+      if((bodytemp_object-bodytemp_ambient) <= bodytemp_deactivate_maxdiff_ambient) {
+        body_present=false;
+        last_change_bodypresent=millis;
+      }
+    }
+  }
+  return body_present;
+}
\ No newline at end of file
diff --git a/prosthesis_controller/bodytemp.h b/prosthesis_controller/bodytemp.h
new file mode 100644
index 0000000..f382dbd
--- /dev/null
+++ b/prosthesis_controller/bodytemp.h
@@ -0,0 +1,13 @@
+#ifndef BODYTEMP_H_
+#define BODYTEMP_H_
+
+#include <Adafruit_MLX90614.h> //Adafruit MLX90614 Library by Adafruit 2.1.5
+#include "definitions.h"
+
+#define BODYTEMP_READ_MIN_INTERVAL 500
+
+void initBodytemp();
+void printBodytempDebug() ;
+bool checkBodypresence(unsigned long millis);
+
+#endif
\ No newline at end of file
diff --git a/prosthesis_controller/definitions.h b/prosthesis_controller/definitions.h
index 762fd8d..3e44085 100644
--- a/prosthesis_controller/definitions.h
+++ b/prosthesis_controller/definitions.h
@@ -22,7 +22,7 @@
 #define PIN_OUT7 8 //PWM
 #define PIN_OUT8 9 //PWM
 
-#define PIN_BUTTON 23
+#define PIN_BUTTON BOARD_BUTTON_PIN //BOARD_BUTTON_PIN = 23
 
 #define PIN_PUMP1 PIN_OUT5
 #define PIN_PUMP2 PIN_OUT7
@@ -32,6 +32,11 @@
 
 #define PIN_VIBRATION PIN_OUT2
 
+#define PIN_SDA 25
+#define PIN_SCL 24
+
+#define PIN_LED BOARD_LED_PIN //BOARD_LED_PIN=14
+
 
 
 #endif
\ No newline at end of file
diff --git a/prosthesis_controller/prosthesis_controller.ino b/prosthesis_controller/prosthesis_controller.ino
index 286da12..ad663b3 100644
--- a/prosthesis_controller/prosthesis_controller.ino
+++ b/prosthesis_controller/prosthesis_controller.ino
@@ -1,6 +1,8 @@
 #include "definitions.h"
 #include "vacuum.h"
 #include "servo.h"
+#include "bodytemp.h"
+#include "behaviour.h"
 
 void setup() {
   Serial.begin(115200);
@@ -9,11 +11,16 @@ void setup() {
   analogWrite(PIN_VIBRATION,0);
 
   pinMode(PIN_BUTTON,INPUT_PULLDOWN);
+  pinMode(PIN_LED,OUTPUT);
+  digitalWrite(PIN_LED,HIGH); //LOW=Light, HIGH=Off
 
   initVacuum();
+  initBodytemp();
 
 
   pinMode(PIN_INPUTARM1,INPUT);
+  pinMode(PIN_INPUTARM2,INPUT);
+  pinMode(PIN_INPUTARM3,INPUT);
 
   pinMode(PIN_INPUTAUX,INPUT);
 
@@ -23,20 +30,34 @@ void setup() {
 
 
 
-
-
-
 void loop() {
   unsigned long loopmillis=millis();
-
-  bool vac=digitalRead(PIN_BUTTON); //Temporary Vacuum Button
-  setVacuum(vac);
+ 
+  
+  bool body_present=checkBodypresence(loopmillis);
+  digitalWrite(PIN_LED,!body_present);
 
   loopVacuum(loopmillis);
 
+  
+  bool buttonstate=digitalRead(PIN_BUTTON); //Temporary Vacuum Button. HIGH=Pressed
+  if (buttonstate) { //Button pressed
+    setVacuum(false); //release
+  }else{
+    setVacuum(body_present);
+  }
+
+  bool contact_main=digitalRead(PIN_INPUTARM1); //Sensor on tip of arm
+  
+  updateMatrixWeights(loopmillis,body_present,contact_main);
+  updateServosByWeights(loopmillis);
+
   loopServos(loopmillis);
 
 
+
+
+
   //Print Debug Information
   static unsigned long last_print=0;
   if (loopmillis - last_print >100) {
@@ -45,9 +66,11 @@ void loop() {
     //Serial.print(" Vac="); Serial.print(vac);
 
     printServoDebug();
-    
-    
     Serial.println();
+    //printBodytempDebug();
+    
+    
+    
     Serial.println();
   }
 
diff --git a/prosthesis_controller/servo.cpp b/prosthesis_controller/servo.cpp
index 4a6909f..0011da7 100644
--- a/prosthesis_controller/servo.cpp
+++ b/prosthesis_controller/servo.cpp
@@ -3,10 +3,10 @@
 
 using namespace DYNAMIXEL;
 
-#define SERVO_COUNT 3
 const uint8_t DXL_ID = 1;
 const float DXL_PROTOCOL_VERSION = 2.0;
 
+
 dxl_servo servos[SERVO_COUNT];
 
 Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
@@ -100,18 +100,15 @@ void initServos()
 
 
 void loopServos(unsigned long millis) {
-  servos[0].angle=sin(millis/1000.0/1.0)*20;
+  /*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;
+  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/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);
-    //}
     
-    dxl.setGoalPosition(servos[servoUpdateI].id, servos[servoUpdateI].offset_angle+servos[servoUpdateI].angle*servos[servoUpdateI].orientation, UNIT_DEGREE);
+    dxl.setGoalPosition(servos[servoUpdateI].id, servos[servoUpdateI].offset_angle+servos[servoUpdateI].angle*servos[servoUpdateI].orientation, UNIT_DEGREE); //asdf
     servoUpdateI++;
     servoUpdateI%=SERVO_COUNT;
   }
@@ -120,7 +117,8 @@ void loopServos(unsigned long millis) {
 
 void printServoDebug() {
   for (uint8_t i=0;i<SERVO_COUNT;i++) {
-    Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(dxl.getPresentPosition(servos[i].id, UNIT_DEGREE)); Serial.print(" -> "); Serial.print(servos[i].offset_angle+servos[i].angle);
+    //Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(dxl.getPresentPosition(servos[i].id, UNIT_DEGREE)); Serial.print(" -> "); Serial.print(servos[i].offset_angle+servos[i].angle);
+    Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(servos[i].offset_angle+servos[i].angle);
     Serial.println();
   }
 }
diff --git a/prosthesis_controller/servo.h b/prosthesis_controller/servo.h
index fdd4689..670b893 100644
--- a/prosthesis_controller/servo.h
+++ b/prosthesis_controller/servo.h
@@ -18,6 +18,11 @@
 #define SERVO_UPDATE_INTERVAL 1000/50
 
 
+#define SERVO_COUNT 3
+
+
+
+
 struct dxl_servo {
 	int id;
 	//int initial_speed;
@@ -31,6 +36,8 @@ struct dxl_servo {
   uint16_t modelnumber;
 };
 
+
+
 void scanDynamixel(int32_t baud, uint8_t protocol,uint8_t maxid);
 void scanDynamixel();
 void changeID(uint8_t present_id, uint8_t new_id);