Stop bætt inn í UML diagram

.vscode/c_cpp_properties.json

@@ -8,29 +8,29 @@
         {
             "name": "PlatformIO",
             "includePath": [
-                "C:/Users/helgi/H26/ESP/Projects/ESP_Project3_H26/include",
-                "C:/Users/helgi/H26/ESP/Projects/ESP_Project3_H26/src",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/cores/arduino",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/HID/src",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/Wire/src",
+                "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/helgi/H26/ESP/Projects/ESP_Project3_H26/include",
-                    "C:/Users/helgi/H26/ESP/Projects/ESP_Project3_H26/src",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/cores/arduino",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/HID/src",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/Wire/src",
+                    "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",
                     ""
                 ]
             },
@@ -45,7 +45,7 @@
             ],
             "cStandard": "gnu11",
             "cppStandard": "gnu++11",
-            "compilerPath": "C:/Users/helgi/.platformio/packages/toolchain-atmelavr/bin/avr-gcc.exe",
+            "compilerPath": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin/avr-gcc.exe",
             "compilerArgs": [
                 "-mmcu=atmega328p",
                 ""

.vscode/launch.json

@@ -12,9 +12,9 @@
             "type": "platformio-debug",
             "request": "launch",
             "name": "PIO Debug",
-            "executable": "C:/Users/helgi/H26/ESP/Projects/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
+            "executable": "C:/Users/isakt/Embedded/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
             "projectEnvName": "nanoatmega328new",
-            "toolchainBinDir": "C:/Users/helgi/.platformio/packages/toolchain-atmelavr/bin",
+            "toolchainBinDir": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin",
             "internalConsoleOptions": "openOnSessionStart",
             "preLaunchTask": {
                 "type": "PlatformIO",
@@ -25,18 +25,18 @@
             "type": "platformio-debug",
             "request": "launch",
             "name": "PIO Debug (skip Pre-Debug)",
-            "executable": "C:/Users/helgi/H26/ESP/Projects/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
+            "executable": "C:/Users/isakt/Embedded/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
             "projectEnvName": "nanoatmega328new",
-            "toolchainBinDir": "C:/Users/helgi/.platformio/packages/toolchain-atmelavr/bin",
+            "toolchainBinDir": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin",
             "internalConsoleOptions": "openOnSessionStart"
         },
         {
             "type": "platformio-debug",
             "request": "launch",
             "name": "PIO Debug (without uploading)",
-            "executable": "C:/Users/helgi/H26/ESP/Projects/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
+            "executable": "C:/Users/isakt/Embedded/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
             "projectEnvName": "nanoatmega328new",
-            "toolchainBinDir": "C:/Users/helgi/.platformio/packages/toolchain-atmelavr/bin",
+            "toolchainBinDir": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin",
             "internalConsoleOptions": "openOnSessionStart",
             "loadMode": "manual"
         }

docs/diagrams/src/motor_state.wsd

@@ -0,0 +1,38 @@
+@startuml
+
+
+skinparam state {
+    BackgroundColor LightBlue
+}
+
+[*] --> Initialization
+
+state Initialization 
+    Initialization : Entry / Stop motor
+    Initialization : Do / Initialize internal parameters
+    Initialization : Do / Reset speed measurement
+    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
+
+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
+
+Stopped --> Initialization : reset
+Stopped --> Operational : set operational
+
+@enduml

include/initialization_state.h

@@ -0,0 +1,19 @@
+#ifndef INITIALIZATION_STATE_H
+#define INITIALIZATION_STATE_H
+
+#include "state.h"
+
+
+class InitializationState : public State
+{
+public:
+    void enter() override;
+
+    void update() override;
+
+    void command(char c) override;
+
+    void exit() override;
+};
+
+#endif

include/motor_controller.h

@@ -0,0 +1,67 @@
+#ifndef MOTOR_CONTROLLER_H
+#define MOTOR_CONTROLLER_H
+
+#include "rotary_encoder_driver.h"
+#include "motor_driver.h"
+#include "p_controller.h"
+
+class State;
+
+
+class MotorController
+{
+private:
+    State* current_state;
+
+    State* initialization_state;
+    State* operational_state;
+    State* stopped_state;
+
+    RotaryEncoderDriver& encoder;
+    MotorDriver& motor;
+    P_controller& controller;
+
+    double reference_rpm;
+
+
+public:
+    MotorController(
+        RotaryEncoderDriver& encoder,
+        MotorDriver& motor,
+        P_controller& controller
+    );
+
+
+    void set_states(
+        State* initialization,
+        State* operational,
+        State* stopped
+    );
+
+
+    void begin();
+
+    void update();
+
+    void command(char c);
+
+    void transition_to(State* new_state);
+
+
+    void to_initialization();
+
+    void to_operational();
+
+    void to_stopped();
+
+    bool fault_active();
+
+
+    void run_speed_controller();
+
+    void stop_motor();
+
+    void reset_speed_measurement();
+};
+
+#endif

include/motor_driver.h

@@ -9,10 +9,14 @@ public:
     void begin();
 
     void forward();
+
     void reverse();
+
     void stop();
 
     void set(double pwm);
+
+    bool fault_active();   // Fault detection for part 2
 };
 
 #endif

include/operational_state.h

@@ -0,0 +1,19 @@
+#ifndef OPERATIONAL_STATE_H
+#define OPERATIONAL_STATE_H
+
+#include "state.h"
+
+
+class OperationalState : public State
+{
+public:
+    void enter() override;
+
+    void update() override;
+
+    void command(char c) override;
+
+    void exit() override;
+};
+
+#endif

include/rotary_encoder_driver.h

@@ -4,11 +4,11 @@
 #include <stdint.h>
 
 #include "digital_in.h"
-#include "digital_out.h"
+//#include "digital_out.h"
 #include "timer_usec.h"
 
 class RotaryEncoderDriver {
-    static const uint8_t HIST_LEN = 4;   // 8 x 5 ms = 40 ms gluggi fyrir rpm-utreikning
+    static const uint8_t HIST_LEN = 4;   // 4 x 4 ms = 16 ms gluggi fyrir rpm-utreikning
 
     volatile uint8_t cycle;
     volatile int16_t count;
@@ -18,7 +18,7 @@ class RotaryEncoderDriver {
     volatile int16_t history[HIST_LEN];
     volatile uint8_t hist_idx;
 
-    Digital_out led;
+    //Digital_out led;
 
     Digital_in eA;
     Digital_in eB;
@@ -29,14 +29,21 @@ public:
     volatile int16_t delta;
 
     RotaryEncoderDriver();
+
     void begin();
+
     void update();
+
     int position(); 
+
     void update_delta();
+
     float count_per_sec();
+
     float rpm();
 
     bool new_speed_available();
+
     void reset_speed_measurement();
 
 };

include/state.h

@@ -0,0 +1,32 @@
+#ifndef STATE_H
+#define STATE_H
+
+class MotorController;
+
+class State
+{
+protected:
+    MotorController* context;
+
+public:
+    virtual ~State()
+    {
+        
+    }
+
+    void set_context(MotorController* context)
+    {
+        this->context = context;
+    }
+
+    virtual void enter() = 0;   // LED ON
+
+    virtual void update() = 0;  // run motor controller
+
+    virtual void command(char c) = 0;   // Listen for reset
+
+    virtual void exit() = 0;  // Stop motor
+
+};
+
+#endif

include/stopped_state.h

@@ -0,0 +1,25 @@
+#ifndef STOPPED_STATE_H
+#define STOPPED_STATE_H
+
+#include "state.h"
+
+class StoppedState : public State
+{
+private:
+    unsigned long previous_time; // Used for blinking LED
+    bool led_state;              // Ditto
+
+public:
+    StoppedState();
+
+    void enter() override;
+
+    void update() override;
+
+    void command(char c) override;
+
+    void exit() override;
+
+};
+
+#endif

platformio.ini

@@ -13,4 +13,5 @@ platform = atmelavr
 board = nanoatmega328new
 framework = arduino
 
-monitor_speed = 115200
+monitor_speed = 115200
+

src/initialization_state.cpp

@@ -0,0 +1,35 @@
+#include "initialization_state.h"
+#include "motor_controller.h"
+
+#include <Arduino.h>
+
+
+void InitializationState::enter()
+{
+    Serial.println();
+    Serial.println("State: Initialization");
+
+    digitalWrite(LED_BUILTIN, LOW);
+
+    context->stop_motor();
+
+    context->reset_speed_measurement();
+
+    Serial.println("Boot-up");
+}
+
+
+void InitializationState::update()
+{
+    context->to_operational();
+}
+
+
+void InitializationState::command(char c)
+{
+}
+
+
+void InitializationState::exit()
+{
+}

src/main.cpp

@@ -1,5 +1,3 @@
-#include <avr/io.h>
-#include <util/delay.h>
 #include <Arduino.h>
 #include <avr/interrupt.h>
 
@@ -7,160 +5,80 @@
 #include "motor_driver.h"
 #include "p_controller.h"
 
-// RotaryEncoderDriver encoder;
-// MotorDriver motor;
+#include "motor_controller.h"
 
-// P_controller controller(12.5);
+#include "initialization_state.h"
+#include "operational_state.h"
+#include "stopped_state.h"
 
-// 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;
+RotaryEncoderDriver encoder;
 
-// const uint16_t STOP_AT_SAMPLE = 1000;  
+MotorDriver motor;
 
-unsigned long state_entry_time;
+P_controller controller(12.5);
 
-unsigned long time_in_state()
+
+MotorController motor_controller(
+    encoder,
+    motor,
+    controller
+);
+
+
+InitializationState initialization_state;
+
+OperationalState operational_state;
+
+StoppedState stopped_state;
+
+int main()
 {
-  return millis() - state_entry_time;
-}
-
-int main() {
     init();
+
     Serial.begin(115200);
-    // Serial.begin(9600);
+
+
+    pinMode(LED_BUILTIN, OUTPUT);
+
+    digitalWrite(LED_BUILTIN, LOW);
+
+
+    cli();
+
+    encoder.begin();
+
+    motor.begin();
+
     sei();
 
-    enum State
-        {
-        Initialization,
-        Operational,
-        Pre_Operational,
-        Stopped
-        };
 
-    State state = State::Initialization; // Define state variable and set the initial state
+    motor_controller.set_states(
+        &initialization_state,
+        &operational_state,
+        &stopped_state
+    );
 
-    // 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);
+    motor_controller.begin();
 
-    // 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 (1) {
-        int c = 0; // for incoming serial data
 
+    while (true)
+    {
         if (Serial.available() > 0)
         {
-        c = Serial.read();
-        Serial.print("I received: ");
-        Serial.println(c, DEC);
+            char c = Serial.read();
+
+            if (c != '\n' && c != '\r')
+            {
+                motor_controller.command(c);
+            }
         }
 
-        switch (state)
-        {
-        case State::Initialization:
-            if (c == 'p')
-            {
-            state_entry_time = millis();
-            // State::Idle exit action
-            // event1 transition action
-            Serial.println("Initialization finished");
-            // set new target state
-            state = State::Pre_Operational;
-            // target state entry action
-            Serial.println("Going into Pre Operation");
-            }
-            if (c == 'o')
-            {
-            state_entry_time = millis();
-            // State::Idle exit action
-            // event2 transition action
-            Serial.println("Initialization finished");
-            // set new target state
-            state = State::Operational;
-            // target state entry action
-            Serial.println("Going straight to Operation");
-            }
-            break;
 
-        case State::Operational:
-            if (c == 'i')
-            {
-            state_entry_time = millis();
-            // State::Cooling exit action
-            Serial.println("Operation over");
-            // event3 transition action
-            // set new target state
-            state = State::Initialization;
-            // target state entry action
-            Serial.println("Initialization state");
-            }
-            if (c == 's')
-            {
-            state_entry_time = millis();
-            // State::Cooling exit action
-            Serial.println("Operation over");
-            // event4 transition action
-            // set new target state
-            state = State::Stopped;
-            // target state entry action
-            Serial.println("Stopping");
-            }
-            break;
+        motor_controller.update();
+    }
 
-        case State::Pre_Operational:
-            if (c == 'o')
-            {
-            state_entry_time = millis();
-            // State::Heating exit action
-            Serial.println("Pre Operation over");
-            // event3 transition action
-            // set new target state
-            state = State::Operational;
-            // target state entry action
-            Serial.println("Going to Operation");
-            }
-            if (c == 's')
-            {
-            state_entry_time = millis();
-            // State::Heating exit action
-            Serial.println("Pre Operation over");
-            // event4 transition action
-            // set new target state
-            state = State::Stopped;
-            // target state entry action
-            Serial.println("Stopping");
-            }
-            break;
 
-            case State::Stopped:
-            if (c == 'i')
-            {
-            state_entry_time = millis();
-            // State::Failure exit action
-            Serial.println("Stop over");
-            // event3 transition action
-            // set new target state
-            state = State::Initialization;
-            // target state entry action
-            Serial.println("Initialization state");
-            }
-
-            }
-        }
     return 0;
-}
+}

src/main_P2.cpp

@@ -0,0 +1,88 @@
+/* #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;
+}
+ */

src/motor_controller.cpp

@@ -0,0 +1,123 @@
+#include "motor_controller.h"
+#include "state.h"
+
+#include <Arduino.h>
+
+
+MotorController::MotorController(
+    RotaryEncoderDriver& encoder,
+    MotorDriver& motor,
+    P_controller& controller
+)
+    : current_state(nullptr),
+      initialization_state(nullptr),
+      operational_state(nullptr),
+      stopped_state(nullptr),
+      encoder(encoder),
+      motor(motor),
+      controller(controller),
+      reference_rpm(30.0)
+{
+}
+
+
+void MotorController::set_states(
+    State* initialization,
+    State* operational,
+    State* stopped
+)
+{
+    initialization_state = initialization;
+    operational_state = operational;
+    stopped_state = stopped;
+}
+
+
+void MotorController::begin()
+{
+    transition_to(initialization_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->enter();
+}
+
+
+void MotorController::update()
+{
+    if (current_state != nullptr)
+    {
+        current_state->update();
+    }
+}
+
+
+void MotorController::command(char c)
+{
+    if (current_state != nullptr)
+    {
+        current_state->command(c);
+    }
+}
+
+
+void MotorController::to_initialization()
+{
+    transition_to(initialization_state);
+}
+
+
+void MotorController::to_operational()
+{
+    transition_to(operational_state);
+}
+
+
+void MotorController::run_speed_controller()
+{
+    if (encoder.new_speed_available())
+    {
+        double actual = encoder.rpm();
+
+        double pwm_percent =
+            controller.update(reference_rpm, actual);
+
+        double pwm =
+            pwm_percent * 2.55;
+
+        motor.set(pwm);
+    }
+}
+
+
+void MotorController::stop_motor()
+{
+    motor.stop();
+}
+
+
+void MotorController::reset_speed_measurement()
+{
+    encoder.reset_speed_measurement();
+}
+
+void MotorController::to_stopped()
+{
+    transition_to(stopped_state);
+}
+
+bool MotorController::fault_active()
+{
+    return motor.fault_active();
+}

src/motor_driver.cpp

@@ -12,10 +12,16 @@ void MotorDriver::begin()
 {
     // Arduino D3  = PD3 = AIN1
     // Arduino D11 = PB3 = AIN2
+    // Arduino D4  = PD4 = FLT 
 
     DDRD |= (1 << DDD3);
     DDRB |= (1 << DDB3);
 
+    DDRD &= ~(1 << DDD4); 
+
+    PORTD |= (1 << PORTD4); // Enable pull-up resistor on D4 for part 2
+
+
     // Timer2 Fast PWM
     TCCR2A = 0;
     TCCR2B = 0;
@@ -107,4 +113,10 @@ void MotorDriver::set(double pwm)
     {
         stop();
     }
-}
+}
+
+
+bool MotorDriver::fault_active()
+{
+    return !(PIND & (1 << PIND4));
+}

src/operational_state.cpp

@@ -0,0 +1,54 @@
+#include "operational_state.h"
+#include "motor_controller.h"
+
+#include <Arduino.h>
+
+
+void OperationalState::enter()
+{
+    Serial.println("State: Operational");
+
+    digitalWrite(LED_BUILTIN, HIGH);
+}
+
+// Part 1 
+
+//void OperationalState::update()
+//{
+//    context->run_speed_controller();
+//}
+
+// Part 2
+
+void OperationalState::update()
+{
+    if (context->fault_active())
+    {
+        Serial.println("Fault detected");
+
+        context->to_stopped();
+
+        return;
+    }
+
+    context->run_speed_controller();
+}
+
+
+void OperationalState::command(char c)
+{
+    if (c == 'r' || c == 'R')
+    {
+        Serial.println("Command: reset");
+
+        context->to_initialization();
+    }
+}
+
+
+void OperationalState::exit()
+{
+    context->stop_motor();
+
+    digitalWrite(LED_BUILTIN, LOW);
+}

src/rotary_encoder_driver.cpp

@@ -9,20 +9,22 @@ RotaryEncoderDriver::RotaryEncoderDriver()
     , prev_count(0)
     , speed_ready(false)
     , hist_idx(0)
-    , led(5)
     , eA(2)
     , eB(5)
+    , delta(0)
 {
-    for (uint8_t i = 0; i < HIST_LEN; i++) {
+    for (uint8_t i = 0; i < HIST_LEN; i++) 
+    {
         history[i] = 0;
     }
 }
 
 
-void RotaryEncoderDriver::begin() {
+void RotaryEncoderDriver::begin() 
+{
     driver = this;
 
-    led.init();
+    //led.init();
     eA.init();
     eB.init();
 
@@ -32,12 +34,14 @@ void RotaryEncoderDriver::begin() {
     timer.init(4000);
 }
 
-void RotaryEncoderDriver::update() {
+void RotaryEncoderDriver::update() 
+{
     count += eB.is_hi() ? 1 : -1;
-    led.set_hi();
+    //led.set_hi();
 }
 
-int RotaryEncoderDriver::position() {
+int RotaryEncoderDriver::position() 
+{
 
     uint8_t oldSREG = SREG;
 
@@ -47,27 +51,31 @@ int RotaryEncoderDriver::position() {
 
     SREG = oldSREG;
 
-    led.set_lo();
+    //led.set_lo();
 
     return local_count;
 } 
 
-void RotaryEncoderDriver::update_delta() {
+void RotaryEncoderDriver::update_delta() 
+{
 
     int16_t oldest = history[hist_idx];
+
     history[hist_idx] = count;
+
     hist_idx = (hist_idx + 1) % HIST_LEN;
 
-    delta = count - oldest;   // breyting yfir HIST_LEN * Ts (40 ms) glugga
+    delta = count - oldest;   // breyting yfir HIST_LEN * Ts (16 ms) glugga
 
     prev_count = count;
 
     speed_ready = true;
 
-    led.set_lo();
+    //led.set_lo();
 }
 
-float RotaryEncoderDriver::count_per_sec() {
+float RotaryEncoderDriver::count_per_sec() 
+{
 
     uint8_t oldSREG = SREG;
 
@@ -118,18 +126,22 @@ void RotaryEncoderDriver::reset_speed_measurement()
     delta = 0;
     speed_ready = false;
 
-    for (uint8_t i = 0; i < HIST_LEN; i++) {
+    for (uint8_t i = 0; i < HIST_LEN; i++) 
+    {
         history[i] = count;
     }
+
     hist_idx = 0;
 
     SREG = oldSREG;
 }
 
-ISR(INT0_vect) {
+ISR(INT0_vect) 
+{
     driver->update();
 }
-ISR(TIMER1_COMPA_vect) {
+ISR(TIMER1_COMPA_vect) 
+{
     driver->update_delta();    
 }
 

src/stopped_state.cpp

@@ -0,0 +1,70 @@
+#include "stopped_state.h"
+#include "motor_controller.h"
+
+#include <Arduino.h>
+
+
+StoppedState::StoppedState()
+    : previous_time(0),
+      led_state(false)
+{
+}
+
+
+void StoppedState::enter()
+{
+    Serial.println("State: Stopped");
+
+    context->stop_motor();
+
+    led_state = false;
+
+    digitalWrite(LED_BUILTIN, LOW);
+
+    previous_time = millis();
+}
+
+// Controlling the blinking of the LED @ 2Hz
+
+void StoppedState::update()
+{
+    unsigned long current_time = millis();
+
+    if (current_time - previous_time >= 250)
+    {
+        previous_time = current_time;
+
+        led_state = !led_state;
+
+        digitalWrite(
+            LED_BUILTIN,
+            led_state ? HIGH : LOW
+        );
+    }
+}
+
+
+// Commands for Stopped [ r = reset and o for operational]
+
+void StoppedState::command(char c)
+{
+    if (c == 'r' || c == 'R')
+    {
+        Serial.println("Command: reset");
+
+        context->to_initialization();
+    }
+
+    else if (c == 'o' || c == 'O')
+    {
+        Serial.println("Command: set operational");
+
+        context->to_operational();
+    }
+}
+
+
+void StoppedState::exit()
+{
+    digitalWrite(LED_BUILTIN, LOW);
+}