added state machine behaviour with switch/case

.vscode/c_cpp_properties.json

@@ -8,29 +8,29 @@
         {
             "name": "PlatformIO",
             "includePath": [
-                "C:/Users/isakt/Embedded/ESP_Project3_H26/include",
+                "C:/Users/helgi/H26/ESP/Projects/ESP_Project3_H26/include",
-                "C:/Users/isakt/Embedded/ESP_Project3_H26/src",
+                "C:/Users/helgi/H26/ESP/Projects/ESP_Project3_H26/src",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/cores/arduino",
+                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/cores/arduino",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs",
+                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src",
+                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/HID/src",
+                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/HID/src",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
+                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src",
+                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src",
-                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/Wire/src",
+                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/Wire/src",
                 ""
             ],
             "browse": {
                 "limitSymbolsToIncludedHeaders": true,
                 "path": [
-                    "C:/Users/isakt/Embedded/ESP_Project3_H26/include",
+                    "C:/Users/helgi/H26/ESP/Projects/ESP_Project3_H26/include",
-                    "C:/Users/isakt/Embedded/ESP_Project3_H26/src",
+                    "C:/Users/helgi/H26/ESP/Projects/ESP_Project3_H26/src",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/cores/arduino",
+                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/cores/arduino",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs",
+                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src",
+                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/HID/src",
+                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/HID/src",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
+                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src",
+                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src",
-                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/Wire/src",
+                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/Wire/src",
                     ""
                 ]
             },
@@ -45,7 +45,7 @@
             ],
             "cStandard": "gnu11",
             "cppStandard": "gnu++11",
-            "compilerPath": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin/avr-gcc.exe",
+            "compilerPath": "C:/Users/helgi/.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/isakt/Embedded/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
+            "executable": "C:/Users/helgi/H26/ESP/Projects/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
             "projectEnvName": "nanoatmega328new",
-            "toolchainBinDir": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin",
+            "toolchainBinDir": "C:/Users/helgi/.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/isakt/Embedded/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
+            "executable": "C:/Users/helgi/H26/ESP/Projects/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
             "projectEnvName": "nanoatmega328new",
-            "toolchainBinDir": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin",
+            "toolchainBinDir": "C:/Users/helgi/.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",
+            "executable": "C:/Users/helgi/H26/ESP/Projects/ESP_Project3_H26/.pio/build/nanoatmega328new/firmware.elf",
             "projectEnvName": "nanoatmega328new",
-            "toolchainBinDir": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin",
+            "toolchainBinDir": "C:/Users/helgi/.platformio/packages/toolchain-atmelavr/bin",
             "internalConsoleOptions": "openOnSessionStart",
             "loadMode": "manual"
         }

docs/diagrams/src/motor_state.wsd

@@ -1,38 +0,0 @@
-@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

@@ -1,19 +0,0 @@
-#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

@@ -1,67 +0,0 @@
-#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,14 +9,10 @@ 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

@@ -1,19 +0,0 @@
-#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;   // 4 x 4 ms = 16 ms gluggi fyrir rpm-utreikning
+    static const uint8_t HIST_LEN = 4;   // 8 x 5 ms = 40 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,21 +29,14 @@ 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

@@ -1,32 +0,0 @@
-#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

@@ -1,25 +0,0 @@
-#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

@@ -14,4 +14,3 @@ board = nanoatmega328new
 framework = arduino
 
 monitor_speed = 115200
-

src/initialization_state.cpp

@@ -1,35 +0,0 @@
-#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,3 +1,5 @@
+#include <avr/io.h>
+#include <util/delay.h>
 #include <Arduino.h>
 #include <avr/interrupt.h>
 
@@ -5,80 +7,160 @@
 #include "motor_driver.h"
 #include "p_controller.h"
 
-#include "motor_controller.h"
+// RotaryEncoderDriver encoder;
+// MotorDriver motor;
 
-#include "initialization_state.h"
+// P_controller controller(12.5);
-#include "operational_state.h"
-#include "stopped_state.h"
 
+// const double REF_RPM1 = 30.0;
+// const double REF_RPM2 = 50.0;
 
-RotaryEncoderDriver encoder;
+// const uint16_t STEP1_AT_SAMPLE = 50; 
+// const uint16_t STEP2_AT_SAMPLE = 500;
 
-MotorDriver motor;
+// const uint16_t STOP_AT_SAMPLE = 1000;  
 
-P_controller controller(12.5);
+unsigned long state_entry_time;
 
-
+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
-    motor_controller.set_states(
-        &initialization_state,
-        &operational_state,
-        &stopped_state
-    );
-
-
-    motor_controller.begin();
-
-
-    while (true)
         {
+        Initialization,
+        Operational,
+        Pre_Operational,
+        Stopped
+        };
+
+    State state = State::Initialization; // Define state variable and set the initial 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);
+
+    // 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
+
         if (Serial.available() > 0)
         {
-            char c = Serial.read();
+        c = Serial.read();
+        Serial.print("I received: ");
+        Serial.println(c, DEC);
+        }
 
-            if (c != '\n' && c != '\r')
+        switch (state)
         {
-                motor_controller.command(c);
+        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;
+
+        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");
             }
 
-
-        motor_controller.update();
             }
-
+        }
-
     return 0;
 }

src/main_P2.cpp

@@ -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;
-}
- */

src/motor_controller.cpp

@@ -1,123 +0,0 @@
-#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,16 +12,10 @@ 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;
@@ -114,9 +108,3 @@ void MotorDriver::set(double pwm)
         stop();
     }
 }
-
-
-bool MotorDriver::fault_active()
-{
-    return !(PIND & (1 << PIND4));
-}

src/operational_state.cpp

@@ -1,54 +0,0 @@
-#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,22 +9,20 @@ 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();
 
@@ -34,14 +32,12 @@ 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;
 
@@ -51,31 +47,27 @@ 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 (16 ms) glugga
+    delta = count - oldest;   // breyting yfir HIST_LEN * Ts (40 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;
 
@@ -126,22 +118,18 @@ 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

@@ -1,70 +0,0 @@
-#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);
-}