diff --git a/.gitignore b/.gitignore
index 8b95b18..5e167ba 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,8 @@
 **/.vscode/
 .DS_Store
 Thumbs.db
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch
\ No newline at end of file
diff --git a/Project_2_Speed_Controller/Part_1_Motor_Speed/src/encoder.cpp b/Project_2_Speed_Controller/Part_1_Motor_Speed/src/encoder.cpp
index 471ead2..915c6ca 100644
--- a/Project_2_Speed_Controller/Part_1_Motor_Speed/src/encoder.cpp
+++ b/Project_2_Speed_Controller/Part_1_Motor_Speed/src/encoder.cpp
@@ -46,9 +46,9 @@ void Encoder::init()
     // Enable global interrupts
     sei();
 
-    // Set initial states for speed
-    pastCount = count;
-    pastTime = micros();
+        // Set initial states for speed
+        pastCount = count;
+        pastTime = micros();
 
 }
 
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/Initialization.h b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/Initialization.h
new file mode 100644
index 0000000..a7809a3
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/Initialization.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include <Arduino.h>
+#include "State.h"
+
+class Idle : public State
+{
+private:
+    uint32_t timer;
+
+public:
+    void on_do() override;
+    void on_entry() override;
+    void on_exit() override;
+    // transisions from Initialization
+
+    void o_pressed() override;
+    //void preoperational() override;
+};
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/Operational.h b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/Operational.h
new file mode 100644
index 0000000..00a91e3
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/Operational.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <Arduino.h>
+#include "State.h"
+
+class Operational : public State
+{
+private:
+    uint32_t timer;
+
+public:
+    void on_do() override;
+    void on_entry() override;
+    void on_exit() override;
+
+    void Reset() override;
+};
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/context.h b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/context.h
new file mode 100644
index 0000000..154feaf
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/context.h
@@ -0,0 +1,28 @@
+#pragma once
+
+#include "State.h"
+/**
+ * The Context defines the interface of interest to clients. It also maintains a
+ * reference to an instance of a State subclass, which represents the current
+ * state of the Context.
+ */
+
+
+
+class Context
+{
+private:
+    State *state_;
+
+public:
+    Context(State *state);
+    ~Context();
+
+    void transition_to(State *state);
+
+    void do_work();
+
+    void o_pressed();
+    void reset();
+
+};
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/digital_in.h b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/digital_in.h
new file mode 100644
index 0000000..a3505c6
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/digital_in.h
@@ -0,0 +1,20 @@
+#ifndef DIGITAL_IN_H
+#define DIGITAL_IN_H
+
+#include <stdint.h>
+
+class Digital_in
+{
+public:
+    Digital_in(int pin);
+
+    void init();
+
+    bool is_hi();
+    bool is_lo();
+
+private:
+    uint8_t pinMask;
+};
+
+#endif
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/digital_out.h b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/digital_out.h
new file mode 100644
index 0000000..dd2b776
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/digital_out.h
@@ -0,0 +1,20 @@
+#ifndef DIGITAL_OUT_H
+#define DIGITAL_OUT_H
+
+#include <stdint.h>
+
+class Digital_out
+{
+public:
+    Digital_out(int pin);
+
+    void init();
+    void set_hi();
+    void set_lo();
+    void toggle();
+
+private:
+    uint8_t pinMask;
+};
+
+#endif
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/encoder.h b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/encoder.h
new file mode 100644
index 0000000..ea9307a
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/encoder.h
@@ -0,0 +1,29 @@
+#ifndef ENCODER_H
+#define ENCODER_H
+
+#include <digital_in.h>
+#include <digital_out.h>
+
+class Encoder
+{
+public:
+    Encoder(int c1Pin, int c2Pin, int ledPin);
+
+    void init();
+    void update();
+    int position();
+    int speed();
+
+private:
+    Digital_in c1;
+    Digital_in c2;
+    Digital_out led;
+
+    bool previousC1;
+    volatile int count;
+    int pastCount;
+    uint32_t pastTime;
+
+};
+
+#endif
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/p_controller.h b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/p_controller.h
new file mode 100644
index 0000000..c7666d1
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/p_controller.h
@@ -0,0 +1,14 @@
+#ifndef P_CONTROLLER_H
+#define P_CONTROLLER_H
+
+class P_controller
+{
+public:
+    P_controller(double kp);
+    double update(double ref, double actual);
+
+private:
+    double kp;
+};
+
+#endif
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/state.h b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/state.h
new file mode 100644
index 0000000..030acab
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/include/state.h
@@ -0,0 +1,34 @@
+#pragma once
+
+class Context;
+
+class State
+{
+protected:
+    Context *context_;
+
+public:
+    virtual ~State()
+    {
+    }
+
+    void set_context(Context *context)
+    {
+        this->context_ = context;
+    }
+
+    virtual void on_do() = 0;
+    virtual void on_entry() = 0;
+    virtual void on_exit() = 0;
+
+
+    //initialize transisions 
+    virtual void reset()
+    {
+    }
+    
+    virtual void o_pressed()
+    {
+    }
+
+};
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/lib/README b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/lib/README
new file mode 100644
index 0000000..9379397
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/lib/README
@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into the executable file.
+
+The source code of each library should be placed in a separate directory
+("lib/your_library_name/[Code]").
+
+For example, see the structure of the following example libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional. for custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+Example contents of `src/main.c` using Foo and Bar:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+The PlatformIO Library Dependency Finder will find automatically dependent
+libraries by scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/lib/docs/diagrams/src/state.wsd b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/lib/docs/diagrams/src/state.wsd
new file mode 100644
index 0000000..2d6fdd2
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/lib/docs/diagrams/src/state.wsd
@@ -0,0 +1,27 @@
+    @startuml
+
+    'Start at initialization
+    [*]--> Initialization
+
+    'states'
+    Initialization: entry / initialize internal parameters
+    
+    Operational: entry / Turn on Led
+    Operational: do / normal input-output behavior
+    Operational: exit / stop operations
+    
+    '-  state changes -
+    Initialization -> Operational: "o" pressed
+    Initialization <- Operational: reset / "r" pressed
+    
+
+
+
+    'Idle -> Cooling: too hot
+    'Idle -> Heating: too cool [restart time >= 5 mins]
+
+    'Cooling -> Idle: ok
+    'Cooling -> Failure : failure
+
+
+    @enduml
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/platformio.ini b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/platformio.ini
new file mode 100644
index 0000000..4dbfb2f
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/platformio.ini
@@ -0,0 +1,15 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:nanoatmega328new]
+platform = atmelavr
+board = nanoatmega328new
+framework = arduino
+monitor_speed = 115200
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/Initialization.cpp b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/Initialization.cpp
new file mode 100644
index 0000000..a69877f
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/Initialization.cpp
@@ -0,0 +1,40 @@
+#include "Initialization.h"
+#include "Context.h"
+
+// include things that will transition to
+//#include "Operational.h"
+//#include "PreOperational.h"
+
+//const uint32_t restart_time = 10000; // 300000 = 5 minutes
+
+// initialization setup
+void Initialization::on_entry()
+{
+    Serial.println("Now Initialization");
+
+    // initialize parameters here
+
+    Serial.println("Boot-up")
+    //timer = millis();
+}
+
+void Initialization::on_do()
+{
+    //if (millis() - timer >= restart_time)
+    //{
+    //    too_cold();
+    //}
+}
+
+void Initialization::on_exit()
+{
+}
+
+
+// special transistions  
+void Initialization::o_pressed()
+{
+    Serial.println(""O" pressed");
+
+    this->context_->transition_to(new Operational);
+}
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/Operational.cpp b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/Operational.cpp
new file mode 100644
index 0000000..35c9613
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/Operational.cpp
@@ -0,0 +1,34 @@
+#include "Operational.h"
+#include "Context.h"
+#include "Initialization.h"
+
+
+
+void Operational::on_entry()
+{
+    Serial.println("Operational Startup");
+
+    timer = millis();
+}
+
+void Operational::on_do()
+{
+    // Automatically go to Idle after 2 seconds
+    if (millis() - timer >= 2000)
+    {
+        Serial.println("2s elapsed");
+
+        this->context_->transition_to(new Idle);
+    }
+}
+
+void Operational::on_exit()
+{
+}
+
+void Operational::Reset()
+{
+    Serial.println("Reset / "r" pressed");
+
+    this->context_->transition_to(new Reset);
+}
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/context.cpp b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/context.cpp
new file mode 100644
index 0000000..1efdf4e
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/context.cpp
@@ -0,0 +1,34 @@
+#include "Context.h"
+
+Context::Context(State *state) : state_(nullptr)
+{
+    this->transition_to(state);
+}
+
+Context::~Context()
+{
+    delete state_;
+}
+
+void Context::transition_to(State *state)
+{
+    if (this->state_ != nullptr)
+    {
+        this->state_->on_exit();
+        delete this->state_;
+    }
+
+    this->state_ = state;
+
+    this->state_->set_context(this);
+
+    this->state_->on_entry();
+}
+
+
+
+
+void Context::reset()
+{
+    this->state_->Initialization();
+}
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/digital_in.cpp b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/digital_in.cpp
new file mode 100644
index 0000000..2bbe6ef
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/digital_in.cpp
@@ -0,0 +1,26 @@
+#include <digital_in.h>
+#include <avr/io.h>
+
+Digital_in::Digital_in(int pin)
+{
+    pinMask = static_cast<uint8_t>(1U << pin);
+}
+
+void Digital_in::init()
+{
+    // Configure pin as input
+    DDRB &= static_cast<uint8_t>(~pinMask);
+
+    // Enable internal pull-up resistor
+    PORTB |= pinMask;
+}
+
+bool Digital_in::is_hi()
+{
+    return (PINB & pinMask) != 0;
+}
+
+bool Digital_in::is_lo()
+{
+    return (PINB & pinMask) == 0;
+}
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/digital_out.cpp b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/digital_out.cpp
new file mode 100644
index 0000000..2ccfa7d
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/digital_out.cpp
@@ -0,0 +1,27 @@
+#include <digital_out.h>
+#include <avr/io.h>
+
+Digital_out::Digital_out(int pin)
+{
+    pinMask = static_cast<uint8_t>(1U << pin);
+}
+
+void Digital_out::init()
+{
+    DDRB |= pinMask;
+}
+
+void Digital_out::set_hi()
+{
+    PORTB |= pinMask;
+}
+
+void Digital_out::set_lo()
+{
+    PORTB &= static_cast<uint8_t>(~pinMask);
+}
+
+void Digital_out::toggle()
+{
+    PORTB ^= pinMask;
+}
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/encoder.cpp b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/encoder.cpp
new file mode 100644
index 0000000..471ead2
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/encoder.cpp
@@ -0,0 +1,106 @@
+#include <encoder.h>
+#include <Arduino.h>
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+static Encoder* activeEncoder = nullptr;
+
+ISR(PCINT0_vect)
+{
+    if (activeEncoder != nullptr)
+    {
+        activeEncoder->update();
+    }
+}
+
+Encoder::Encoder(int c1Pin, int c2Pin, int ledPin)
+    : c1(c1Pin), c2(c2Pin), led(ledPin),
+      previousC1(false), count(0)
+{
+}
+
+void Encoder::init()
+{
+    c1.init();
+    c2.init();
+    led.init();
+
+    led.set_lo();
+
+    activeEncoder = this;
+
+    // Clear any pending PORTB pin-change interrupt
+    PCIFR |= (1 << PCIF0);
+
+    // Enable pin-change interrupts for PORTB 
+    PCICR |= (1 << PCIE0);
+
+    // Enable interrupt specifically for PB3 / D8 / C1 
+    PCMSK0 |= (1 << PCINT0);
+
+
+    //capture baseline
+    previousC1 = c1.is_hi();
+    count = 0;
+
+    // Enable global interrupts
+    sei();
+
+    // Set initial states for speed
+    pastCount = count;
+    pastTime = micros();
+
+}
+
+void Encoder::update()
+{
+    led.set_lo();
+
+    bool currentC1 = c1.is_hi();
+
+    if (currentC1 != previousC1)
+    {
+        bool currentC2 = c2.is_hi();
+
+        if (currentC1 == currentC2)
+        {
+            count++;
+        }
+        else
+        {
+            count--;
+        }
+
+        previousC1 = currentC1;
+
+        led.set_hi();
+    }
+}
+
+int Encoder::position()
+{
+    uint8_t oldSREG = SREG;
+
+    cli();
+    int currentCount = count;
+    SREG = oldSREG;
+
+    return currentCount;
+}
+
+
+
+int Encoder::speed()
+{
+    int currentCount = position();
+    uint32_t currentTime = micros(); //gives arduino microseconds since program started running
+
+    //from spec sheet we get 1400 counts per revolution from driver 
+    // or from assignment 1: 2100
+    int rpm = ((currentCount-pastCount)*6e7)/((currentTime-pastTime)*2100);  // we turn counts to rotations by dividing by 1400 and microsecond to minutes by multiplying with 60 and 10^6 
+
+    pastTime = currentTime;
+    pastCount = currentCount;
+
+    return rpm;
+}
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/main.cpp b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/main.cpp
new file mode 100644
index 0000000..f9f9630
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/main.cpp
@@ -0,0 +1,45 @@
+// State Behaviour based on the C++ example at Refactoring Guru
+#include <Arduino.h>
+#include <avr/interrupt.h>
+
+#include "Context.h"
+#include "Initialization.h"
+
+Context *context;
+
+void setup()
+{
+    Serial.begin(9600);
+    sei();
+
+    // Start the state machine in Idle
+    context = new Context(new Idle);
+}
+
+void loop()
+{
+    int c = 0;
+
+    if (Serial.available() > 0)
+    {
+        c = Serial.read();
+
+        Serial.print("I received: ");
+        Serial.println(c, DEC);
+
+        if (c == 'o')
+        {
+            // Initializing -> operational
+            context->o_pressed();
+        }
+
+        if (c == 'r')
+        {
+            // operational -> initializing
+            context->reset();
+        }
+    }
+
+    // Run the behavior of the current state
+    context->do_work();
+}
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/p_controller.cpp b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/p_controller.cpp
new file mode 100644
index 0000000..4536a71
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/p_controller.cpp
@@ -0,0 +1,11 @@
+#include <p_controller.h>
+
+P_controller::P_controller(double kp)
+    : kp(kp)
+{
+}
+
+double P_controller::update(double ref, double actual)
+{
+    return kp * (ref - actual);
+}
\ No newline at end of file
diff --git a/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/speedcontrollermain.cpp b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/speedcontrollermain.cpp
new file mode 100644
index 0000000..c338d9b
--- /dev/null
+++ b/Project_3_Dynamic_controller_behaviour/Part_1_INIT_and_OP/src/speedcontrollermain.cpp
@@ -0,0 +1,177 @@
+/*
+
+#include <Arduino.h>
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+#include <encoder.h>
+#include <p_controller.h>
+
+Encoder encoder(0, 1, 5);
+P_controller controller(40.0);
+
+volatile bool updateFlag = false;
+
+ISR(TIMER1_COMPA_vect)
+{
+    updateFlag = true;
+}
+
+
+int main()
+{
+    init();
+
+    encoder.init();
+
+    Serial.begin(115200);
+
+    // D11 / PB3 -> AIN1
+    DDRB |= (1 << PB3);
+    PORTB &= ~(1 << PB3);
+
+    // D3 / PD3 / OC2B -> AIN2 PWM
+    DDRD |= (1 << PD3);
+
+    // Timer1: 250 Hz
+    TCCR1A = 0;
+    TCCR1B = 0;
+    TCNT1 = 0;
+
+    OCR1A = 999;
+
+    TCCR1B |= (1 << WGM12);
+    TCCR1B |= (1 << CS11) | (1 << CS10);
+
+    TIMSK1 |= (1 << OCIE1A);
+
+    // Timer2: 7.8 kHz PWM
+    TCCR2A = 0;
+    TCCR2B = 0;
+    TCNT2 = 0;
+
+    OCR2B = 0;
+
+    TCCR2A =
+        (1 << COM2B1) |
+        (1 << WGM21) |
+        (1 << WGM20);
+
+    TCCR2B = (1 << CS21);
+
+    sei();
+
+    Serial.println("Load test starting in 2 seconds...");
+
+    uint16_t waitCount = 0;
+
+    while (waitCount < 500)
+    {
+        if (updateFlag)
+        {
+            updateFlag = false;
+            waitCount++;
+        }
+    }
+
+    int previousCount = encoder.position();
+
+    int countHistory[10] = {0};
+    int rollingCounts = 0;
+
+    uint8_t historyIndex = 0;
+    uint8_t samplesFilled = 0;
+
+    uint32_t updateCount = 0;
+
+    const double reference = 50.0;
+
+    Serial.println("time_ms,reference_rpm,actual_rpm,pwm");
+
+    while (1)
+    {
+        if (updateFlag)
+        {
+            updateFlag = false;
+            updateCount++;
+
+            uint32_t time_ms = updateCount * 4;
+
+            int currentCount = encoder.position();
+
+            int deltaCount = currentCount - previousCount;
+            previousCount = currentCount;
+
+            rollingCounts -= countHistory[historyIndex];
+
+            countHistory[historyIndex] = deltaCount;
+            rollingCounts += deltaCount;
+
+            historyIndex++;
+
+            if (historyIndex >= 10)
+            {
+                historyIndex = 0;
+            }
+
+            if (samplesFilled < 10)
+            {
+                samplesFilled++;
+            }
+
+            double windowTime = samplesFilled * 0.004;
+
+            double rpm =
+                (rollingCounts * 60.0) /
+                (2100.0 * windowTime);
+
+            double output =
+                controller.update(reference, rpm);
+
+            if (output < 0.0)
+            {
+                output = 0.0;
+            }
+
+            if (output > 255.0)
+            {
+                output = 255.0;
+            }
+
+            uint8_t pwm = (uint8_t)output;
+
+            OCR2B = pwm;
+
+            if ((updateCount % 10) == 0)
+            {
+                Serial.print(time_ms);
+                Serial.print(",");
+
+                Serial.print(reference);
+                Serial.print(",");
+
+                Serial.print(rpm);
+                Serial.print(",");
+
+                Serial.println(pwm);
+            }
+
+            if (time_ms >= 10000)
+            {
+                OCR2B = 0;
+
+                Serial.println("TEST COMPLETE");
+
+                break;
+            }
+        }
+    }
+
+    while (1)
+    {
+    }
+
+    return 0;
+}
+
+*/
\ No newline at end of file