diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 496bf11..60a4ac6 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -3,8 +3,8 @@ add_executable(${PROJECT_NAME}
 )
 
 target_link_libraries(${PROJECT_NAME}
-    RegEdit
-    ADC
+    #RegEdit
+    #ADC
     #timer
 )
 
diff --git a/src/main.c b/src/main.c
index b3e13e1..89176aa 100644
--- a/src/main.c
+++ b/src/main.c
@@ -3,7 +3,7 @@
 #endif
 
 #include "main.h"
-// #include "ADC.h"
+//#include "ADC.h"
 #include <avr/eeprom.h>
 #include <avr/interrupt.h>
 #include <util/delay.h>
@@ -33,7 +33,7 @@ volatile uint16_t tick_count;
 // #############################
 
 static inline void InitTimer0(void);
-static void InitBtn(btn_state *b, uint8_t input_pin, uint8_t output_pin);
+static void InitBtn(btn_state *b, uint8_t input_pin);
 static void ClearButtonTimer(btn_state *b);
 static void StartButtonTimer(btn_state *b);
 static void CheckButtonLongpress(btn_state *b);
@@ -50,7 +50,6 @@ int main() {
   while (1) {
     UpdateButtonOutput(&btn1);
     UpdateButtonInput(&btn1);
-    // MotorMoveTo(0);
   }
 
   return 0;
@@ -64,7 +63,7 @@ void InitProg(void) {
   /*Set the debounced state to all high*/
   debounced_state = 0xFF;
 
-  InitBtn(&btn1, BUTTON_PIN, PIN_ACTIVE1);
+  InitBtn(&btn1, BUTTON_PIN);
 
   /*Wait 5ms for pull-up resistors voltage to become stable.*/
   _delay_ms(5);
@@ -75,11 +74,11 @@ void InitProg(void) {
 
   // Checks against a bit pattern we defined to represent the start of data.
   if (eeprom_read_byte((uint8_t *)ROM_SP_ADR) == START_PATTERN) {
-    MotorMoveTo(eeprom_read_byte((uint8_t *)ROM_SS1_ADR));
+    MotorMoveTo(eeprom_read_byte((uint8_t *)POSITION1_ADR));
   } else {
     // otherwise we write the init values for the start pattern and states.
     eeprom_write_byte((uint8_t *)ROM_SP_ADR, START_PATTERN);
-    eeprom_write_byte((uint8_t *)ROM_SS1_ADR, 0x7F);
+    eeprom_write_byte((uint8_t *)POSITION1_ADR, 0x7F);
   }
 
   InitTimer0();
@@ -95,17 +94,18 @@ uint8_t ReadSpeed(void) {
   while (ADCSRA & (1 << ADSC)) {
   } // Wait for conversion to finish
 
-  uint8_t val = (uint8_t)(ADC >> 2);
-
-  // Map the speed value to the 100%-50% duty cycle range.
-  // val = (uint8_t)( (float)val / 255.0 ) * 100;
+    //Normally a bitshift of 2 is done for 8bit ADC values,
+    //however we want to divide by two after this as well so we
+    //do a third shift followed by an addition of 127 to yield a mapping of
+    //approximatly 50%-96.6% duty cycle range.
+    uint8_t val = (uint8_t)(ADC >> 3) + 127;
 
   return val;
 }
 
 // change to ReadFader(void)
 uint16_t ReadFader(void) {
-  // Initialize ADC
+    // Initialize ADC
   ADMUX = (1 << MUX1) | (1 << MUX0);                  // Select ADC3 (PB3)
   ADCSRA = (1 << ADEN) | (1 << ADPS1) | (1 << ADPS0); // Enable ADC, prescaler 8
 
@@ -128,14 +128,15 @@ static inline uint8_t diff(uint8_t a, uint8_t b) {
   return b - a;
 }
 
-void MotorSetSavePos() {
+void MotorSetSavePos(uint8_t *ADR) {
   uint8_t pos = (uint8_t)(ReadFader() >> 2);
-  eeprom_write_byte((uint8_t *)ROM_SS1_ADR, pos);
+  //eeprom_write_byte((uint8_t *)ADR, pos);
+  eeprom_update_byte((uint8_t *)ADR, pos);
   return;
 }
 
-uint8_t MotorGetSavedPos(void) {
-  return (uint8_t)eeprom_read_byte((uint8_t *)ROM_SS1_ADR);
+uint8_t MotorGetSavedPos(uint8_t *ADR) {
+  return (uint8_t)eeprom_read_byte((uint8_t *)POSITION1_ADR);
 }
 
 void MotorMoveTo(uint8_t target) {
@@ -190,21 +191,17 @@ void MotorCoast(void) {
  */
 
 /*This is kinda like our button constructor*/
-static void InitBtn(btn_state *b, uint8_t input_pin, uint8_t output_pin) {
+static void InitBtn(btn_state *b, uint8_t input_pin) {
   b->is_long_pressed = 0;
   b->is_pressed = 0;
   b->is_active = 0;
   b->pressed_ticks = 0;
   b->timer_enabled = 0;
   b->input_pin = input_pin;
-  b->output_pin = output_pin;
 
   /*Configure the buttons inputs and outputs*/
   DDRB &= ~(1 << b->input_pin);
   PORTB |= (1 << b->input_pin);
-
-  DDRB |= (1 << b->output_pin);
-  PORTB &= ~(1 << b->output_pin);
 }
 
 static void ClearButtonTimer(btn_state *b) {
@@ -248,6 +245,7 @@ static void UpdateButtonOutput(btn_state *b) {
       /*Then start the timer and update the output*/
       // ToggleOutput(b);
       StartButtonTimer(b);
+
       return;
     }
 
@@ -267,12 +265,21 @@ static void UpdateButtonOutput(btn_state *b) {
   else if (!b->is_pressed) {
     /*If the button was released on a long press.*/
     if (b->is_long_pressed) {
-      MotorSetSavePos();
+      MotorSetSavePos((uint8_t *)POSITION1_ADR);
     }
     /*If the button pres was a short one.*/
     else if (!b->is_long_pressed) {
       if (b->is_active) {
-        MotorMoveTo(MotorGetSavedPos());
+          /*Save the current position into position 2 EEPROM*/
+          MotorSetSavePos((uint8_t *)POSITION2_ADR);
+
+            /*If already in saved position then go back to original pos.*/
+          if(diff(MotorGetSavedPos((uint8_t *)POSITION1_ADR), ReadFader()) > 8){
+                MotorMoveTo(MotorGetSavedPos((uint8_t *)POSITION1_ADR));
+          }
+          else{
+                MotorMoveTo(MotorGetSavedPos((uint8_t *)POSITION1_ADR));
+          }
       } else {
         MotorCoast();
       }