diff --git a/include/motor_driver.h b/include/motor_driver.h
index 3527827..d69cc6d 100644
--- a/include/motor_driver.h
+++ b/include/motor_driver.h
@@ -11,6 +11,8 @@ public:
     void forward();
     void reverse();
     void stop();
+
+    void set(double pwm);
 };
 
 #endif
\ No newline at end of file
diff --git a/include/p_controller.h b/include/p_controller.h
new file mode 100644
index 0000000..2a6a802
--- /dev/null
+++ b/include/p_controller.h
@@ -0,0 +1,15 @@
+#ifndef P_CONTROLLER_H
+#define P_CONTROLLER_H
+
+class P_controller
+{
+private:
+    double Kp;
+
+public:
+    P_controller(double Kp);
+
+    double update(double ref, double actual);
+};
+
+#endif
\ No newline at end of file
diff --git a/platformio.ini b/platformio.ini
index 8ba220f..96d263e 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -12,3 +12,5 @@
 platform = atmelavr
 board = nanoatmega328new
 framework = arduino
+
+monitor_speed = 115200
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 6536c11..d70fe98 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,4 +1,4 @@
-#include <avr/io.h>
+/* #include <avr/io.h>
 #include <util/delay.h>
 #include <Arduino.h>
 
@@ -76,3 +76,4 @@ int main() {
     }
     return 0;
 }
+ */
\ No newline at end of file
diff --git a/src/motor_driver.cpp b/src/motor_driver.cpp
index bb62543..8dae91a 100644
--- a/src/motor_driver.cpp
+++ b/src/motor_driver.cpp
@@ -16,7 +16,27 @@ void MotorDriver::begin()
     DDRD |= (1 << DDD3);
     DDRB |= (1 << DDB3);
 
-    stop();
+    // Timer2 Fast PWM
+    TCCR2A = 0;
+    TCCR2B = 0;
+
+    // Fast PWM mode
+    TCCR2A |= (1 << WGM20);
+    TCCR2A |= (1 << WGM21);
+
+    // Non-inverting PWM on OC2A and OC2B
+    TCCR2A |= (1 << COM2A1);
+    TCCR2A |= (1 << COM2B1);
+
+
+    // Timer2 prescaler = 32
+    TCCR2B |= (1 << CS21);
+    TCCR2B |= (1 << CS20);
+
+    OCR2A = 0;
+    OCR2B = 0;
+
+    //stop();
 }
 
 
@@ -25,8 +45,11 @@ void MotorDriver::forward()
     // AIN1 / D3  = HIGH
     // AIN2 / D11 = LOW
 
-    PORTD |= (1 << PORTD3);
-    PORTB &= ~(1 << PORTB3);
+    //PORTD |= (1 << PORTD3);
+    //PORTB &= ~(1 << PORTB3);
+
+    OCR2A = 0;
+    OCR2B = 255;
 }
 
 
@@ -35,8 +58,11 @@ void MotorDriver::reverse()
     // AIN1 / D3  = LOW
     // AIN2 / D11 = HIGH
 
-    PORTD &= ~(1 << PORTD3);
-    PORTB |= (1 << PORTB3);
+    //PORTD &= ~(1 << PORTD3);
+    //PORTB |= (1 << PORTB3);
+
+    OCR2B = 0;
+    OCR2A = 255;
 }
 
 
@@ -44,6 +70,41 @@ void MotorDriver::stop()
 {
     // Both LOW
 
-    PORTD &= ~(1 << PORTD3);
-    PORTB &= ~(1 << PORTB3);
+    //PORTD &= ~(1 << PORTD3);
+    //PORTB &= ~(1 << PORTB3);
+
+    OCR2A = 0;
+    OCR2B = 0;
+}
+
+void MotorDriver::set(double pwm)
+{
+    // Limit PWM command
+    if (pwm > 255.0)
+    {
+        pwm = 255.0;
+    }
+
+    if (pwm < -255.0)
+    {
+        pwm = -255.0;
+    }
+
+
+    if (pwm > 0)
+    {
+        // Forward
+        OCR2A = 0;
+        OCR2B = (uint8_t)pwm;
+    }
+    else if (pwm < 0)
+    {
+        // Reverse
+        OCR2B = 0;
+        OCR2A = (uint8_t)(-pwm);
+    }
+    else
+    {
+        stop();
+    }
 }
\ No newline at end of file
diff --git a/src/p_controller.cpp b/src/p_controller.cpp
new file mode 100644
index 0000000..616207b
--- /dev/null
+++ b/src/p_controller.cpp
@@ -0,0 +1,13 @@
+#include "p_controller.h"
+
+P_controller::P_controller(double Kp)
+    : Kp(Kp)
+{
+}
+
+double P_controller::update(double ref, double actual)
+{
+    double error = ref - actual;
+
+    return Kp * error;
+}
\ No newline at end of file
diff --git a/src/temporary_main.cpp b/src/temporary_main.cpp
new file mode 100644
index 0000000..b74df73
--- /dev/null
+++ b/src/temporary_main.cpp
@@ -0,0 +1,59 @@
+/* #include <Arduino.h>
+
+#include "p_controller.h"
+
+P_controller controller(2.0);
+
+int main()
+{
+    init();
+
+    Serial.begin(115200);
+
+    double result = controller.update(100.0, 70.0);
+
+    Serial.print("PWM = ");
+    Serial.println(result);
+
+    while (true)
+    {
+    }
+
+    return 0;
+} */
+
+
+#include <Arduino.h>
+#include <util/delay.h>
+
+#include "motor_driver.h"
+
+MotorDriver motor;
+
+int main()
+{
+    init();
+
+    motor.begin();
+
+    //_delay_ms(3000);
+    //motor.set(200);
+
+    while (true)
+    {
+        motor.set(60);
+        _delay_ms(3000);
+
+        motor.set(120);
+        _delay_ms(3000);
+
+        motor.set(240);
+        _delay_ms(3000);
+
+        //motor.stop();
+        //_delay_ms(2000);
+
+    }
+        
+
+}
\ No newline at end of file