From 48f2d1aa43acd36b8063bbe7c23ac5b131fb9461 Mon Sep 17 00:00:00 2001
From: isakg22 <isakg22@ru.is>
Date: Wed, 30 Sep 2026 14:22:20 +0000
Subject: [PATCH] =?UTF-8?q?n=C3=BDtt=20branch=20fyrir=20part3=20og=20k?=
 =?UTF-8?q?=C3=B3=C3=B0ar?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 include/motor_controller.h   | 10 ++++++--
 include/p_controller.h       |  3 +++
 include/pi_controller.h      | 24 +++++++++++++++++++
 src/initialization_state.cpp |  3 +++
 src/main.cpp                 | 12 +++++++---
 src/motor_controller.cpp     |  8 ++++++-
 src/pi_controller.cpp        | 45 ++++++++++++++++++++++++++++++++++++
 7 files changed, 99 insertions(+), 6 deletions(-)
 create mode 100644 include/pi_controller.h
 create mode 100644 src/pi_controller.cpp

diff --git a/include/motor_controller.h b/include/motor_controller.h
index 770af32..e62a481 100644
--- a/include/motor_controller.h
+++ b/include/motor_controller.h
@@ -3,7 +3,8 @@
 
 #include "rotary_encoder_driver.h"
 #include "motor_driver.h"
-#include "p_controller.h"
+//#include "p_controller.h"
+#include "pi_controller.h"
 #include "digital_out.h"
 
 class State;
@@ -24,7 +25,8 @@ private:
 
     RotaryEncoderDriver& encoder;
     MotorDriver& motor;
-    P_controller& controller;
+    //P_controller& controller;
+    PI_controller& controller;
 
     Digital_out led;
 
@@ -76,6 +78,10 @@ public:
     void led_on();
 
     void led_off();
+
+    void reset_controller();
 };
 
+
+
 #endif
diff --git a/include/p_controller.h b/include/p_controller.h
index 2a6a802..a7d5813 100644
--- a/include/p_controller.h
+++ b/include/p_controller.h
@@ -10,6 +10,9 @@ public:
     P_controller(double Kp);
 
     double update(double ref, double actual);
+
+
+    
 };
 
 #endif
\ No newline at end of file
diff --git a/include/pi_controller.h b/include/pi_controller.h
new file mode 100644
index 0000000..da19f73
--- /dev/null
+++ b/include/pi_controller.h
@@ -0,0 +1,24 @@
+#ifndef PI_CONTROLLER_H
+#define PI_CONTROLLER_H
+
+class PI_controller
+{
+private:
+    double Kp;
+    double Ti;
+    double T;
+    double integral;
+
+public:
+    PI_controller(
+        double Kp,
+        double Ti,
+        double T
+    );
+
+    double update(double ref, double actual);
+
+    void reset();
+};
+
+#endif
\ No newline at end of file
diff --git a/src/initialization_state.cpp b/src/initialization_state.cpp
index 5d27d09..17389f1 100644
--- a/src/initialization_state.cpp
+++ b/src/initialization_state.cpp
@@ -15,7 +15,10 @@ void InitializationState::enter()
 
     context->reset_speed_measurement();
 
+    context->reset_controller();
+
     enter_time_ms = millis();
+
 }
 
 
diff --git a/src/main.cpp b/src/main.cpp
index 261b8e0..e43abc6 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -3,7 +3,8 @@
 
 #include "rotary_encoder_driver.h"
 #include "motor_driver.h"
-#include "p_controller.h"
+//#include "p_controller.h"
+#include "pi_controller.h"
 
 #include "motor_controller.h"
 
@@ -16,10 +17,15 @@ RotaryEncoderDriver encoder;
 
 MotorDriver motor;
 
-const double KP = 12.5;
+//const double KP = 12.5;
 
-P_controller controller(KP);
+//P_controller controller(KP);
 
+PI_controller controller(
+    12.5,           // Kp
+    1.0,            // Ti
+    0.004           // T = 4 ms
+)
 
 InitializationState initialization_state;
 
diff --git a/src/motor_controller.cpp b/src/motor_controller.cpp
index 3efb811..3a65f31 100644
--- a/src/motor_controller.cpp
+++ b/src/motor_controller.cpp
@@ -7,7 +7,8 @@
 MotorController::MotorController(
     RotaryEncoderDriver& encoder,
     MotorDriver& motor,
-    P_controller& controller,
+    //P_controller& controller,
+    PI_controller& controller;
     State& initialization,
     State& operational,
     State& stopped
@@ -140,3 +141,8 @@ void MotorController::led_off()
 {
    led.set_lo();
 }
+
+void MotorController::reset_controller()
+{
+    controller.reset();
+}
\ No newline at end of file
diff --git a/src/pi_controller.cpp b/src/pi_controller.cpp
new file mode 100644
index 0000000..de793be
--- /dev/null
+++ b/src/pi_controller.cpp
@@ -0,0 +1,45 @@
+#include "pi_controller.h"
+
+PI_controller::PI_controller(
+    double Kp,
+    double Ti,
+    double T
+)
+    : Kp(Kp),
+      Ti(Ti),
+      T(T),
+      integral(0.0)
+{
+}
+
+
+double PI_controller::update(double ref, double actual)
+{
+    double error = ref - actual;
+
+    double new_integral = integral + error * T;
+
+    double output =
+        Kp * (error + new_integral / Ti);
+
+    // Saturation + anti-windup
+    if (output > 100.0)
+    {
+        return 100.0;
+    }
+
+    if (output < -100.0)
+    {
+        return -100.0;
+    }
+
+    integral = new_integral;
+
+    return output;
+}
+
+
+void PI_controller::reset()
+{
+    integral = 0.0;
+}
\ No newline at end of file