diff --git a/Homework/Homework6/Homework6.ino b/Homework/Homework6/Homework6.ino
new file mode 100644
index 0000000..29ab0d2
--- /dev/null
+++ b/Homework/Homework6/Homework6.ino
@@ -0,0 +1,108 @@
+#include "TimerOne.h"
+#include "U8glib.h"
+
+// Drive the motor
+const byte IN1 = 10;
+const byte IN2 = 11;
+const byte ENABLE = 6;
+const byte ENCO_B = 3;
+const byte ENCO_A = 2;
+
+double output_level = 0;
+
+// PID
+double err = 0, diff_err = 0, accumulate_err = 0;
+const double Kp = 1, Ki = 0, Kd = 2.5;
+const unsigned long DO_PID_CONTROL_IN_US = 50000;
+
+// Settings for the level
+long goal_step = 0;
+long current_step = 0;
+
+const int DISTANCE_BETWEEN_LEVELS = 300;
+const int NUMBER_OF_LEVELS = 5;
+const long MAX_GOAL_STEP = DISTANCE_BETWEEN_LEVELS * NUMBER_OF_LEVELS;
+
+// OLED Args and initialization
+U8GLIB_SSD1306_128X32 u8g(U8G_I2C_OPT_NONE);
+
+const byte OLED_WIDTH = 128;
+const byte OLED_HEIGHT = 32;
+const byte PROGESS_BAR_HEIGHT = 8;
+
+void onEncoAChange() {
+  if (digitalRead(ENCO_A) == digitalRead(ENCO_B)) {
+    current_step++;
+  } else {
+    current_step--;
+  }
+
+  if ((current_step - goal_step) % DISTANCE_BETWEEN_LEVELS >
+          DISTANCE_BETWEEN_LEVELS / 2 &&
+      goal_step < MAX_GOAL_STEP) {
+    goal_step += DISTANCE_BETWEEN_LEVELS;
+  }
+  if ((goal_step - current_step) % DISTANCE_BETWEEN_LEVELS >
+          DISTANCE_BETWEEN_LEVELS / 2 &&
+      goal_step > 0) {
+    goal_step -= DISTANCE_BETWEEN_LEVELS;
+  }
+}
+
+void doPIDControl() {
+  diff_err = goal_step - current_step - err;
+  err = goal_step - current_step;
+  accumulate_err += err;
+
+  output_level = Kp * err + Ki * accumulate_err + Kd * diff_err;
+
+  if (output_level > 255) {
+    output_level = 255;
+  } else if (output_level < -255) {
+    output_level = -255;
+  }
+
+  if (output_level < 0) {
+    digitalWrite(IN1, HIGH);
+    digitalWrite(IN2, LOW);
+    analogWrite(ENABLE, -output_level);
+  } else {
+    digitalWrite(IN1, LOW);
+    digitalWrite(IN2, HIGH);
+    analogWrite(ENABLE, output_level);
+  }
+}
+
+void drawOLED() {
+  u8g.firstPage();
+  do {
+    u8g.drawFrame(0, (OLED_HEIGHT - PROGESS_BAR_HEIGHT) / 2, OLED_WIDTH,
+                  PROGESS_BAR_HEIGHT);
+    u8g.drawBox(2, (OLED_HEIGHT - PROGESS_BAR_HEIGHT) / 2 + 2,
+                (OLED_WIDTH - 4) * goal_step / MAX_GOAL_STEP,
+                PROGESS_BAR_HEIGHT - 4);
+  } while (u8g.nextPage());
+}
+
+void setup() {
+  pinMode(IN1, OUTPUT);
+  pinMode(IN2, OUTPUT);
+  pinMode(ENABLE, OUTPUT);
+  digitalWrite(IN1, HIGH);
+  digitalWrite(IN2, LOW);
+  pinMode(ENCO_B, INPUT);
+  pinMode(ENCO_A, INPUT);
+  Timer1.initialize();
+  Timer1.setPeriod(DO_PID_CONTROL_IN_US);
+  Timer1.attachInterrupt(doPIDControl);
+  attachInterrupt(digitalPinToInterrupt(ENCO_A), onEncoAChange, CHANGE);
+  Serial.begin(115200);
+}
+
+void loop() {
+  Serial.print("Goal: ");
+  Serial.print(goal_step);
+  Serial.print("  Current: ");
+  Serial.println(current_step);
+  drawOLED();
+}
\ No newline at end of file