diff --git a/Homework/Homework5/Encoder/Encoder.ino b/Homework/Homework5/Encoder/Encoder.ino
index 8065388..5d180d0 100644
--- a/Homework/Homework5/Encoder/Encoder.ino
+++ b/Homework/Homework5/Encoder/Encoder.ino
@@ -10,140 +10,110 @@ const unsigned long RESET_COUNT_PERIOD_IN_US = 50000;
 const int EDGES_PER_CYCLE = 13;
 const int REDUCTION_RATIO = 20;
 int count = 0;
-int forward_count = 0;
-int backward_count = 0;
-unsigned long aFallingTime = 0;
-unsigned long bFallingTime = 0;
+int current_rpm = 0;
+double output_level = 0;
+
+double err = 0, diff_err = 0, accumulate_err = 0;
+const double Kp = 0.4, Ki = 0, Kd = 0.5;
+const int goal_rpm = 100;
 
 void setup() {
-    pinMode(IN1, OUTPUT);
-    pinMode(IN2, OUTPUT);
-    pinMode(ENABLE, OUTPUT);
-    pinMode(A0, INPUT);
-    pinMode(A1, INPUT);
-    digitalWrite(IN1, LOW);
-    digitalWrite(IN2, HIGH);
-    pinMode(ENCO_B, INPUT);
-    pinMode(ENCO_A, INPUT);
-    Serial.begin(115200);
-    Timer1.initialize();
-    Timer1.setPeriod(RESET_COUNT_PERIOD_IN_US);
-    Timer1.attachInterrupt(resetCountAndPrint);
-    // attachInterrupt(digitalPinToInterrupt(ENCO_A), onEncoAChange, CHANGE);
-    // attachInterrupt(digitalPinToInterrupt(ENCO_B), onEncoBChange, CHANGE);
-    attachInterrupt(digitalPinToInterrupt(ENCO_A), onEncoAFalling, FALLING);
-    // attachInterrupt(digitalPinToInterrupt(ENCO_B), onEncoBFalling, FALLING);
-    analogWrite(ENABLE, 50);
+  pinMode(IN1, OUTPUT);
+  pinMode(IN2, OUTPUT);
+  pinMode(ENABLE, OUTPUT);
+  digitalWrite(IN1, HIGH);
+  digitalWrite(IN2, LOW);
+  pinMode(ENCO_B, INPUT);
+  pinMode(ENCO_A, INPUT);
+  Serial.begin(9600);
+  Timer1.initialize();
+  Timer1.setPeriod(RESET_COUNT_PERIOD_IN_US);
+  Timer1.attachInterrupt(resetCountAndPrint);
+  attachInterrupt(digitalPinToInterrupt(ENCO_A), onEncoAChange, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(ENCO_B), onEncoBChange, CHANGE);
+  analogWrite(ENABLE, output_level);
 }
 
 void loop() {
-    // int statusA = digitalRead(ENCO_A);
-    // int statusB = digitalRead(ENCO_B);
-    // // Serial.print("A: ");
-    // Serial.print('$');
-    // Serial.print(statusA);
-    // // Serial.print("B: ");
-    // Serial.print(' ');
-    // Serial.print(statusB);
-    // Serial.println(";");
-
-    // bFallingTime = micros();
-    
-    // aFallingTime = micros();
-
-    // Serial.println(bFallingTime - aFallingTime);
+  // if (Serial.available()) {
+  //   switch (Serial.read()) {
+  //   case 's':
+  //     analogWrite(ENABLE, 50);
+  //     break;
+  //   case 'p':
+  //     analogWrite(ENABLE, 0);
+  //     break;
+  //   }
+  // }
 }
 
 void resetCountAndPrint() {
-    Serial.print("Rpm: ");
-    // Serial.println((double)count * 60 * 1000 * 1000 / RESET_COUNT_PERIOD_IN_US / REDUCTION_RATIO / EDGES_PER_CYCLE);
-    // Serial.print(forward_count);
-    // Serial.print(' ');
-    // Serial.print(backward_count);
-    // Serial.print(' ');
-    count = forward_count - backward_count;
-    Serial.print(forward_count);
-    Serial.print(' ');
-    Serial.print(backward_count);
-    Serial.print(' ');
-    Serial.println(count);
-    count = 0;
-    forward_count = 0;
-    backward_count = 0;
+  current_rpm = (double)count * 60 * 1000 * 1000 / RESET_COUNT_PERIOD_IN_US /
+                REDUCTION_RATIO / EDGES_PER_CYCLE / 4;
+  count = 0;
+  output_level += calculatePID();
+  if (output_level > 255) {
+    output_level = 255;
+  } else if (output_level < -255) {
+    output_level = -255;
+  }
+
+  if (output_level < 0) {
+    digitalWrite(IN1, LOW);
+    digitalWrite(IN2, HIGH);
+    analogWrite(ENABLE, -output_level);
+  }
+  else {
+    digitalWrite(IN1, HIGH);
+    digitalWrite(IN2, LOW);
+    analogWrite(ENABLE, output_level);
+  }
+
+  Serial.print("goal:");
+  Serial.print(goal_rpm);
+  Serial.print(" rpm:");
+  Serial.println(current_rpm);
 }
 
-void onEncoAFalling() {
-    if (digitalRead(ENCO_B) == LOW) {
-        backward_count++;
-    }
-    else {
-        forward_count++;
-    }
-
-    // aFallingTime = micros();
-}
-
-void onEncoBFalling() {
-    // if (digitalRead(ENCO_A) == LOW) {
-    //     forward_count++;
-    // }
-    // else {
-    //     backward_count++;
-    // }
-
-    bFallingTime = micros();
+double calculatePID() {
+  diff_err = goal_rpm - current_rpm - err;
+  err = goal_rpm - current_rpm;
+  accumulate_err += err;
+  return Kp * err + Ki * accumulate_err + Kd * diff_err;
 }
 
 void onEncoAChange() {
-    if (LOW == digitalRead(ENCO_A)) {
-        if (LOW == digitalRead(ENCO_B)) {
-            // Serial.println("A 1");
-            // count--;
-            backward_count++;
-        }
-        if (HIGH == digitalRead(ENCO_B)) {
-            // Serial.println("A 2");
-            // count ++;
-            forward_count++;
-        }
+  if (LOW == digitalRead(ENCO_A)) {
+    if (LOW == digitalRead(ENCO_B)) {
+      count--;
     }
-    else {
-        if (HIGH == digitalRead(ENCO_B)) {
-            // Serial.println("A 3");
-            // count--;
-            backward_count++;
-        }
-        if (LOW == digitalRead(ENCO_B)) {
-            // Serial.println("A 4");
-            // count++;
-            forward_count++;
-        }
+    if (HIGH == digitalRead(ENCO_B)) {
+      count++;
     }
+  } else {
+    if (HIGH == digitalRead(ENCO_B)) {
+      count--;
+    }
+    if (LOW == digitalRead(ENCO_B)) {
+      count++;
+    }
+  }
 }
 
 void onEncoBChange() {
-    if (LOW == digitalRead(ENCO_B)) {
-        if (HIGH == digitalRead(ENCO_A)) {
-            // Seria/l.println("B 1");
-            // count--;
-            backward_count++;
-        }
-        if (LOW == digitalRead(ENCO_A)) {
-            // Serial.println("B 2");
-            // count++;
-            forward_count++;
-        }
+  if (LOW == digitalRead(ENCO_B)) {
+    if (HIGH == digitalRead(ENCO_A)) {
+      count--;
     }
-    else {
-        if (LOW == digitalRead(ENCO_A)) {
-            // Serial.println("B 3");
-            // count--;
-            backward_count++;
-        }
-        if (HIGH == digitalRead(ENCO_A)) {
-            // Serial.println("B 4");
-            // count++;
-            forward_count++;
-        }
+    if (LOW == digitalRead(ENCO_A)) {
+      count++;
     }
+  } else {
+    if (LOW == digitalRead(ENCO_A)) {
+      count--;
+    }
+    if (HIGH == digitalRead(ENCO_A)) {
+      count++;
+    }
+  }
 }
\ No newline at end of file