diff --git a/Project_2_Speed_Controller/Part_4_Speed_Controller/include/p_controller.h b/Project_2_Speed_Controller/Part_4_Speed_Controller/include/p_controller.h
new file mode 100644
index 0000000..c7666d1
--- /dev/null
+++ b/Project_2_Speed_Controller/Part_4_Speed_Controller/include/p_controller.h
@@ -0,0 +1,14 @@
+#ifndef P_CONTROLLER_H
+#define P_CONTROLLER_H
+
+class P_controller
+{
+public:
+    P_controller(double kp);
+    double update(double ref, double actual);
+
+private:
+    double kp;
+};
+
+#endif
\ No newline at end of file
diff --git a/Project_2_Speed_Controller/Part_4_Speed_Controller/src/main.cpp b/Project_2_Speed_Controller/Part_4_Speed_Controller/src/main.cpp
index c53e82e..ed80cb9 100644
--- a/Project_2_Speed_Controller/Part_4_Speed_Controller/src/main.cpp
+++ b/Project_2_Speed_Controller/Part_4_Speed_Controller/src/main.cpp
@@ -1,57 +1,168 @@
 #include <Arduino.h>
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
 #include <encoder.h>
-#include <digital_out.h>
+#include <p_controller.h>
+
+Encoder encoder(0, 1, 5);
+P_controller controller(40.0);
+
+volatile bool updateFlag = false;
+
+ISR(TIMER1_COMPA_vect)
+{
+    updateFlag = true;
+}
 
 int main()
 {
     init();
 
-    Encoder encoder(0, 1, 5);
-
-    Digital_out ain1(3);   // D11
-    Digital_out ain2(4);   // D12
-
     encoder.init();
 
-    ain1.init();
-    ain2.init();
-
-    // Motor initially OFF
-    ain1.set_lo();
-    ain2.set_lo();
-
     Serial.begin(115200);
 
-    // Give time to open the serial monitor
-    delay(3000);
+    // D11 / PB3 -> AIN1
+    DDRB |= (1 << PB3);
+    PORTB &= ~(1 << PB3);
 
-    Serial.println("time_ms,rpm");
+    // D3 / PD3 / OC2B -> AIN2 PWM
+    DDRD |= (1 << PD3);
 
-    // Fresh baseline for speed calculation
-    encoder.speed();
+    // Timer1: 250 Hz
+    TCCR1A = 0;
+    TCCR1B = 0;
+    TCNT1 = 0;
 
-    uint32_t startTime = micros();
+    OCR1A = 999;
 
-    // Step input: motor OFF -> full voltage
-    // This direction should give positive RPM
-    ain1.set_lo();
-    ain2.set_hi();
+    TCCR1B |= (1 << WGM12);
+    TCCR1B |= (1 << CS11) | (1 << CS10);
 
-    while ((micros() - startTime) < 4000000UL)
+    TIMSK1 |= (1 << OCIE1A);
+
+    // Timer2: 7.8 kHz PWM
+    TCCR2A = 0;
+    TCCR2B = 0;
+    TCNT2 = 0;
+
+    OCR2B = 0;
+
+    TCCR2A =
+        (1 << COM2B1) |
+        (1 << WGM21) |
+        (1 << WGM20);
+
+    TCCR2B = (1 << CS21);
+
+    sei();
+
+    Serial.println("Load test starting in 2 seconds...");
+
+    uint16_t waitCount = 0;
+
+    while (waitCount < 500)
     {
-        int rpm = encoder.speed();
-        uint32_t time_ms = (micros() - startTime) / 1000UL;
-
-        Serial.print(time_ms);
-        Serial.print(",");
-        Serial.println(rpm);
-
-        delay(10);
+        if (updateFlag)
+        {
+            updateFlag = false;
+            waitCount++;
+        }
     }
 
-    // Stop motor
-    ain1.set_lo();
-    ain2.set_lo();
+    int previousCount = encoder.position();
+
+    int countHistory[10] = {0};
+    int rollingCounts = 0;
+
+    uint8_t historyIndex = 0;
+    uint8_t samplesFilled = 0;
+
+    uint32_t updateCount = 0;
+
+    const double reference = 50.0;
+
+    Serial.println("time_ms,reference_rpm,actual_rpm,pwm");
+
+    while (1)
+    {
+        if (updateFlag)
+        {
+            updateFlag = false;
+            updateCount++;
+
+            uint32_t time_ms = updateCount * 4;
+
+            int currentCount = encoder.position();
+
+            int deltaCount = currentCount - previousCount;
+            previousCount = currentCount;
+
+            rollingCounts -= countHistory[historyIndex];
+
+            countHistory[historyIndex] = deltaCount;
+            rollingCounts += deltaCount;
+
+            historyIndex++;
+
+            if (historyIndex >= 10)
+            {
+                historyIndex = 0;
+            }
+
+            if (samplesFilled < 10)
+            {
+                samplesFilled++;
+            }
+
+            double windowTime = samplesFilled * 0.004;
+
+            double rpm =
+                (rollingCounts * 60.0) /
+                (2100.0 * windowTime);
+
+            double output =
+                controller.update(reference, rpm);
+
+            if (output < 0.0)
+            {
+                output = 0.0;
+            }
+
+            if (output > 255.0)
+            {
+                output = 255.0;
+            }
+
+            uint8_t pwm = (uint8_t)output;
+
+            OCR2B = pwm;
+
+            if ((updateCount % 10) == 0)
+            {
+                Serial.print(time_ms);
+                Serial.print(",");
+
+                Serial.print(reference);
+                Serial.print(",");
+
+                Serial.print(rpm);
+                Serial.print(",");
+
+                Serial.println(pwm);
+            }
+
+            if (time_ms >= 10000)
+            {
+                OCR2B = 0;
+
+                Serial.println("TEST COMPLETE");
+
+                break;
+            }
+        }
+    }
 
     while (1)
     {
diff --git a/Project_2_Speed_Controller/Part_4_Speed_Controller/src/p_controller.cpp b/Project_2_Speed_Controller/Part_4_Speed_Controller/src/p_controller.cpp
new file mode 100644
index 0000000..4536a71
--- /dev/null
+++ b/Project_2_Speed_Controller/Part_4_Speed_Controller/src/p_controller.cpp
@@ -0,0 +1,11 @@
+#include <p_controller.h>
+
+P_controller::P_controller(double kp)
+    : kp(kp)
+{
+}
+
+double P_controller::update(double ref, double actual)
+{
+    return kp * (ref - actual);
+}
\ No newline at end of file