 }
 
 float read_enc2() {
-  // Get current reading
   unsigned int reading = analogRead(ENC2);
-  int reading_diff = int(reading)-int(prev_reading);
-  // Add/Remove a full-revolution when the reading jumps
-  if (reading_diff > 0.1*min_max_diff) {
-    if (enc2_jump_flag == 0) enc2_revs--;
-    enc2_jump_flag = -4;
-  }
-  else {
-    if (reading_diff < -0.1*min_max_diff) {
-      if (enc2_jump_flag == 0) enc2_revs++;
-      enc2_jump_flag = 4;
-    }
-    else enc2_jump_flag /= 2;
-  }
-
-  // Avoid "double" jumps
-//  int limit = 10;
-//  float new_val = 0;
-//  while (limit--) {
-//    new_val = enc2_revs*min_max_diff + int(reading) - int(init_reading);
-//    float val_diff = new_val - out_angle_vec.get_by_id(-1);
-////    Serial.println("new_val: " + String(new_val) + " | prev_val: " + String(out_angle_vec.get_by_id(-1)));
-//    if (val_diff > 0.7*min_max_diff) enc2_revs--;
-//    else {
-//      if (val_diff < -0.7*min_max_diff) enc2_revs++;
-//      else break;
-//    }
-//  }
-
-  float new_val = 0;
-  if (enc2_jump_flag>0) new_val = enc2_revs*min_max_diff;
-  if (enc2_jump_flag<0) new_val = (enc2_revs+1)*min_max_diff;
-  if (enc2_jump_flag==0) new_val = enc2_revs*min_max_diff + int(reading) - int(init_reading);
-  
-  out_angle_vec.push(new_val);
+  int diff = uns_int_diff(reading, prev_reading);
   prev_reading = reading;
 
+  out_angle_vec.push(out_angle_vec.get_by_id(-1)+diff);
   // Use simpler filter
   out_angle = -out_angle_vec.get_avg()/ENC2_CPD;
 }
+
+//float read_enc2() {
+//  // Get current reading
+//  unsigned int reading = analogRead(ENC2);
+//  int reading_diff = int(reading)-int(prev_reading);
+//  // Add/Remove a full-revolution when the reading jumps
+//  if (reading_diff > 0.1*min_max_diff) {
+//    if (enc2_jump_flag == 0) enc2_revs--;
+//    enc2_jump_flag = -4;
+//  }
+//  else {
+//    if (reading_diff < -0.1*min_max_diff) {
+//      if (enc2_jump_flag == 0) enc2_revs++;
+//      enc2_jump_flag = 4;
+//    }
+//    else enc2_jump_flag /= 2;
+//  }
+//
+//  // Avoid "double" jumps
+////  int limit = 10;
+////  float new_val = 0;
+////  while (limit--) {
+////    new_val = enc2_revs*min_max_diff + int(reading) - int(init_reading);
+////    float val_diff = new_val - out_angle_vec.get_by_id(-1);
+//////    Serial.println("new_val: " + String(new_val) + " | prev_val: " + String(out_angle_vec.get_by_id(-1)));
+////    if (val_diff > 0.7*min_max_diff) enc2_revs--;
+////    else {
+////      if (val_diff < -0.7*min_max_diff) enc2_revs++;
+////      else break;
+////    }
+////  }
+//
+//  float new_val = 0;
+//  if (enc2_jump_flag>0) new_val = enc2_revs*min_max_diff;
+//  if (enc2_jump_flag<0) new_val = (enc2_revs+1)*min_max_diff;
+//  if (enc2_jump_flag==0) new_val = enc2_revs*min_max_diff + int(reading) - int(init_reading);
+//  
+//  out_angle_vec.push(new_val);
+//  prev_reading = reading;
+//
+//  // Use simpler filter
+//  out_angle = -out_angle_vec.get_avg()/ENC2_CPD;
+//}
 
 float update_encoders() {
   // Keep Enc1_count safe from overflow

// Output angle variables
MyVector out_angle_vec(5);
//volatile unsigned int prev_reading = 0;
unsigned int init_reading = 0;
const unsigned int min_reading = 53;
const unsigned int max_reading = 979;
const int min_max_diff = int(max_reading-min_reading);


  delay(200);
  prev_reading = analogRead(ENC2);
  out_angle_vec.fill_with(0);
}

