diff --git a/charger.fzz b/charger.fzz
index 962ed86..fa4c6f7 100644
Binary files a/charger.fzz and b/charger.fzz differ
diff --git a/charger/charger.ino b/charger/charger.ino
index 11b24f1..885dca9 100644
--- a/charger/charger.ino
+++ b/charger/charger.ino
@@ -1,5 +1,63 @@
 #include <ModbusRTU.h>
 
+class Reader
+{
+  const static int AMOUNT = 20;
+  const static int FILTER_SIZE = 5;
+  int pin;
+  int reads[AMOUNT];
+  int current_read;
+  public:
+    
+    Reader(int pin)
+    {
+      this->pin = pin;
+      current_read = 0;
+      for (int i = 0; i < AMOUNT; i++) reads[i] = 0;
+    }
+
+    void Read()
+    {
+      reads[current_read] = analogRead(pin);
+      current_read++;
+      if (current_read >= AMOUNT) current_read -= AMOUNT;
+    }
+
+    int sorted_reads[AMOUNT];
+
+    int Get()
+    {
+      for (int i = 0; i < AMOUNT; i++)
+      {
+        sorted_reads[i] = reads[i];
+      }
+      bool f = true;
+      while (f)
+      {
+        f = false;
+        for (int i = 0; i < AMOUNT - 1; i++)
+        {
+          if (sorted_reads[i] > sorted_reads[i + 1])
+          {
+            int t = sorted_reads[i];
+            sorted_reads[i] = sorted_reads[i + 1];
+            sorted_reads[i + 1] = t;
+            f = true;
+          }
+        }
+      }/*
+      float sum = 0;
+      int cnt = 0;
+      for (int i = FILTER_SIZE; i < AMOUNT - FILTER_SIZE; i++)
+      {
+        sum += reads[i];
+        cnt++;
+      }
+      return (int)(sum / cnt + 0.5);*/
+      return sorted_reads[AMOUNT / 2];
+    }
+};
+
 ModbusRTU mb;
 
 #define MODBUS_ADDRESS 1
@@ -47,6 +105,12 @@ void setup()
   }
 }
 
+Reader R2(A2);
+Reader R3(A3);
+Reader R4(A4);
+Reader R5(A5);
+Reader R7(A7);
+
 float I_acc;
 float U4;
 float U5;
@@ -55,8 +119,8 @@ float U3;
 
 void ReadLamp()
 {
-  int a4 = analogRead(A4);
-  int a5 = analogRead(A5);
+  int a4 = R4.Get();
+  int a5 = R5.Get();
   // multiply by 5 as reference current, 
   // multiply by 3 because of the divider
   U4 = a4 / 1023.0 * 5 * 3;
@@ -65,36 +129,19 @@ void ReadLamp()
 
 void ReadAcc()
 {
-  int a2 = analogRead(A2);
-  int a3 = analogRead(A3);
+  int a2 = R2.Get();
+  int a3 = R3.Get();
   // multiply by 5 as reference current, 
   // multiply by 3 because of the divider
   U2 = a2 / 1023.0 * 5 * 3;
   U3 = a3 / 1023.0 * 5 * 3;
 }
 
-const float CURRENT_SENSOR_MAX = 30;
+const float CURRENT_SENSOR_MAX = 5;
 
 void ReadI()
 {
-  static int reads[10];
-  for (int i = 0; i < 10; i++)
-  {
-    reads[i] = analogRead(A7);
-  }
-  int imin = 0, imax = 0;
-  for (int i = 0; i < 10; i++)
-  {
-    if (reads[i] < reads[imin]) imin = i;
-    if (reads[i] < reads[imax]) imax = i;
-  }
-  int total = 0;
-  for (int i = 0; i < 10; i++)
-  {
-    if (i == imin || i == imax) continue;
-    total += reads[i];
-  }
-  I_acc = ((total / 8.0 + 0.0) / 1023.0 - 0.5) * CURRENT_SENSOR_MAX;
+  I_acc = ((R7.Get() + 0.0) / 1023.0 - 0.5) * CURRENT_SENSOR_MAX;
 }
 
 void WriteFloatReg(int start, float val)
@@ -109,6 +156,11 @@ uint16_t lamp_state = 0;
 
 void loop() 
 {
+  R2.Read();
+  R3.Read();
+  R4.Read();
+  R5.Read();
+  R7.Read();
   ReadLamp();
   ReadAcc();
   ReadI();
diff --git a/charger_mosfet2.fzz b/charger_mosfet2.fzz
index 7099aac..51a4316 100644
Binary files a/charger_mosfet2.fzz and b/charger_mosfet2.fzz differ