Byrjaði á kóðum fyrir part 3 en lenti í veseni með test kóðann

.gitignore

@@ -4,8 +4,6 @@ __pycache__
 *.csv
 *.typ
 *.pdf
-scripts
-*.bak
 
 # Vélarsértækar skrár sem PlatformIO býr til sjálft
 .vscode/c_cpp_properties.json

docs/diagrams/src/motor_state.wsd

@@ -11,17 +11,12 @@ skinparam state {
 
 state Initialization 
     Initialization : Entry / Stop motor
-    Initialization : Entry / Initialize internal parameters
-    Initialization : Entry / Reset speed measurement
+    Initialization : Do / Initialize internal parameters
+    Initialization : Do / Reset speed measurement
     Initialization : Do / Wait 2 seconds
     Initialization : Exit / Send Boot-up message
 
-state PreOperational
-    PreOperational : Entry / Stop motor
-    PreOperational : Do / Blink LED at 1 Hz
-    PreOperational : Do / Accept Kp and Ti configuration
-    PreOperational : Do / Select P or PI controller [c]
-    PreOperational : Exit / LED off
+
 
 state Operational 
     Operational : Entry / LED on
@@ -33,18 +28,13 @@ state Stopped
     Stopped : Do / Blink LED at 2 Hz
     Stopped : Exit / LED off
 
-Initialization -r-> PreOperational : initialization\n complete
-' Initialization -d-> Operational : initialization\n complete
 
-PreOperational -d-> Operational : set operational [o]
-PreOperational -l--> Initialization : reset [r]
 
-Operational -u-> PreOperational : set pre-operational [p]
-Operational -> Initialization : reset [r]
+Initialization --> Operational : initialization\n complete
+Operational --> Initialization : reset
 Operational --> Stopped : fault detected
 
-Stopped -l-> Initialization : reset [r]
-Stopped -> Operational : set operational [o]
-Stopped -u-> PreOperational : set pre-operational [p]
+Stopped --> Initialization : reset
+Stopped --> Operational : set operational
 
 @enduml

include/controller.h

@@ -0,0 +1,18 @@
+#ifndef CONTROLLER_H
+#define CONTROLLER_H
+
+class Controller
+{
+public:
+    virtual ~Controller()
+    {
+    }
+
+    virtual double update(double ref, double actual) = 0;
+
+    virtual void reset()
+    {
+    }
+};
+
+#endif

include/initialization_state.h

@@ -7,7 +7,7 @@
 class InitializationState : public State
 {
 private:
-    static const unsigned long WAIT_MS = 2000;
+    static const unsigned long WAIT_MS = 2000UL;
 
     unsigned long enter_time_ms = 0;
 

include/motor_controller.h

@@ -3,7 +3,9 @@
 
 #include "rotary_encoder_driver.h"
 #include "motor_driver.h"
-#include "p_controller.h"
+//#include "p_controller.h"
+//#include "pi_controller.h"
+#include "controller.h"
 #include "digital_out.h"
 
 class State;
@@ -19,35 +21,39 @@ private:
     State* current_state;
 
     State& initialization_state;
-    State& pre_operational_state;
     State& operational_state;
     State& stopped_state;
 
     RotaryEncoderDriver& encoder;
     MotorDriver& motor;
-    P_controller* controller;
+    //P_controller& controller;
+    //PI_controller& controller;
+    Controller& controller;
 
     Digital_out led;
 
     double reference_rpm;
 
-    // Síðasta sýni, notað fyrir CSV-útprentun
-    double last_actual_rpm;
-    double last_pwm;
-
 
 public:
     MotorController(
         RotaryEncoderDriver& encoder,
         MotorDriver& motor,
-        P_controller& controller,
+        //P_controller& controller,
+        Controller& controller,
         State& initialization,
-        State& pre_operational,
         State& operational,
         State& stopped
     );
 
 
+    // void set_states(
+    //     State* initialization,
+    //     State* operational,
+    //     State* stopped
+    // );
+
+
     void begin();
 
     void update();
@@ -59,8 +65,6 @@ public:
 
     void to_initialization();
 
-    void to_pre_operational();
-
     void to_operational();
 
     void to_stopped();
@@ -68,12 +72,7 @@ public:
     bool fault_active();
 
 
-    // Skilar true þegar nýtt sýni var keyrt í gegnum stýringuna
-    bool run_speed_controller();
-
-    double get_reference_rpm() const { return reference_rpm; }
-    double get_actual_rpm() const { return last_actual_rpm; }
-    double get_pwm() const { return last_pwm; }
+    void run_speed_controller();
 
     void stop_motor();
 
@@ -81,15 +80,15 @@ public:
 
     void reset_controller();
 
-    void set_Kp(double value);
 
-    void set_Ti(double value);
-
-    void set_controller(P_controller& new_controller);
 
     void led_on();
 
     void led_off();
+
+    
 };
 
+
+
 #endif

include/operational_state.h

@@ -2,20 +2,10 @@
 #define OPERATIONAL_STATE_H
 
 #include "state.h"
-#include <stdint.h>
-
-// Sett á 1 til að prenta CSV (time_ms,ref_rpm,actual_rpm,pwm) í Operational,
-// t.d. fyrir step og load response. Einnig hægt með build_flags = -D LOG_CSV=1
-#ifndef LOG_CSV
-#define LOG_CSV 0
-#endif
 
 
 class OperationalState : public State
 {
-private:
-    uint32_t sample_count = 0;
-
 public:
     void enter() override;
 

include/p_controller.h

@@ -1,26 +1,20 @@
 #ifndef P_CONTROLLER_H
 #define P_CONTROLLER_H
 
-class P_controller
+#include "controller.h"
+
+class P_controller : public Controller
 {
-protected:
+private:
     double Kp;
 
 public:
     P_controller(double Kp);
-    virtual ~P_controller() {}
-    virtual double update(double ref, double actual);
-    virtual void reset() {}
 
-    virtual void set_Kp(double value)
-    {
-        Kp = value;
-    }
+    double update(double ref, double actual) override;
+
+
     
-    virtual void set_Ti(double value)
-    {
-        // P controller does not use Ti
-    }
 };
 
 #endif

include/pi_controller.h

@@ -1,23 +1,27 @@
 #ifndef PI_CONTROLLER_H
 #define PI_CONTROLLER_H
 
-#include "p_controller.h"
+#include "controller.h"
 
-class PI_controller : public P_controller
+class PI_controller : public Controller
 {
-    private:
-        static constexpr double U_MAX = 100.0;  // %
-        static constexpr double U_MIN = -100.0; // %
+private:
+    double Kp;
+    double Ti;
+    double T;
 
-        double Ti;
-        double T;
-        double integral;
-    public:
-        PI_controller(double Kp, double Ti, double T);
-        double update(double ref, double actual) override;
-        void reset() override;
+    double integral;
 
-        void set_Ti(double value) override;
+public:
+    PI_controller(
+        double Kp,
+        double Ti,
+        double T
+    );
+
+    double update(double ref, double actual) override;
+
+    void reset() override;
 };
 
 #endif

include/pre_operational_state.h

@@ -1,43 +0,0 @@
-#ifndef PRE_OPERATIONAL_STATE_H
-#define PRE_OPERATIONAL_STATE_H
-
-#include "state.h"
-#include "stdint.h"
-
-class P_controller;
-
-class PreOperationalState : public State
-{
-private:
-    P_controller& p_controller;
-    P_controller& pi_controller;
-    bool pi_selected;
-
-    unsigned long previous_time;
-    bool led_state;
-
-    //bool waiting_for_kp;
-    //bool waiting_for_ti;
-
-    enum InputMode
-    {
-        NONE,
-        ENTER_KP,
-        ENTER_TI
-    };
-
-    InputMode input_mode;
-
-    char input_buffer[16];
-    uint8_t input_index;
-
-public:
-    PreOperationalState(P_controller& p_controller, P_controller& pi_controller);
-
-    void enter() override;
-    void update() override;
-    void command(char c) override;
-    void exit() override;
-};
-
-#endif

platformio.ini

@@ -16,6 +16,3 @@ framework = arduino
 monitor_speed = 115200
 test_framework = unity
 test_build_src = yes
-
-; CSV-útprentun í Operational fyrir step/load response (sjá capture_serial.py)
-; build_flags = -D LOG_CSV=1

src/initialization_state.cpp

@@ -15,7 +15,10 @@ void InitializationState::enter()
 
     context->reset_speed_measurement();
 
+    context->reset_controller();
+
     enter_time_ms = millis();
+
 }
 
 
@@ -24,8 +27,7 @@ void InitializationState::update()
     // Non-blocking bið svo aðallykkjan haldi áfram að keyra
     if (millis() - enter_time_ms >= WAIT_MS)
     {
-        context->to_pre_operational();
-        // context->to_operational();
+        context->to_operational();
     }
 }
 

src/main.cpp

@@ -1,19 +1,14 @@
-// main.cpp er sleppt i test-build (pio test) svo setup()/loop() rekist ekki a prof
-#ifndef PIO_UNIT_TESTING
-
 #include <Arduino.h>
 #include <avr/interrupt.h>
 
 #include "rotary_encoder_driver.h"
 #include "motor_driver.h"
+//#include "p_controller.h"
 #include "pi_controller.h"
 
 #include "motor_controller.h"
 
 #include "initialization_state.h"
-
-#include "pre_operational_state.h"
-
 #include "operational_state.h"
 #include "stopped_state.h"
 
@@ -22,29 +17,28 @@ RotaryEncoderDriver encoder;
 
 MotorDriver motor;
 
-const double Kp_P = 10.0;     // P-controller from Project 2
+//const double KP = 12.5;
 
-const double Kp = 10.125;     // Ziegler-Nichols PI
-const double Ti = 0.0334;
-const double T = 0.004;
-
-P_controller p_controller(Kp_P);
-PI_controller pi_controller(Kp, Ti, T);
+//P_controller controller(KP);
 
+PI_controller controller(
+    12.5,           // Kp
+    1.0,            // Ti
+    0.004           // T = 4 ms
+);
 
 InitializationState initialization_state;
-// Pre-operational velur á milli stýringanna með 'c'
-PreOperationalState pre_operational_state(p_controller, pi_controller);
+
 OperationalState operational_state;
+
 StoppedState stopped_state;
 
 // Stöðurnar verða að vera skilgreindar á undan motor_controller
 MotorController motor_controller(
     encoder,
     motor,
-    pi_controller,      // Sjálfgefin stýring
+    controller,
     initialization_state,
-    pre_operational_state,
     operational_state,
     stopped_state
 );
@@ -53,24 +47,39 @@ MotorController motor_controller(
 int main()
 {
     init();
+
     Serial.begin(115200);
+
+
     cli();
 
     encoder.begin();
+
     motor.begin();
 
     sei();
 
+
+    // motor_controller.set_states(
+    //     &initialization_state,
+    //     &operational_state,
+    //     &stopped_state
+    // );
+
+
     motor_controller.begin();
 
+
     while (true)
     {
         if (Serial.available() > 0)
         {
             char c = Serial.read();
 
-            motor_controller.command(c);   
-
+            if (c != '\n' && c != '\r')
+            {
+                motor_controller.command(c);
+            }
         }
 
 
@@ -81,4 +90,6 @@ int main()
     return 0;
 }
 
-#endif 
+// int main(){
+//     return 0;
+// }

src/motor_controller.cpp

@@ -7,27 +7,24 @@
 MotorController::MotorController(
     RotaryEncoderDriver& encoder,
     MotorDriver& motor,
-    P_controller& controller,
+    //P_controller& controller,
+    //PI_controller& controller;
+    Controller& controller,
     State& initialization,
-    State& pre_operational,
     State& operational,
     State& stopped
 )
     : current_state(nullptr),
       initialization_state(initialization),
-      pre_operational_state(pre_operational),
       operational_state(operational),
       stopped_state(stopped),
       encoder(encoder),
       motor(motor),
-      controller(&controller),
+      controller(controller),
       led(LED_PIN),
-      reference_rpm(DEFAULT_REFERENCE_RPM),
-      last_actual_rpm(0.0),
-      last_pwm(0.0)
+      reference_rpm(DEFAULT_REFERENCE_RPM)
 {
     initialization_state.set_context(this);
-    pre_operational_state.set_context(this);
     operational_state.set_context(this);
     stopped_state.set_context(this);
 }
@@ -63,6 +60,8 @@ void MotorController::transition_to(State& new_state)
 
     current_state = &new_state;
 
+    current_state->set_context(this);
+
     current_state->enter();
 }
 
@@ -90,11 +89,6 @@ void MotorController::to_initialization()
     transition_to(initialization_state);
 }
 
-void MotorController::to_pre_operational()
-{
-    transition_to(pre_operational_state);
-}
-
 
 void MotorController::to_operational()
 {
@@ -102,37 +96,20 @@ void MotorController::to_operational()
 }
 
 
-bool MotorController::run_speed_controller()
+void MotorController::run_speed_controller()
 {
-    if (!encoder.new_speed_available())
+    if (encoder.new_speed_available())
     {
-        return false;
+        double actual = encoder.rpm();
+
+        double pwm_percent =
+            controller.update(reference_rpm, actual);
+
+        double pwm =
+            pwm_percent * PWM_PER_PERCENT;
+
+        motor.set(pwm);
     }
-
-    double actual = encoder.rpm();
-
-    double pwm_percent =
-        controller->update(reference_rpm, actual);
-
-    double pwm =
-        pwm_percent * PWM_PER_PERCENT;
-
-    // Klippt hér svo last_pwm sé nákvæmlega það sem fer á mótorinn
-    if (pwm > 255.0)
-    {
-        pwm = 255.0;
-    }
-    else if (pwm < -255.0)
-    {
-        pwm = -255.0;
-    }
-
-    motor.set(pwm);
-
-    last_actual_rpm = actual;
-    last_pwm = pwm;
-
-    return true;
 }
 
 
@@ -147,28 +124,6 @@ void MotorController::reset_speed_measurement()
     encoder.reset_speed_measurement();
 }
 
-void MotorController::reset_controller()
-{
-    controller->reset();
-}
-
-void MotorController::set_Kp(double value)
-{
-    controller->set_Kp(value);
-}
-
-void MotorController::set_Ti(double value)
-{
-    controller->set_Ti(value);
-}
-
-void MotorController::set_controller(P_controller& new_controller)
-{
-    controller = &new_controller;
-
-    controller->reset();
-}
-
 void MotorController::to_stopped()
 {
     transition_to(stopped_state);
@@ -179,6 +134,11 @@ bool MotorController::fault_active()
     return motor.fault_active();
 }
 
+void MotorController::reset_controller()
+{
+    controller.reset();
+}
+
 void MotorController::led_on()
 {
    led.set_hi();
@@ -189,3 +149,4 @@ void MotorController::led_off()
 {
    led.set_lo();
 }
+

src/operational_state.cpp

@@ -8,15 +8,7 @@ void OperationalState::enter()
 {
     Serial.println("State: Operational");
 
-    context->reset_controller();
-
     context->led_on();
-
-    sample_count = 0;
-
-#if LOG_CSV
-    Serial.println("time_ms,ref_rpm,actual_rpm,pwm");
-#endif
 }
 
 // Part 1 
@@ -39,21 +31,7 @@ void OperationalState::update()
         return;
     }
 
-    if (context->run_speed_controller())
-    {
-        sample_count++;
-
-#if LOG_CSV
-        // 4 ms á milli sýna, tíminn byrjar á 0 við hverja innkomu í Operational
-        Serial.print(sample_count * 4UL);
-        Serial.print(',');
-        Serial.print(context->get_reference_rpm(), 1);
-        Serial.print(',');
-        Serial.print(context->get_actual_rpm(), 2);
-        Serial.print(',');
-        Serial.println(context->get_pwm(), 1);
-#endif
-    }
+    context->run_speed_controller();
 }
 
 
@@ -65,13 +43,6 @@ void OperationalState::command(char c)
 
         context->to_initialization();
     }
-
-    else if (c == 'p' || c == 'P')
-    {
-        Serial.println("Command: set pre-operational");
-
-        context->to_pre_operational();
-    }
 }
 
 

src/pi_controller.cpp

@@ -1,37 +1,57 @@
 #include "pi_controller.h"
 
-PI_controller::PI_controller(double Kp, double Ti, double T) : P_controller(Kp), Ti(Ti), T(T), integral(0.0)
+
+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 e = ref - actual;
-    double new_integral = integral + e * T;
-    double u = Kp * (e + new_integral / Ti);
-
-    if (u > U_MAX)
-    {
-        u = U_MAX;
-    }
-    else if (u < U_MIN)
-    {
-        u = U_MIN;
-    }
-    else
-    {
-        integral = new_integral;
-    }
-    return u;
-    // return 0.0;
+    return 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);
+
+
+//     // Positive saturation
+//     if (output > 100.0)
+//     {
+//         return 100.0;
+//     }
+
+
+//     // Negative saturation
+//     if (output < -100.0)
+//     {
+//         return -100.0;
+//     }
+
+
+//     // Only save the integral if the controller
+//     // is not saturated
+//     integral = new_integral;
+
+//     return output;
+// }
+
+
 void PI_controller::reset()
 {
     integral = 0.0;
 }
-
-void PI_controller::set_Ti(double value)
-{
-    Ti = value;
-}

src/pre_operational_state.cpp

@@ -1,161 +0,0 @@
-#include "pre_operational_state.h"
-#include "motor_controller.h"
-
-#include <Arduino.h>
-#include <stdlib.h>
-
-PreOperationalState::PreOperationalState(P_controller& p_controller, P_controller& pi_controller)
-    : p_controller(p_controller),
-      pi_controller(pi_controller),
-      pi_selected(true),
-      previous_time(0),
-      led_state(false),
-      //waiting_for_kp(false),
-      //waiting_for_ti(false),
-      input_mode(NONE),
-      input_index(0)
-{
-}
-
-void PreOperationalState::enter()
-{
-    Serial.println("State: Pre-operational");
-
-    context->stop_motor();
-
-    context->led_off();
-
-    led_state = false;
-
-    previous_time = millis();
-}
-
-void PreOperationalState::update()
-{
-    unsigned long current_time = millis();
-
-    if (current_time - previous_time >= 500)
-    {
-        previous_time = current_time;
-
-        led_state = !led_state;
-
-        if (led_state)
-        {
-            context->led_on();
-        }
-        else
-        {
-            context->led_off();
-        }
-    }
-}
-
-void PreOperationalState::command(char c)
-{
-    // We are currently entering a Kp or Ti value
-    if (input_mode != NONE)
-    {
-        // Enter pressed
-        if (c == '\n' || c == '\r')
-        {
-            if (input_index > 0)
-            {
-                // End the string
-                input_buffer[input_index] = '\0';
-
-                // Convert text to a number
-                double value = atof(input_buffer);
-
-                if (input_mode == ENTER_KP)
-                {
-                    context->set_Kp(value);
-
-                    Serial.print("Kp set to: ");
-                    Serial.println(value);
-                }
-                else if (input_mode == ENTER_TI)
-                {
-                    context->set_Ti(value);
-
-                    Serial.print("Ti set to: ");
-                    Serial.println(value);
-                }
-            }
-
-            // Finished entering a value
-            input_index = 0;
-            input_mode = NONE;
-
-            return;
-        }
-
-        // Store number characters
-        if ((c >= '0' && c <= '9') ||
-            c == '.' ||
-            c == '-')
-        {
-            if (input_index < sizeof(input_buffer) - 1)
-            {
-                input_buffer[input_index] = c;
-                input_index++;
-            }
-        }
-
-        return;
-    }
-
-
-    // Normal state commands
-    if (c == 'o' || c == 'O')
-    {
-        Serial.println("Command: set operational");
-
-        context->to_operational();
-    }
-
-    else if (c == 'r' || c == 'R')
-    {
-        Serial.println("Command: reset");
-
-        context->to_initialization();
-    }
-
-    else if (c == 'k' || c == 'K')
-    {
-        input_mode = ENTER_KP;
-        input_index = 0;
-
-        Serial.println("Enter Kp and press Enter:");
-    }
-
-    else if (c == 't' || c == 'T')
-    {
-        input_mode = ENTER_TI;
-        input_index = 0;
-
-        Serial.println("Enter Ti and press Enter:");
-    }
-
-    // Select controller algorithm, the controller is passed to the motor controller
-    else if (c == 'c' || c == 'C')
-    {
-        pi_selected = !pi_selected;
-
-        if (pi_selected)
-        {
-            context->set_controller(pi_controller);
-            Serial.println("Controller: PI");
-        }
-        else
-        {
-            context->set_controller(p_controller);
-            Serial.println("Controller: P");
-        }
-    }
-}
-
-void PreOperationalState::exit()
-{
-    context->led_off();
-}

src/stopped_state.cpp

@@ -19,7 +19,7 @@ void StoppedState::enter()
 
     led_state = false;
 
-    context->led_off();
+    digitalWrite(LED_BUILTIN, LOW);
 
     previous_time = millis();
 }
@@ -36,14 +36,10 @@ void StoppedState::update()
 
         led_state = !led_state;
 
-        if (led_state)
-        {
-            context->led_on();
-        }
-        else
-        {
-            context->led_off();
-        }
+        digitalWrite(
+            LED_BUILTIN,
+            led_state ? HIGH : LOW
+        );
     }
 }
 
@@ -65,17 +61,10 @@ void StoppedState::command(char c)
 
         context->to_operational();
     }
-
-    else if (c == 'p' || c == 'P')
-    {
-        Serial.println("Command: set pre-operational");
-
-        context->to_pre_operational();
-    }
 }
 
 
 void StoppedState::exit()
 {
-    context->led_off();
+    digitalWrite(LED_BUILTIN, LOW);
 }

test/test_pi_controller/test_main.cpp

@@ -1,150 +1,28 @@
 #include <unity.h>
 
-// #ifdef ARDUINO
-#include <Arduino.h>
-// #endif
-
-#include "p_controller.h"
+//#include "p_controller.h"
 #include "pi_controller.h"
 
 static const double KP = 2.0;
-static const double TI = 1.0;
+static const double KI = 1.0;
 static const double T = 0.1;
-static const double TOL = 1e-4;  // double er 32-bita float a AVR
+static const double TOL = 1e-4; // Tolerance
 
-// void setUp(void) {}
-// void tearDown(void) {}
-
-// Normal flow: engin villa -> ekkert úttak
-void test_zero_error_gives_zero_output(void)
+void test_positive_error()
 {
-    PI_controller c(KP, TI, T);
-    TEST_ASSERT_FLOAT_WITHIN(TOL, 0.0, c.update(10.0, 10.0));
+    PI_controller controller(KP, KI, T);
+
+    double output = controller.update(10.0, 8.0);
+
+    TEST_ASSERT_DOUBLE_WITHIN(TOL, 4.4, output);
 }
 
-// Normal flow: fyrsta skref u = Kp * (e + e*T/Ti)
-void test_first_step(void)
-{
-    PI_controller c(KP, TI, T);
-    double e = 5.0;
-    TEST_ASSERT_FLOAT_WITHIN(TOL, KP * (e + e * T / TI), c.update(e, 0.0));
-}
 
-// Normal flow: stöðug villa safnast upp, u_n = Kp * (e + n*e*T/Ti)
-void test_integral_accumulates(void)
+int main()
 {
-    PI_controller c(KP, TI, T);
-    double e = 5.0;
-    double u = 0.0;
-    for (int n = 1; n <= 5; n++)
-    {
-        u = c.update(e, 0.0);
-        TEST_ASSERT_FLOAT_WITHIN(TOL, KP * (e + n * e * T / TI), u);
-    }
-}
-
-// Normal flow: þegar villan er orðin 0 heldur integralið úttakinu uppi
-void test_output_held_when_error_zero(void)
-{
-    PI_controller c(KP, TI, T);
-    double e = 5.0;
-    for (int n = 0; n < 5; n++)
-    {
-        c.update(e, 0.0);
-    }
-    double E = 5 * e * T;
-    TEST_ASSERT_FLOAT_WITHIN(TOL, KP * E / TI, c.update(0.0, 0.0));
-}
-
-// Alt flow: villa langt utan getu -> mettun í +100 %
-void test_saturates_high(void)
-{
-    PI_controller c(KP, TI, T);
-    TEST_ASSERT_FLOAT_WITHIN(TOL, 100.0, c.update(10000.0, 0.0));
-}
-
-// Alt flow: mettun í -100 %
-void test_saturates_low(void)
-{
-    PI_controller c(KP, TI, T);
-    TEST_ASSERT_FLOAT_WITHIN(TOL, -100.0, c.update(-10000.0, 0.0));
-}
-
-// Alt flow: anti-windup. Eftir langa mettun á lítil neikvæð villa
-// strax að gefa úttak undir mettun (integralið óx ekki)
-void test_no_integral_windup(void)
-{
-    PI_controller c(KP, TI, T);
-    for (int n = 0; n < 1000; n++)
-    {
-        c.update(10000.0, 0.0);
-    }
-    double u = c.update(0.0, 1.0);  // e = -1
-    TEST_ASSERT_TRUE(u < 0.0);
-}
-
-// Alt flow: reset (stopped -> operational) hreinsar integralið
-void test_reset_clears_integral(void)
-{
-    PI_controller c(KP, TI, T);
-    for (int n = 0; n < 5; n++)
-    {
-        c.update(5.0, 0.0);
-    }
-    c.reset();
-    TEST_ASSERT_FLOAT_WITHIN(TOL, 0.0, c.update(0.0, 0.0));
-}
-
-// Polymorphism: kall í gegnum P_controller& gefur PI hegðun
-void test_polymorphic_update(void)
-{
-    PI_controller pi(KP, TI, T);
-    P_controller& c = pi;
-    double e = 5.0;
-    TEST_ASSERT_FLOAT_WITHIN(TOL, KP * (e + e * T / TI), c.update(e, 0.0));
-}
-
-// P_controller hegðar sér óbreytt
-void test_p_controller_unchanged(void)
-{
-    P_controller c(KP);
-    TEST_ASSERT_FLOAT_WITHIN(TOL, KP * 5.0, c.update(5.0, 0.0));
-}
-
-void setup()
-{
-    // init();
-    delay(2000);
     UNITY_BEGIN();
-    RUN_TEST(test_zero_error_gives_zero_output);
-    RUN_TEST(test_first_step);
-    RUN_TEST(test_integral_accumulates);
-    RUN_TEST(test_output_held_when_error_zero);
-    RUN_TEST(test_saturates_high);
-    RUN_TEST(test_saturates_low);
-    RUN_TEST(test_no_integral_windup);
-    RUN_TEST(test_reset_clears_integral);
-    RUN_TEST(test_polymorphic_update);
-    RUN_TEST(test_p_controller_unchanged);
-    // return UNITY_END();
-    UNITY_END();
+
+    RUN_TEST(test_positive_error);
+
+    return UNITY_END();
 }
-
-void loop() {}
-
-// #ifdef ARDUINO
-// void setup()
-// {
-//     delay(2000);  // bida eftir serial tengingu eftir reset
-//     run_tests();
-// }
-
-// void loop()
-// {
-// }
-// #else
-// int main(int argc, char** argv)
-// {
-//     return run_tests();
-// }
-// #endif