Tune-aði PI stýringuna og plot-aði myndir

bætti við prentskipunum til þess að geta plottað step response
Framkvæmdi test á hegðun PI_controller
2026-10-03 15:09:19 +00:00 · 2026-10-01 12:42:35 +00:00 · 2026-10-01 10:58:06 +00:00 · 2026-10-01 09:47:04 +00:00 · 2026-09-30 13:55:23 +00:00 · 2026-09-30 13:54:05 +00:00
25 changed files with 673 additions and 678 deletions
@@ -1,6 +1,15 @@
 .pio
 venv
+__pycache__
+*.csv
+*.typ
+*.pdf
 *.py
+*.png
 *.csv
 scripts
-*.typ
+
+# Skrár sem PlatformIO býr til sjálft
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+compile_commands.json
@@ -1,56 +0,0 @@
-//
-// !!! WARNING !!! AUTO-GENERATED FILE!
-// PLEASE DO NOT MODIFY IT AND USE "platformio.ini":
-// https://docs.platformio.org/page/projectconf/section_env_build.html#build-flags
-//
-{
-    "configurations": [
-        {
-            "name": "PlatformIO",
-            "includePath": [
-                "C:/Users/isakt/Embedded/ESP_Project3_H26/include",
-                "C:/Users/isakt/Embedded/ESP_Project3_H26/src",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/cores/arduino",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/HID/src",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/Wire/src",
-                ""
-            ],
-            "browse": {
-                "limitSymbolsToIncludedHeaders": true,
-                "path": [
-                    "C:/Users/isakt/Embedded/ESP_Project3_H26/include",
-                    "C:/Users/isakt/Embedded/ESP_Project3_H26/src",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/cores/arduino",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/HID/src",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/Wire/src",
-                    ""
-                ]
-            },
-            "defines": [
-                "PLATFORMIO=60200",
-                "ARDUINO_AVR_NANO",
-                "F_CPU=16000000L",
-                "ARDUINO_ARCH_AVR",
-                "ARDUINO=10808",
-                "__AVR_ATmega328P__",
-                ""
-            ],
-            "cStandard": "gnu11",
-            "cppStandard": "gnu++11",
-            "compilerPath": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin/avr-gcc.exe",
-            "compilerArgs": [
-                "-mmcu=atmega328p",
-                ""
-            ]
-        }
-    ],
-    "version": 4
-}
@@ -1,44 +0,0 @@
-// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY
-//
-// PlatformIO Debugging Solution
-//
-// Documentation: https://docs.platformio.org/en/latest/plus/debugging.html
-// Configuration: https://docs.platformio.org/en/latest/projectconf/sections/env/options/debug/index.html
-
-{
-    "version": "0.2.0",
-    "configurations": [
-        {
-            "type": "platformio-debug",
-            "request": "launch",
-            "name": "PIO Debug",
-            "executable": "C:/Users/isakt/Embedded/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
-            "projectEnvName": "nanoatmega328new",
-            "toolchainBinDir": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin",
-            "internalConsoleOptions": "openOnSessionStart",
-            "preLaunchTask": {
-                "type": "PlatformIO",
-                "task": "Pre-Debug"
-            }
-        },
-        {
-            "type": "platformio-debug",
-            "request": "launch",
-            "name": "PIO Debug (skip Pre-Debug)",
-            "executable": "C:/Users/isakt/Embedded/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
-            "projectEnvName": "nanoatmega328new",
-            "toolchainBinDir": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin",
-            "internalConsoleOptions": "openOnSessionStart"
-        },
-        {
-            "type": "platformio-debug",
-            "request": "launch",
-            "name": "PIO Debug (without uploading)",
-            "executable": "C:/Users/isakt/Embedded/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
-            "projectEnvName": "nanoatmega328new",
-            "toolchainBinDir": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin",
-            "internalConsoleOptions": "openOnSessionStart",
-            "loadMode": "manual"
-        }
-    ]
-}
@@ -1,200 +0,0 @@
-[
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/CDC.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/CDC.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/CDC.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/CDC.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/HardwareSerial.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/HardwareSerial.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/HardwareSerial.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/HardwareSerial.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/HardwareSerial0.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/HardwareSerial0.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/HardwareSerial0.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/HardwareSerial0.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/HardwareSerial1.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/HardwareSerial1.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/HardwareSerial1.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/HardwareSerial1.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/HardwareSerial2.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/HardwareSerial2.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/HardwareSerial2.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/HardwareSerial2.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/HardwareSerial3.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/HardwareSerial3.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/HardwareSerial3.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/HardwareSerial3.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/IPAddress.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/IPAddress.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/IPAddress.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/IPAddress.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/PluggableUSB.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/PluggableUSB.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/PluggableUSB.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/PluggableUSB.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/Print.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/Print.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/Print.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/Print.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/Stream.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/Stream.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/Stream.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/Stream.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/Tone.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/Tone.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/Tone.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/Tone.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/USBCore.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/USBCore.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/USBCore.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/USBCore.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-gcc -o .pio/build/nanoatmega328new/FrameworkArduino/WInterrupts.c.o -c -std=gnu11 -fno-fat-lto-objects -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/WInterrupts.c",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/WInterrupts.c",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/WInterrupts.c.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/WMath.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/WMath.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/WMath.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/WMath.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/WString.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/WString.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/WString.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/WString.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/abi.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/abi.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/abi.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/abi.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-gcc -o .pio/build/nanoatmega328new/FrameworkArduino/hooks.c.o -c -std=gnu11 -fno-fat-lto-objects -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/hooks.c",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/hooks.c",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/hooks.c.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/main.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/main.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/main.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/main.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/FrameworkArduino/new.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/new.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/new.cpp",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/new.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-gcc -o .pio/build/nanoatmega328new/FrameworkArduino/wiring.c.o -c -std=gnu11 -fno-fat-lto-objects -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/wiring.c",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/wiring.c",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/wiring.c.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-gcc -o .pio/build/nanoatmega328new/FrameworkArduino/wiring_analog.c.o -c -std=gnu11 -fno-fat-lto-objects -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/wiring_analog.c",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/wiring_analog.c",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/wiring_analog.c.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-gcc -o .pio/build/nanoatmega328new/FrameworkArduino/wiring_digital.c.o -c -std=gnu11 -fno-fat-lto-objects -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/wiring_digital.c",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/wiring_digital.c",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/wiring_digital.c.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-gcc -mmcu=atmega328p -x assembler-with-cpp -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs -c -o .pio/build/nanoatmega328new/FrameworkArduino/wiring_pulse.S.o /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/wiring_pulse.S",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/wiring_pulse.S",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/wiring_pulse.S.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-gcc -o .pio/build/nanoatmega328new/FrameworkArduino/wiring_pulse.c.o -c -std=gnu11 -fno-fat-lto-objects -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/wiring_pulse.c",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/wiring_pulse.c",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/wiring_pulse.c.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-gcc -o .pio/build/nanoatmega328new/FrameworkArduino/wiring_shift.c.o -c -std=gnu11 -fno-fat-lto-objects -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs /Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/wiring_shift.c",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino/wiring_shift.c",
-        "output": ".pio/build/nanoatmega328new/FrameworkArduino/wiring_shift.c.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/src/digital_in.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -Iinclude -Isrc -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs src/digital_in.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "src/digital_in.cpp",
-        "output": ".pio/build/nanoatmega328new/src/digital_in.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/src/digital_out.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -Iinclude -Isrc -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs src/digital_out.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "src/digital_out.cpp",
-        "output": ".pio/build/nanoatmega328new/src/digital_out.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/src/main.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -Iinclude -Isrc -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs src/main.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "src/main.cpp",
-        "output": ".pio/build/nanoatmega328new/src/main.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/src/part1_first_working_main.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -Iinclude -Isrc -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs src/part1_first_working_main.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "src/part1_first_working_main.cpp",
-        "output": ".pio/build/nanoatmega328new/src/part1_first_working_main.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/src/part1part2.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -Iinclude -Isrc -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs src/part1part2.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "src/part1part2.cpp",
-        "output": ".pio/build/nanoatmega328new/src/part1part2.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/src/part3part1.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -Iinclude -Isrc -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs src/part3part1.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "src/part3part1.cpp",
-        "output": ".pio/build/nanoatmega328new/src/part3part1.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/src/rotary_encoder_driver.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -Iinclude -Isrc -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs src/rotary_encoder_driver.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "src/rotary_encoder_driver.cpp",
-        "output": ".pio/build/nanoatmega328new/src/rotary_encoder_driver.cpp.o"
-    },
-    {
-        "command": "/Users/dadi/.platformio/packages/toolchain-atmelavr/bin/avr-g++ -o .pio/build/nanoatmega328new/src/timer_usec.cpp.o -c -fno-exceptions -fno-threadsafe-statics -fpermissive -std=gnu++11 -mmcu=atmega328p -Os -Wall -ffunction-sections -fdata-sections -flto -DPLATFORMIO=60119 -DARDUINO_AVR_NANO -DF_CPU=16000000L -DARDUINO_ARCH_AVR -DARDUINO=10808 -Iinclude -Isrc -I/Users/dadi/.platformio/packages/framework-arduino-avr/cores/arduino -I/Users/dadi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs src/timer_usec.cpp",
-        "directory": "/Users/dadi/embedded/proj1",
-        "file": "src/timer_usec.cpp",
-        "output": ".pio/build/nanoatmega328new/src/timer_usec.cpp.o"
-    }
-]
@@ -1,5 +1,7 @@
@startuml
-
+' skinparam linetype ortho
+' skinparam nodesep 100
+skinparam ranksep 80

 skinparam state {
    BackgroundColor LightBlue
@@ -9,25 +11,21 @@ skinparam state {

 state Initialization 
    Initialization : Entry / Stop motor 
-    Initialization : Do / Initialize internal parameters
-    Initialization : Do / Reset speed measurement
+    Initialization : Entry / Initialize internal parameters
+    Initialization : Entry / Reset speed measurement
+    Initialization : Do / Wait 2 seconds
    Initialization : Exit / Send Boot-up message

-
-
 state Operational 
    Operational : Entry / LED on
    Operational : Do / Run speed controller
-    Operational : Exit / Stop motor
-    Operational : Exit / LED off
+    Operational : Exit / Stop motor and LED off

 state Stopped
    Stopped : Entry / Stop motor
    Stopped : Do / Blink LED at 2 Hz
    Stopped : Exit / LED off

-
-
 Initialization --> Operational : initialization complete
 Operational --> Initialization : reset
 Operational --> Stopped : fault detected
@@ -6,6 +6,11 @@

 class InitializationState : public State
 {
+private:
+    static const unsigned long WAIT_MS = 2000;
+
+    unsigned long enter_time_ms = 0;
+
 public:
    void enter() override;

@@ -4,23 +4,39 @@
 #include "rotary_encoder_driver.h"
 #include "motor_driver.h"
 #include "p_controller.h"
+#include "digital_out.h"

 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:
+    static constexpr double DEFAULT_REFERENCE_RPM = 25.0;
+    static constexpr double PWM_PER_PERCENT = 255.0 / 100.0;
+    static const uint8_t LED_PIN = 5;   // PB5 = D13 = LED_BUILTIN
+
    State* current_state;

-    State* initialization_state;
-    State* operational_state;
-    State* stopped_state;
+    State& initialization_state;
+    State& operational_state;
+    State& stopped_state;

    RotaryEncoderDriver& encoder;
    MotorDriver& motor;
    P_controller& controller;

+    Digital_out led;
+
    double reference_rpm;


@@ -28,14 +44,10 @@ public:
    MotorController(
        RotaryEncoderDriver& encoder,
        MotorDriver& motor,
-        P_controller& controller
-    );
-
-
-    void set_states(
-        State* initialization,
-        State* operational,
-        State* stopped
+        P_controller& controller,
+        State& initialization,
+        State& operational,
+        State& stopped
    );


@@ -45,7 +57,7 @@ public:

    void command(char c);

-    void transition_to(State* new_state);
+    void transition_to(State& new_state);


    void to_initialization();
@@ -57,11 +69,18 @@ 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();

    void reset_speed_measurement();
+
+    void reset_controller();
+
+    void led_on();
+
+    void led_off();
 };

 #endif
@@ -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;

@@ -3,13 +3,14 @@

 class P_controller
 {
-private:
+protected:
    double Kp;

 public:
    P_controller(double Kp);
-
-    double update(double ref, double actual);
+    virtual ~P_controller() {}
+    virtual double update(double ref, double actual);
+    virtual void reset() {}
 };

 #endif
@@ -0,0 +1,21 @@
+#ifndef PI_CONTROLLER_H
+#define PI_CONTROLLER_H
+
+#include "p_controller.h"
+
+class PI_controller : public P_controller
+{
+    private:
+        static constexpr double U_MAX = 100.0;  // %
+        static constexpr double U_MIN = -100.0; // %
+
+        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;
+};
+
+#endif
@@ -4,37 +4,36 @@
 #include <stdint.h>

 #include "digital_in.h"
-//#include "digital_out.h"
 #include "timer_usec.h"

 class RotaryEncoderDriver {
-    static const uint8_t HIST_LEN = 4;   // 4 x 4 ms = 16 ms gluggi fyrir rpm-utreikning
+public:
+    static constexpr uint16_t SAMPLE_PERIOD_US = 4000;   // Ts = 4 ms
+    static constexpr float COUNTS_PER_REV = 1050.0f;

-    volatile uint8_t cycle;
-    volatile int16_t count;
-    volatile int16_t prev_count;
+private:
+    static const uint8_t HIST_LEN = 4;   // 4 x Ts = 16 ms gluggi fyrir rpm-utreikning
+
+    volatile int32_t count;
+    volatile int16_t delta;
    volatile bool speed_ready;

-    volatile int16_t history[HIST_LEN];
+    volatile int32_t history[HIST_LEN];
    volatile uint8_t hist_idx;

-    //Digital_out led;
-
    Digital_in eA;
    Digital_in eB;

    Timer_usec timer;

 public:
-    volatile int16_t delta;
-
    RotaryEncoderDriver();

    void begin();

    void update();

-    int position(); 
+    int32_t position();

    void update_delta();

@@ -6,7 +6,7 @@ class MotorController;
 class State
 {
 protected:
-    MotorController* context;
+    MotorController* context = nullptr;

 public:
    virtual ~State()
@@ -14,4 +14,12 @@ board = nanoatmega328new
 framework = arduino

 monitor_speed = 115200
+test_framework = unity
+test_build_src = yes

+
+; [env:native]
+; platform = native
+; test_framework = unity
+; test_build_src = yes
+; build_src_filter = -<*> +<p_controller.cpp> +<pi_controller.cpp>
@@ -0,0 +1,176 @@
+#!/usr/bin/env python3
+"""
+Teiknar step response-plott ur CSV-gognum sem Arduino-verkefnid prentar ut
+i Operational state (dalkar: time_ms, ref_rpm, actual_rpm, pwm_actual).
+
+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] [--no-pwm]
+
+--no-pwm teiknar bara RPM (PWM-dalkurinn ma lika vanta i CSV-skranni)
+
+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"]
+REQUIRED = ["time_ms", "ref_rpm", "actual_rpm"]
+
+# Onnur nofn sem eldri utgafur firmware-sins hafa notad fyrir sama dalk
+ALIASES = {"pwm": "pwm_actual"}
+
+
+def empty_cols():
+    return {key: [] for key in COLUMNS}
+
+
+def load_csv(path):
+    cols = empty_cols()
+    header = None
+    with open(path, newline="") as f:
+        for row in csv.reader(f):
+            if row and row[0].strip() == "time_ms":
+                header = [name.strip() for name in row]
+                header = [ALIASES.get(name, name) for name in header]
+                missing = [key for key in REQUIRED if key not in header]
+                if missing:
+                    sys.exit(f"Dalka vantar i haus {row}: {missing}")
+                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 if key in header}
+            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 values:
+                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="", show_pwm=True):
+    time_s = [t / 1000.0 for t in cols["time_ms"]]
+    ref_rpm = cols["ref_rpm"]
+    actual_rpm = cols["actual_rpm"]
+    pwm_actual = cols["pwm_actual"]
+    show_pwm = show_pwm and len(pwm_actual) == len(time_s)
+
+    fig, ax1 = plt.subplots(figsize=(10, 5))
+
+    ax1.plot(time_s, ref_rpm, label="Reference (ref_rpm)", color="#666666",
+              linestyle="--", linewidth=1.5)
+    ax1.plot(time_s, actual_rpm, label="Measured (actual_rpm)", color="#1f77b4",
+              linewidth=1.8)
+    ax1.set_xlabel("Time [s]")
+    ax1.set_ylabel("Angular velocity [RPM]")
+    ax1.grid(True, alpha=0.3)
+
+    lines, labels = ax1.get_legend_handles_labels()
+    if show_pwm:
+        ax2 = ax1.twinx()
+        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)
+        ax2.axhline(-255, color="#d62728", linewidth=0.6, alpha=0.3)
+        ax2.set_ylabel("PWM [-255, 255]")
+        lines2, labels2 = ax2.get_legend_handles_labels()
+        lines += lines2
+        labels += labels2
+
+    # Merkjum stokkpunktinn (fyrsta stadin thar sem ref hoppar fra 0)
+    step_idx = next((i for i, r in enumerate(ref_rpm) if r > 0), None)
+    if step_idx is not None:
+        ax1.axvline(time_s[step_idx], color="black", linewidth=0.8,
+                     linestyle=":")
+        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.99, 0.98, f"Tu ≈ {tu * 1000:.1f} ms\n"
+    #                           f"amplitude ≈ ±{amplitude:.2f} RPM",
+    #               transform=ax1.transAxes, fontsize=9, va="top", ha="right",
+    #               bbox=dict(facecolor="white", alpha=0.8, edgecolor="#cccccc"))
+    # else:
+    #     print("Engin skyr sveifla fannst i seinni helmingi gagnanna")
+
+    ax1.legend(lines, labels, loc="lower right", fontsize=8)
+
+    if show_pwm:
+        title = "Step response: RPM and PWM over time"
+    else:
+        title = "Step response: RPM over time"
+    if title_suffix:
+        title += f" ({title_suffix})"
+    fig.suptitle(title)
+    fig.tight_layout()
+    fig.savefig(out_path, dpi=150)
+    print(f"Vistad: {out_path}")
+
+
+if __name__ == "__main__":
+    show_pwm = "--no-pwm" not in sys.argv
+    args = [a for a in sys.argv[1:] if a != "--no-pwm"]
+    in_path = args[0] if len(args) > 0 else "data/step_response.csv"
+    out_path = args[1] if len(args) > 1 else "data/step_response.png"
+    title_suffix = args[2] if len(args) > 2 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, show_pwm)
@@ -9,19 +9,23 @@ void InitializationState::enter()
    Serial.println();
    Serial.println("State: Initialization");

-    digitalWrite(LED_BUILTIN, LOW);
+    context->led_off();

    context->stop_motor();

    context->reset_speed_measurement();

-    Serial.println("Boot-up");
+    enter_time_ms = millis();
 }


 void InitializationState::update()
 {
+    // Non-blocking bið svo aðallykkjan haldi áfram að keyra
+    if (millis() - enter_time_ms >= WAIT_MS)
+    {
        context->to_operational();
+    }
 }


@@ -32,4 +36,5 @@ void InitializationState::command(char c)

 void InitializationState::exit()
 {
+    Serial.println("Boot-up");
 }
@@ -1,9 +1,12 @@
+// 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"

@@ -16,14 +19,12 @@ RotaryEncoderDriver encoder;

 MotorDriver motor;

-P_controller controller(12.5);
+const double Kp = 10.125; // Ku * 0.45
+const double Ti = 0.0334; //  Tu * 0.83 37
+const double T = 0.004; // 0.004

-
-MotorController motor_controller(
-    encoder,
-    motor,
-    controller
-);
+// P_controller controller(Kp);
+PI_controller controller(Kp, Ti, T);


 InitializationState initialization_state;
@@ -32,6 +33,17 @@ OperationalState operational_state;

 StoppedState stopped_state;

+// Stöðurnar verða að vera skilgreindar á undan motor_controller
+MotorController motor_controller(
+    encoder,
+    motor,
+    controller,
+    initialization_state,
+    operational_state,
+    stopped_state
+);
+
+
 int main()
 {
    init();
@@ -39,11 +51,6 @@ int main()
    Serial.begin(115200);


-    pinMode(LED_BUILTIN, OUTPUT);
-
-    digitalWrite(LED_BUILTIN, LOW);
-
-
    cli();

    encoder.begin();
@@ -52,12 +59,11 @@ int main()

    sei();
    
-
-    motor_controller.set_states(
-        &initialization_state,
-        &operational_state,
-        &stopped_state
-    );
+    // motor_controller.set_states(
+    //     &initialization_state,
+    //     &operational_state,
+    //     &stopped_state
+    // );


    motor_controller.begin();
@@ -82,3 +88,8 @@ int main()

    return 0;
 }
+
+// int main(){
+//     return 0;
+// }
+#endif // PIO_UNIT_TESTING
@@ -1,88 +0,0 @@
-/* #include <avr/io.h>
-#include <util/delay.h>
-#include <Arduino.h>
-
-#include "rotary_encoder_driver.h"
-#include "motor_driver.h"
-#include "p_controller.h"
-
-RotaryEncoderDriver encoder;
-MotorDriver motor;
-
-P_controller controller(12.5);
-
-const double REF_RPM1 = 30.0;
-const double REF_RPM2 = 50.0;
-
-const uint16_t STEP1_AT_SAMPLE = 50; 
-const uint16_t STEP2_AT_SAMPLE = 500;
-
-const uint16_t STOP_AT_SAMPLE = 1000;  
-
-int main() {
-    init();
-    Serial.begin(115200);
-
-    cli();
-    encoder.begin();
-    motor.begin();
-    sei();
-
-    // Mótor stopp og smá bið svo kerfið sé í kyrrstöðu áður en mælingin hefst
-    motor.stop();
-    _delay_ms(2000);
-
-    Serial.println("time_ms,ref_rpm,actual_rpm,pwm_requested,pwm_actual");
-    Serial.println("0.0,0.0,0.0,0.0,0.0");
-
-    encoder.reset_speed_measurement();
-
-    uint16_t sample = 0;
-    double ref = 0.0;
-
-    while (true) {
-        if (encoder.new_speed_available()) {
-            sample++;
-
-            float time_ms = sample * 4.0f;
-
-            // Step-inntak: viðmiðið hoppar úr 0 í REF_RPM á STEP_AT_SAMPLE
-            if (sample == STEP1_AT_SAMPLE) {
-                ref = REF_RPM1;
-            }
-
-            if (sample == STEP2_AT_SAMPLE) {
-                ref = REF_RPM2;
-            }
-
-            double actual = encoder.rpm();
-
-            double pwm_percent   = controller.update(ref, actual);  
-            double pwm_requested = pwm_percent * 2.55;               
-
-            double pwm_actual = pwm_requested;                       
-            if (pwm_actual > 255.0)  pwm_actual = 255.0;
-            if (pwm_actual < -255.0) pwm_actual = -255.0;
-
-            motor.set(pwm_actual);
-
-            Serial.print(time_ms, 1);
-            Serial.print(",");
-            Serial.print(ref, 1);
-            Serial.print(",");
-            Serial.print(actual, 2);
-            Serial.print(",");
-            Serial.print(pwm_requested, 1);
-            Serial.print(",");
-            Serial.println(pwm_actual, 1);
-
-            if (sample >= STOP_AT_SAMPLE) {
-                motor.stop();
-                while (true) {
-                }
-            }
-        }
-    }
-    return 0;
-}
- */
@@ -7,48 +7,56 @@
 MotorController::MotorController(
    RotaryEncoderDriver& encoder,
    MotorDriver& motor,
-    P_controller& controller
+    P_controller& controller,
+    State& initialization,
+    State& operational,
+    State& stopped
 )
    : current_state(nullptr),
-      initialization_state(nullptr),
-      operational_state(nullptr),
-      stopped_state(nullptr),
+      initialization_state(initialization),
+      operational_state(operational),
+      stopped_state(stopped),
      encoder(encoder),
      motor(motor),
      controller(controller),
-      reference_rpm(30.0)
+      led(LED_PIN),
+      reference_rpm(DEFAULT_REFERENCE_RPM)
 {
+    initialization_state.set_context(this);
+    operational_state.set_context(this);
+    stopped_state.set_context(this);
 }


-void MotorController::set_states(
-    State* initialization,
-    State* operational,
-    State* stopped
-)
-{
-    initialization_state = initialization;
-    operational_state = operational;
-    stopped_state = stopped;
-}
+// void MotorController::set_states(
+//     State* initialization,
+//     State* operational,
+//     State* stopped
+// )
+
+    // initialization_state = initialization;
+    // operational_state = operational;
+    // stopped_state = stopped;
+


 void MotorController::begin()
 {
+    led.init();
+    led.set_lo();
+
    transition_to(initialization_state);
 }


-void MotorController::transition_to(State* new_state)
+void MotorController::transition_to(State& new_state)
 {
    if (current_state != nullptr)
    {
        current_state->exit();
    }

-    current_state = new_state;
-
-    current_state->set_context(this);
+    current_state = &new_state;

    current_state->enter();
 }
@@ -84,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())
    {
+        return false;
+    }
+
    double actual = encoder.rpm();

    double pwm_percent =
        controller.update(reference_rpm, actual);

    double pwm =
-            pwm_percent * 2.55;
+        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;
 }


@@ -112,6 +139,11 @@ void MotorController::reset_speed_measurement()
    encoder.reset_speed_measurement();
 }

+void MotorController::reset_controller()
+{
+    controller.reset();
+}
+
 void MotorController::to_stopped()
 {
    transition_to(stopped_state);
@@ -121,3 +153,14 @@ bool MotorController::fault_active()
 {
    return motor.fault_active();
 }
+
+void MotorController::led_on()
+{
+   led.set_hi();
+}
+
+
+void MotorController::led_off()
+{
+   led.set_lo();
+}
@@ -3,6 +3,37 @@
 #include <avr/io.h>


+// Í Fast PWM gefur OCR = 0 samt einn stuttan púls í hverri lotu.
+// Til að pinni sé alveg LOW er hann aftengdur frá timernum (COM-bitar)
+// og keyrður LOW gegnum PORT-registerið.
+
+static void ain1_off()   // D3 = PD3 = OC2B
+{
+    TCCR2A &= ~(1 << COM2B1);
+    PORTD  &= ~(1 << PORTD3);
+    OCR2B = 0;
+}
+
+static void ain2_off()   // D11 = PB3 = OC2A
+{
+    TCCR2A &= ~(1 << COM2A1);
+    PORTB  &= ~(1 << PORTB3);
+    OCR2A = 0;
+}
+
+static void ain1_pwm(uint8_t duty)
+{
+    OCR2B = duty;
+    TCCR2A |= (1 << COM2B1);
+}
+
+static void ain2_pwm(uint8_t duty)
+{
+    OCR2A = duty;
+    TCCR2A |= (1 << COM2A1);
+}
+
+
 MotorDriver::MotorDriver()
 {
 }
@@ -30,57 +61,32 @@ void MotorDriver::begin()
    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();
+    // PWM-úttök eru tengd í set() eftir þörfum
+    stop();
 }


 void MotorDriver::forward()
 {
-    // AIN1 / D3  = HIGH
-    // AIN2 / D11 = LOW
-
-    //PORTD |= (1 << PORTD3);
-    //PORTB &= ~(1 << PORTB3);
-
-    OCR2A = 0;
-    OCR2B = 255;
+    set(255.0);
 }


 void MotorDriver::reverse()
 {
-    // AIN1 / D3  = LOW
-    // AIN2 / D11 = HIGH
-
-    //PORTD &= ~(1 << PORTD3);
-    //PORTB |= (1 << PORTB3);
-
-    OCR2B = 0;
-    OCR2A = 255;
+    set(-255.0);
 }


 void MotorDriver::stop()
 {
    // Both LOW
-
-    //PORTD &= ~(1 << PORTD3);
-    //PORTB &= ~(1 << PORTB3);
-
-    OCR2A = 0;
-    OCR2B = 0;
+    ain1_off();
+    ain2_off();
 }

 void MotorDriver::set(double pwm)
@@ -97,21 +103,23 @@ void MotorDriver::set(double pwm)
    }


-    if (pwm > 0)
+    uint8_t duty = (uint8_t)(pwm > 0 ? pwm : -pwm);
+
+    if (duty == 0)
+    {
+        stop();
+    }
+    else if (pwm > 0)
    {
        // Forward
-        OCR2A = 0;
-        OCR2B = (uint8_t)pwm;
-    }
-    else if (pwm < 0)
-    {
-        // Reverse
-        OCR2B = 0;
-        OCR2A = (uint8_t)(-pwm);
+        ain2_off();
+        ain1_pwm(duty);
    }
    else
    {
-        stop();
+        // Reverse
+        ain1_off();
+        ain2_pwm(duty);
    }
 }

@@ -8,7 +8,14 @@ void OperationalState::enter()
 {
    Serial.println("State: Operational");

-    digitalWrite(LED_BUILTIN, HIGH);
+    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");
 }

 // Part 1 
@@ -31,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.println(sample.actual_rpm, 2);
+        // Serial.print(",");
+        // Serial.println(sample.pwm_actual, 1);
+    }
 }


@@ -50,5 +70,5 @@ void OperationalState::exit()
 {
    context->stop_motor();

-    digitalWrite(LED_BUILTIN, LOW);
+    context->led_off();
 }
@@ -0,0 +1,32 @@
+#include "pi_controller.h"
+
+PI_controller::PI_controller(double Kp, double Ti, double T) : P_controller(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;
+}
+
+void PI_controller::reset()
+{
+    integral = 0.0;
+}
@@ -1,17 +1,15 @@
 #include "rotary_encoder_driver.h"
 #include <avr/interrupt.h>

-RotaryEncoderDriver* driver = nullptr;
+static RotaryEncoderDriver* driver = nullptr;

 RotaryEncoderDriver::RotaryEncoderDriver()
-    : cycle(0)
-    , count(0)
-    , prev_count(0)
+    : count(0)
+    , delta(0)
    , speed_ready(false)
    , hist_idx(0)
    , eA(2)
    , eB(5)
-    , delta(0)
 {
    for (uint8_t i = 0; i < HIST_LEN; i++)
    {
@@ -24,54 +22,46 @@ void RotaryEncoderDriver::begin()
 {
    driver = this;

-    //led.init();
    eA.init();
    eB.init();

    EICRA |= (1<<ISC01) | (1<<ISC00);
    EIMSK |= (1<<INT0);

-    timer.init(4000);
+    timer.init(SAMPLE_PERIOD_US);
 }

 void RotaryEncoderDriver::update()
 {
    count += eB.is_hi() ? 1 : -1;
-    //led.set_hi();
 }

-int RotaryEncoderDriver::position() 
+int32_t RotaryEncoderDriver::position()
 {

    uint8_t oldSREG = SREG;

    cli();

-    int16_t local_count = count;
+    int32_t local_count = count;

    SREG = oldSREG;

-    //led.set_lo();
-
    return local_count;
 }

 void RotaryEncoderDriver::update_delta()
 {

-    int16_t oldest = history[hist_idx];
+    int32_t oldest = history[hist_idx];

    history[hist_idx] = count;

    hist_idx = (hist_idx + 1) % HIST_LEN;

-    delta = count - oldest;   // breyting yfir HIST_LEN * Ts (16 ms) glugga
-
-    prev_count = count;
+    delta = (int16_t)(count - oldest);   // breyting yfir HIST_LEN * Ts (16 ms) glugga

    speed_ready = true;
-
-    //led.set_lo();
 }

 float RotaryEncoderDriver::count_per_sec()
@@ -85,14 +75,12 @@ float RotaryEncoderDriver::count_per_sec()

    SREG = oldSREG;

-    return local_delta / (HIST_LEN * 0.004f);
+    return local_delta / (HIST_LEN * SAMPLE_PERIOD_US * 1e-6f);
 }


 float RotaryEncoderDriver::rpm()
 {
-    const float COUNTS_PER_REV = 1050.0f;
-
    return count_per_sec() * 60.0f / COUNTS_PER_REV;
 }

@@ -121,8 +109,6 @@ void RotaryEncoderDriver::reset_speed_measurement()

    cli();

-    cycle = 0;
-    prev_count = count;
    delta = 0;
    speed_ready = false;

@@ -144,5 +130,3 @@ ISR(TIMER1_COMPA_vect)
 {
    driver->update_delta();
 }
-
-
@@ -1,61 +0,0 @@
-/* #include "rotary_encoder_driver.h"
-#include <avr/interrupt.h>
-
-RotaryEncoderDriver* driver = nullptr;
-
-RotaryEncoderDriver::RotaryEncoderDriver()
-    : cycle(0)
-    , count(0)
-    , prev_count(0)
-    , led(5)
-    , eA(2)
-    , eB(5) {}
-
-void RotaryEncoderDriver::begin() {
-    driver = this;
-
-    led.init();
-    eA.init();
-    eB.init();
-
-    EICRA |= (1<<ISC01) | (1<<ISC00);
-    EIMSK |= (1<<INT0);
-
-    timer.init(100);
-}
-
-void RotaryEncoderDriver::update() {
-    count += eB.is_hi() ? 1 : -1;
-    led.set_hi();
-}
-
-int RotaryEncoderDriver::position() {
-    led.set_lo();
-    return count;
-} 
-
-void RotaryEncoderDriver::update_delta() {
-    if (++cycle == 0) {
-        delta = count - prev_count;
-        prev_count = count;
-    }
-    led.set_lo();
-}
-
-float RotaryEncoderDriver::count_per_sec() {
-    return delta / (256.0f * 0.0001f);
-} 
-
-float RotaryEncoderDriver::rpm() {
-    return (count_per_sec() * 60) / 1050.0;
-}
-
-
-ISR(INT0_vect) {
-    driver->update();
-}
-ISR(TIMER1_COMPA_vect) {
-    driver->update_delta();    
-}
-
- */
@@ -1,53 +0,0 @@
-/* Óvirkt núna, "alvöru" main() er í main.cpp. Þessi skrá var notuð í undirbúning/prófun
-
-#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();
-
-    while (true)
-    {
-        motor.set(60);
-        _delay_ms(3000);
-
-        motor.set(120);
-        _delay_ms(3000);
-
-        motor.set(240);
-        _delay_ms(3000);
-    }
-}
-*/
@@ -0,0 +1,150 @@
+#include <unity.h>
+
+// #ifdef ARDUINO
+#include <Arduino.h>
+// #endif
+
+#include "p_controller.h"
+#include "pi_controller.h"
+
+static const double KP = 2.0;
+static const double TI = 1.0;
+static const double T = 0.1;
+static const double TOL = 1e-4;  // double er 32-bita float a AVR
+
+// void setUp(void) {}
+// void tearDown(void) {}
+
+// Normal flow: engin villa -> ekkert úttak
+void test_zero_error_gives_zero_output(void)
+{
+    PI_controller c(KP, TI, T);
+    TEST_ASSERT_FLOAT_WITHIN(TOL, 0.0, c.update(10.0, 10.0));
+}
+
+// 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)
+{
+    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();
+}
+
+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
Author	SHA1	Message	Date
helgie22	a82084a8f6	Tune-aði PI stýringuna og plot-aði myndir	2026-10-03 15:09:19 +00:00
helgie22	e39d9e487e	bætti við prentskipunum til þess að geta plottað step response	2026-10-01 12:42:35 +00:00
helgie22	bf389cd012	Framkvæmdi test á hegðun PI_controller	2026-10-01 10:58:06 +00:00
helgie22	11df24f592	Bætti við test-um, pi controller með polymorphism og bætti við virkni í operational mode	2026-10-01 09:47:04 +00:00
helgie22	6c38a1db37	Lagað: git merge vesen	2026-09-30 13:55:23 +00:00
helgie22	9607a26f8f	Er í miklu veseni með git en held ég sé búinn að redda þessu, nýjasta útgáfa af part2	2026-09-30 13:54:05 +00:00
helgie22	1d93605cc3	Merging main with Isak_part2 branch	2026-09-30 13:38:27 +00:00
helgie22	be4cf10d7e	UML diagram changed	2026-09-30 13:29:43 +00:00
isakg22	3c5cedc413	Lagað sameinaða kóða eftir merge. Muna fokkin kommur!!	2026-09-30 13:18:03 +00:00
isakg22	fa30d35401	Merge branch 'main' of ssh://gitea.cs.ru.is:2222/helgie22/ESP_Project3_H26 into Isak_part2	2026-09-30 12:21:01 +00:00
helgie22	1e02205344	Encoder fyrir position í int32, stöðuvél án null-pointera, nefndir fastar, LED gegnum context	2026-09-30 11:55:41 +00:00
helgie22	c46539a57b	Initialization bíður 2 s, mótor stöðvast alveg, hreinsun - InitializationState: 2 s bið með millis() án þess að blokka, Boot-up sent í exit() - MotorDriver: PWM-pinnar aftengdir frá Timer2 þegar þeir eiga að vera LOW (OCR=0 í Fast PWM gaf samt stuttan púls) - Eytt útkommentuðum skrám: main_P2, temporary_main, rotary_encoder_driver_unused - .gitignore: plot-skriftan fer í git, vélarsértækar PlatformIO-skrár út	2026-09-30 11:49:51 +00:00