diff --git a/include/motor_controller.h b/include/motor_controller.h
index 02dd9dc..0018626 100644
--- a/include/motor_controller.h
+++ b/include/motor_controller.h
@@ -9,6 +9,15 @@
 class State;
 
 
+// Gildi úr einni keyrslu á hraðastýringunni, til útprentunar
+struct ControlSample
+{
+    double ref_rpm;
+    double actual_rpm;
+    double pwm_actual;
+};
+
+
 class MotorController
 {
 private:
@@ -60,7 +69,8 @@ public:
     bool fault_active();
 
 
-    void run_speed_controller();
+    // Skilar true ef ný mæling var tekin og sample fyllt út
+    bool run_speed_controller(ControlSample& sample);
 
     void stop_motor();
 
diff --git a/include/operational_state.h b/include/operational_state.h
index f66ef07..a136097 100644
--- a/include/operational_state.h
+++ b/include/operational_state.h
@@ -3,9 +3,17 @@
 
 #include "state.h"
 
+#include <stdint.h>
+
 
 class OperationalState : public State
 {
+private:
+    static constexpr double SAMPLE_PERIOD_MS = 4.0;
+
+    // uint32_t svo teljarinn flæði ekki yfir eftir ~4 mín (eins og uint16_t)
+    uint32_t sample_count = 0;
+
 public:
     void enter() override;
 
diff --git a/scripts/plot_step_response.py b/scripts/plot_step_response.py
index cee95a2..1e3f085 100644
--- a/scripts/plot_step_response.py
+++ b/scripts/plot_step_response.py
@@ -1,11 +1,16 @@
 #!/usr/bin/env python3
 """
 Teiknar step response-plott ur CSV-gognum sem Arduino-verkefnid prentar ut
-(dalkar: time_ms, ref_rpm, actual_rpm, pwm_requested, pwm_actual).
+i Operational state (dalkar: time_ms, ref_rpm, actual_rpm, pwm_actual).
 
-pwm_requested = Kp * error * 2.55, oklippt (getur farid langt yfir +-255)
-pwm_actual    = sama gildi en klippt vid +-255, þ.e. þad sem raunverulega
-                var sent a motor.set() / vélbúnaðinn
+pwm_actual = stýrimerkid klippt vid +-255, þ.e. þad sem raunverulega
+             var sent a motor.set() / vélbúnaðinn
+
+Hausinn er prentadur i hvert skipti sem farid er inn i Operational, svo ein
+skra getur innihaldid nokkrar keyrslur. Adeins sidasta keyrslan er teiknud.
+
+Skriptan metur lika sveiflutima (Tu) og utslag ef kerfid sveiflast, sem
+nytist til ad finna Ku/Tu fyrir Ziegler-Nichols.
 
 Notkun:
     python plot_step_response.py [inntak.csv] [uttak.png] [titill]
@@ -14,32 +19,83 @@ Sjalfgefid: data/step_response.csv -> data/step_response.png
 """
 import sys
 import csv
+import statistics
 import matplotlib.pyplot as plt
 
 
+COLUMNS = ["time_ms", "ref_rpm", "actual_rpm", "pwm_actual"]
+
+
+def empty_cols():
+    return {key: [] for key in COLUMNS}
+
+
 def load_csv(path):
-    cols = {"time_ms": [], "ref_rpm": [], "actual_rpm": [],
-            "pwm_requested": [], "pwm_actual": []}
+    cols = empty_cols()
+    header = None
     with open(path, newline="") as f:
-        reader = csv.DictReader(f)
-        for row in reader:
-            try:
-                for key in cols:
-                    cols[key].append(float(row[key]))
-            except (ValueError, KeyError):
-                # sleppir linum sem eru ekki gild data (t.d. rusl fyrir/eftir)
-                for key in cols:
-                    if len(cols[key]) > len(cols["time_ms"]):
-                        cols[key].pop()
+        for row in csv.reader(f):
+            if row and row[0].strip() == "time_ms":
+                header = [name.strip() for name in row]
                 continue
+            if header is None:
+                # rusl a undan fyrsta haus (t.d. "State: Initialization")
+                continue
+            try:
+                values = {key: float(row[header.index(key)])
+                          for key in COLUMNS}
+            except (ValueError, IndexError):
+                # sleppir linum sem eru ekki gild data (t.d. "State: ...")
+                continue
+
+            # Timinn byrjar aftur a 0 -> ny keyrsla, hendum theirri gomlu
+            if cols["time_ms"] and values["time_ms"] <= cols["time_ms"][-1]:
+                cols = empty_cols()
+
+            for key in COLUMNS:
+                cols[key].append(values[key])
     return cols
 
 
+def estimate_oscillation(time_s, actual_rpm):
+    """
+    Metur sveiflutima ut fra uppleid i gegnum medaltal seinni helmings
+    gagnanna (fyrri helmingur er transient). Hysteresis kemur i veg fyrir
+    ad sudid i rpm-maelingunni gefi falskar skurdpunkta.
+    Skilar (Tu [s], utslag [rpm]) eda None ef engin skyr sveifla.
+    """
+    half = len(actual_rpm) // 2
+    t = time_s[half:]
+    y = actual_rpm[half:]
+    if len(y) < 20:
+        return None
+
+    mean = statistics.fmean(y)
+    band = 0.25 * statistics.pstdev(y)
+    if band == 0:
+        return None
+
+    crossings = []
+    below = y[0] < mean - band
+    for ti, yi in zip(t, y):
+        if below and yi > mean + band:
+            crossings.append(ti)
+            below = False
+        elif not below and yi < mean - band:
+            below = True
+
+    if len(crossings) < 3:
+        return None
+
+    periods = [b - a for a, b in zip(crossings, crossings[1:])]
+    amplitude = (max(y) - min(y)) / 2.0
+    return statistics.fmean(periods), amplitude
+
+
 def plot(cols, out_path, title_suffix=""):
     time_s = [t / 1000.0 for t in cols["time_ms"]]
     ref_rpm = cols["ref_rpm"]
     actual_rpm = cols["actual_rpm"]
-    pwm_requested = cols["pwm_requested"]
     pwm_actual = cols["pwm_actual"]
 
     fig, ax1 = plt.subplots(figsize=(10, 5))
@@ -53,8 +109,6 @@ def plot(cols, out_path, title_suffix=""):
     ax1.grid(True, alpha=0.3)
 
     ax2 = ax1.twinx()
-    ax2.plot(time_s, pwm_requested, label="PWM requested (unclamped)",
-              color="#d62728", linewidth=1.0, alpha=0.5, linestyle=":")
     ax2.plot(time_s, pwm_actual, label="PWM actual (clamped)",
               color="#d62728", linewidth=1.4)
     ax2.axhline(255, color="#d62728", linewidth=0.6, alpha=0.3)
@@ -69,12 +123,23 @@ def plot(cols, out_path, title_suffix=""):
         ax1.text(time_s[step_idx], max(actual_rpm) * 1.02, " step",
                   fontsize=9, va="bottom")
 
+    osc = estimate_oscillation(time_s, actual_rpm)
+    if osc is not None:
+        tu, amplitude = osc
+        print(f"Sveifla: Tu = {tu * 1000:.1f} ms, utslag = +-{amplitude:.2f} RPM")
+        ax1.text(0.01, 0.98, f"Tu ≈ {tu * 1000:.1f} ms\n"
+                              f"amplitude ≈ ±{amplitude:.2f} RPM",
+                  transform=ax1.transAxes, fontsize=9, va="top",
+                  bbox=dict(facecolor="white", alpha=0.8, edgecolor="#cccccc"))
+    else:
+        print("Engin skyr sveifla fannst i seinni helmingi gagnanna")
+
     lines1, labels1 = ax1.get_legend_handles_labels()
     lines2, labels2 = ax2.get_legend_handles_labels()
     ax1.legend(lines1 + lines2, labels1 + labels2, loc="lower right",
                fontsize=8)
 
-    title = "Double step response: RPM and PWM over time"
+    title = "Step response: RPM and PWM over time"
     if title_suffix:
         title += f" ({title_suffix})"
     fig.suptitle(title)
@@ -84,8 +149,10 @@ def plot(cols, out_path, title_suffix=""):
 
 
 if __name__ == "__main__":
-    in_path = sys.argv[1] if len(sys.argv) > 1 else "data/twostep.csv"
-    out_path = sys.argv[2] if len(sys.argv) > 2 else "data/twostep.png"
+    in_path = sys.argv[1] if len(sys.argv) > 1 else "data/step_response.csv"
+    out_path = sys.argv[2] if len(sys.argv) > 2 else "data/step_response.png"
     title_suffix = sys.argv[3] if len(sys.argv) > 3 else ""
     data = load_csv(in_path)
+    if not data["time_ms"]:
+        sys.exit(f"Engin gogn fundust i {in_path}")
     plot(data, out_path, title_suffix)
diff --git a/src/main.cpp b/src/main.cpp
index 48ff325..7d67ae3 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -19,9 +19,9 @@ RotaryEncoderDriver encoder;
 
 MotorDriver motor;
 
-const double Kp = 12.5;
-const double Ti = 1.0;
-const double T = 0.004;
+const double Kp = 45.0; // Ziegler-Nichols Method 45.0
+const double Ti = 0.0; // 0.226
+const double T = 0.0; // 0.004
 
 // P_controller controller(Kp);
 PI_controller controller(Kp, Ti, T);
diff --git a/src/motor_controller.cpp b/src/motor_controller.cpp
index 5d72809..585ea45 100644
--- a/src/motor_controller.cpp
+++ b/src/motor_controller.cpp
@@ -92,20 +92,39 @@ void MotorController::to_operational()
 }
 
 
-void MotorController::run_speed_controller()
+bool MotorController::run_speed_controller(ControlSample& sample)
 {
-    if (encoder.new_speed_available())
+    if (!encoder.new_speed_available())
     {
-        double actual = encoder.rpm();
-
-        double pwm_percent =
-            controller.update(reference_rpm, actual);
-
-        double pwm =
-            pwm_percent * PWM_PER_PERCENT;
-
-        motor.set(pwm);
+        return false;
     }
+
+    double actual = encoder.rpm();
+
+    double pwm_percent =
+        controller.update(reference_rpm, actual);
+
+    double pwm =
+        pwm_percent * PWM_PER_PERCENT;
+
+    // Klippt hér svo pwm_actual sé nákvæmlega það sem fer á mótorinn
+    if (pwm > 255.0)
+    {
+        pwm = 255.0;
+    }
+
+    if (pwm < -255.0)
+    {
+        pwm = -255.0;
+    }
+
+    motor.set(pwm);
+
+    sample.ref_rpm = reference_rpm;
+    sample.actual_rpm = actual;
+    sample.pwm_actual = pwm;
+
+    return true;
 }
 
 
diff --git a/src/operational_state.cpp b/src/operational_state.cpp
index b081512..dbeb5a7 100644
--- a/src/operational_state.cpp
+++ b/src/operational_state.cpp
@@ -11,6 +11,11 @@ void OperationalState::enter()
     context->reset_controller();
 
     context->led_on();
+
+    // Tíminn byrjar á 0 í hvert skipti sem farið er inn í Operational
+    sample_count = 0;
+
+    Serial.println("time_ms,ref_rpm,actual_rpm,pwm");
 }
 
 // Part 1 
@@ -33,7 +38,20 @@ void OperationalState::update()
         return;
     }
 
-    context->run_speed_controller();
+    ControlSample sample;
+
+    if (context->run_speed_controller(sample))
+    {
+        sample_count++;
+
+        Serial.print(sample_count * SAMPLE_PERIOD_MS, 1);
+        Serial.print(",");
+        Serial.print(sample.ref_rpm, 1);
+        Serial.print(",");
+        Serial.print(sample.actual_rpm, 2);
+        Serial.print(",");
+        Serial.println(sample.pwm_actual, 1);
+    }
 }