diff --git a/main.cpp b/main.cpp
index 774205c..9fd64c6 100644
--- a/main.cpp
+++ b/main.cpp
@@ -20,13 +20,33 @@ class CustomArray {
     int size;
 
     public:
-    CustomArray(int s) {
-        size = s;
-        items = (int*) malloc(s * sizeof(int));
-        for (int i = 0; i < s; i++ )
-        {
-            items[i] = 0;
+    CustomArray() {
+        size = MAX_STORED_VALUES;
+        items = (int*) malloc(size * sizeof(int));
+        for (int i = 0; i < size; i++ ) items[i] = 0;
+    }
+
+    void PushItem(int item) {
+        for (int i = 1; i < MAX_STORED_VALUES; i++) {
+            items[i] = items[i-1];
         }
+
+        items[0] = item;
+    }
+
+    int getElement(int index) { return items[index]; }
+    int getSum() {
+        int sum = 0;
+
+        for (int i = 0; i < MAX_STORED_VALUES; i++ ) {
+            sum += items[i];
+        }
+
+        return sum;
+    }
+
+    ~CustomArray() {
+        free(items);
     }
 };
 
@@ -36,28 +56,17 @@ class PID {
         bool on;
         CustomArray error_array;
 
-    int calculate_integral(int* array) {
-        int sum = 0;
-
-        for (int i = 0; i < MAX_STORED_VALUES; i++ ) {
-            sum += array[i];
-        }
-
-        sum *= POLL_PERIOD;
-        return sum;
-    }
-    
     public:
         PID(int kp) {
-            error_array = new CustomArray(MAX_STORED_VALUES);
-            this.kp = kp;
+            error_array = CustomArray();
+            this->kp = kp;
         }
 
         int calulatePid( int x ) {
             int e = desired_temp-x; 
 
-            int e_dot = (e - error_array[0]) / POLL_PERIOD;
-            int e_int = calculate_integral(error_array);
+            int e_dot = (e - error_array.getElement(0)) / POLL_PERIOD;
+            int e_int = error_array.getSum() * POLL_PERIOD;
         
 
             return e * kp + e_dot * kd + e_int * ki;