Fixed file management setup and pushed completed part 2 code

2026-10-03 14:20:27 +00:00
parent 6f3c388cef
commit e6f164b25f
79 changed files with 1714 additions and 11 deletions
@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch
@@ -15,9 +15,9 @@
 #include <initialization.h>
 #include <operational.h>

-// --------------------------------------------------
+// --------------------
 // Hardware / drivers
-// --------------------------------------------------
+// --------------------

 // Encoder:
 // PB0 = D8
@@ -49,9 +49,9 @@ Speed_controller speedController(
    pController
 );

-// --------------------------------------------------
+// --------------------
 // State machine states
-// --------------------------------------------------
+// --------------------

 Initialization initialization(
    speedController,
@@ -109,9 +109,9 @@ void setup()

 void loop()
 {
-    // ----------------------------------------------
+    // -------------
    // Serial events
-    // ----------------------------------------------
+    // -------------

    if (Serial.available() > 0)
    {
@@ -130,9 +130,9 @@ void loop()
        }
    }

-    // ----------------------------------------------
+    // ----------------------------
    // Stable 250 Hz state activity
-    // ----------------------------------------------
+    // ----------------------------

    if (controlTimer.take_tick())
    {
@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch
@@ -0,0 +1,21 @@
+@startuml
+
+[*] --> Initialization
+
+state Initialization
+state Operational
+
+Initialization : entry / initialize parameters
+Initialization : entry / send Boot-up
+Initialization : do / wait for command
+
+Operational : entry / enable motor control
+Operational : entry / LED ON
+Operational : do / update speed controller at 250 Hz
+Operational : exit / stop motor control
+Operational : exit / LED OFF
+
+Initialization --> Operational : set operational / 'o'
+Operational --> Initialization : reset / 'r'
+
+@enduml
@@ -0,0 +1,30 @@
+@startuml
+
+[*] --> Initialization
+
+state Initialization
+state Operational
+state Stopped
+
+Initialization : entry / initialize parameters
+Initialization : entry / send Boot-up
+Initialization : do / wait for command
+
+Operational : entry / enable motor control
+Operational : entry / LED ON
+Operational : do / update speed controller at 250 Hz
+Operational : exit / stop motor control
+Operational : exit / LED OFF
+
+Stopped : entry / brake motor
+Stopped : entry / disable normal controller activity
+Stopped : do / LED blink at 2 Hz
+Stopped : exit / LED OFF
+
+Initialization --> Operational : set operational / 'o'
+Operational --> Initialization : reset / 'r'
+
+Operational --> Stopped : emergency stop / fault
+Stopped --> Initialization : reset / 'r'
+
+@enduml
@@ -0,0 +1,37 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the convention is to give header files names that end with `.h'.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html
@@ -0,0 +1,26 @@
+#ifndef CONTEXT_H
+#define CONTEXT_H
+
+#include <state.h>
+
+class Context
+{
+public:
+    Context(State& initialState);
+
+    void transition_to(State& newState);
+
+    void do_work();
+
+    void reset();
+    void set_operational();
+    void set_pre_operational();
+    void fault();
+
+    const char* current_state_name() const;
+
+private:
+    State* state;
+};
+
+#endif
@@ -0,0 +1,17 @@
+#ifndef CONTROL_TIMER_H
+#define CONTROL_TIMER_H
+
+#include <stdint.h>
+
+class Control_timer
+{
+public:
+    void init();
+
+    // Returns true once for every 4 ms control tick.
+    bool take_tick();
+
+    uint32_t tick_count() const;
+};
+
+#endif
@@ -0,0 +1,19 @@
+#ifndef CONTROLLER_H
+#define CONTROLLER_H
+
+class Controller
+{
+public:
+    virtual ~Controller() = default;
+
+    virtual double update(
+        double reference,
+        double actual
+    ) = 0;
+
+    virtual void reset() = 0;
+
+    virtual const char* name() const = 0;
+};
+
+#endif
@@ -0,0 +1,22 @@
+#ifndef DIGITAL_IN_H
+#define DIGITAL_IN_H
+
+#include <stdint.h>
+
+class Digital_in
+{
+public:
+    // pin is the PORTB bit number:
+    // PB0 = D8, PB1 = D9, ..., PB5 = D13
+    Digital_in(uint8_t pin);
+
+    void init();
+
+    bool is_hi() const;
+    bool is_lo() const;
+
+private:
+    uint8_t pinMask;
+};
+
+#endif
@@ -0,0 +1,23 @@
+#ifndef DIGITAL_OUT_H
+#define DIGITAL_OUT_H
+
+#include <stdint.h>
+
+class Digital_out
+{
+public:
+    // pin is the PORTB bit number:
+    // PB0 = D8, ..., PB5 = D13
+    Digital_out(uint8_t pin);
+
+    void init();
+
+    void set_hi();
+    void set_lo();
+    void toggle();
+
+private:
+    uint8_t pinMask;
+};
+
+#endif
@@ -0,0 +1,28 @@
+#ifndef ENCODER_H
+#define ENCODER_H
+
+#include <stdint.h>
+#include <digital_in.h>
+
+class Encoder
+{
+public:
+    Encoder(uint8_t c1Pin, uint8_t c2Pin);
+
+    void init();
+    void update();
+
+    int32_t position() const;
+
+private:
+    Digital_in c1;
+    Digital_in c2;
+
+    uint8_t c1Pin;
+
+    bool previousC1;
+
+    volatile int32_t count;
+};
+
+#endif
@@ -0,0 +1,35 @@
+#ifndef INITIALIZATION_H
+#define INITIALIZATION_H
+
+#include <state.h>
+#include <speed_controller.h>
+#include <digital_out.h>
+
+class Initialization : public State
+{
+public:
+    Initialization(
+        Speed_controller& speedController,
+        Digital_out& statusLED
+    );
+
+    const char* name() const override;
+
+    void on_entry() override;
+    void on_do() override;
+    void on_exit() override;
+
+    void set_operational() override;
+
+    void set_operational_state(
+        State& operationalState
+    );
+
+private:
+    Speed_controller& speedController;
+    Digital_out& statusLED;
+
+    State* operationalState;
+};
+
+#endif
@@ -0,0 +1,26 @@
+#ifndef MOTOR_DRIVER_H
+#define MOTOR_DRIVER_H
+
+class Motor_driver
+{
+public:
+    void init();
+
+    // Range: -255 ... +255
+    void set_command(double command);
+
+    void coast();
+
+    void brake();
+
+    double command() const;
+
+private:
+    double currentCommand;
+
+    void enable_pwm_outputs();
+
+    unsigned char magnitude(double value) const;
+};
+
+#endif
@@ -0,0 +1,45 @@
+#ifndef OPERATIONAL_H
+#define OPERATIONAL_H
+
+#include <stdint.h>
+
+#include <state.h>
+#include <speed_controller.h>
+#include <digital_out.h>
+
+class Operational : public State
+{
+public:
+    Operational(
+        Speed_controller& speedController,
+        Digital_out& statusLED
+    );
+
+    const char* name() const override;
+
+    void on_entry() override;
+    void on_do() override;
+    void on_exit() override;
+
+    void reset() override;
+    void fault() override;
+
+    void set_initialization_state(
+        State& initializationState
+    );
+
+    void set_stopped_state(
+        State& stoppedState
+    );
+
+private:
+    Speed_controller& speedController;
+    Digital_out& statusLED;
+
+    State* initializationState;
+    State* stoppedState;
+
+    uint8_t logCounter;
+};
+
+#endif
@@ -0,0 +1,29 @@
+#ifndef P_CONTROLLER_H
+#define P_CONTROLLER_H
+
+#include <controller.h>
+
+class P_controller : public Controller
+{
+public:
+    P_controller(double kp);
+
+    double update(
+        double reference,
+        double actual
+    ) override;
+
+    void reset() override;
+
+    const char* name() const override;
+
+    void set_kp(double kp);
+    double get_kp() const;
+
+private:
+    double kp;
+
+    double limit(double value) const;
+};
+
+#endif
@@ -0,0 +1,47 @@
+#ifndef SPEED_CONTROLLER_H
+#define SPEED_CONTROLLER_H
+
+#include <encoder.h>
+#include <speed_estimator.h>
+#include <motor_driver.h>
+#include <controller.h>
+
+class Speed_controller
+{
+public:
+    Speed_controller(
+        Encoder& encoder,
+        Speed_estimator& estimator,
+        Motor_driver& motor,
+        Controller& controller
+    );
+
+    void start();
+    void stop();
+    void brake();
+
+    void update();
+
+    void set_reference(double rpm);
+    double reference() const;
+
+    void set_controller(Controller& controller);
+
+    double actual_rpm() const;
+    double control_output() const;
+
+private:
+    Encoder& encoder;
+    Speed_estimator& estimator;
+    Motor_driver& motor;
+
+    Controller* controller;
+
+    bool running;
+
+    double referenceRPM;
+    double actualRPM;
+    double controlOutput;
+};
+
+#endif
@@ -0,0 +1,37 @@
+#ifndef SPEED_ESTIMATOR_H
+#define SPEED_ESTIMATOR_H
+
+#include <stdint.h>
+
+class Speed_estimator
+{
+public:
+    Speed_estimator(
+        double countsPerRevolution,
+        double sampleTimeSeconds
+    );
+
+    void reset(int32_t initialPosition);
+
+    double update(int32_t currentPosition);
+
+    double rpm() const;
+
+private:
+    static const uint8_t WINDOW_SIZE = 10;
+
+    double countsPerRevolution;
+    double sampleTime;
+
+    int32_t previousPosition;
+
+    int32_t countHistory[WINDOW_SIZE];
+    int32_t rollingCounts;
+
+    uint8_t historyIndex;
+    uint8_t samplesFilled;
+
+    double currentRPM;
+};
+
+#endif
@@ -0,0 +1,43 @@
+#ifndef STATE_H
+#define STATE_H
+
+class Context;
+
+class State
+{
+protected:
+    Context* context_ = nullptr;
+
+public:
+    virtual ~State() = default;
+
+    void set_context(Context* context)
+    {
+        context_ = context;
+    }
+
+    virtual const char* name() const = 0;
+
+    virtual void on_entry() = 0;
+    virtual void on_do() = 0;
+    virtual void on_exit() = 0;
+
+    // Events
+    virtual void reset()
+    {
+    }
+
+    virtual void set_operational()
+    {
+    }
+
+    virtual void set_pre_operational()
+    {
+    }
+
+    virtual void fault()
+    {
+    }
+};
+
+#endif
@@ -0,0 +1,39 @@
+#ifndef STOPPED_H
+#define STOPPED_H
+
+#include <stdint.h>
+
+#include <state.h>
+#include <speed_controller.h>
+#include <digital_out.h>
+
+class Stopped : public State
+{
+public:
+    Stopped(
+        Speed_controller& speedController,
+        Digital_out& statusLED
+    );
+
+    const char* name() const override;
+
+    void on_entry() override;
+    void on_do() override;
+    void on_exit() override;
+
+    void reset() override;
+
+    void set_initialization_state(
+        State& initializationState
+    );
+
+private:
+    Speed_controller& speedController;
+    Digital_out& statusLED;
+
+    State* initializationState;
+
+    uint8_t ledCounter;
+};
+
+#endif
@@ -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
@@ -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
@@ -0,0 +1,80 @@
+#include <Arduino.h>
+
+#include <context.h>
+
+Context::Context(State& initialState)
+    : state(nullptr)
+{
+    transition_to(initialState);
+}
+
+void Context::transition_to(
+    State& newState
+)
+{
+    if (state != nullptr)
+    {
+        state->on_exit();
+
+        Serial.print("Transition: ");
+        Serial.print(state->name());
+        Serial.print(" -> ");
+        Serial.println(newState.name());
+    }
+
+    state = &newState;
+
+    state->set_context(this);
+
+    state->on_entry();
+}
+
+void Context::do_work()
+{
+    if (state != nullptr)
+    {
+        state->on_do();
+    }
+}
+
+void Context::reset()
+{
+    if (state != nullptr)
+    {
+        state->reset();
+    }
+}
+
+void Context::set_operational()
+{
+    if (state != nullptr)
+    {
+        state->set_operational();
+    }
+}
+
+void Context::set_pre_operational()
+{
+    if (state != nullptr)
+    {
+        state->set_pre_operational();
+    }
+}
+
+void Context::fault()
+{
+    if (state != nullptr)
+    {
+        state->fault();
+    }
+}
+
+const char* Context::current_state_name() const
+{
+    if (state == nullptr)
+    {
+        return "None";
+    }
+
+    return state->name();
+}
@@ -0,0 +1,70 @@
+#include <control_timer.h>
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <util/atomic.h>
+
+static volatile uint8_t pendingTicks = 0;
+static volatile uint32_t totalTicks = 0;
+
+ISR(TIMER1_COMPA_vect)
+{
+    totalTicks++;
+
+    if (pendingTicks < 255)
+    {
+        pendingTicks++;
+    }
+}
+
+void Control_timer::init()
+{
+    pendingTicks = 0;
+    totalTicks = 0;
+
+    // Timer1 CTC mode, 250 Hz
+    // 16 MHz / 64 / (999 + 1) = 250 Hz
+
+    TCCR1A = 0;
+    TCCR1B = 0;
+    TCNT1 = 0;
+
+    OCR1A = 999;
+
+    TCCR1B |= (1 << WGM12);
+
+    // Prescaler = 64
+    TCCR1B |=
+        (1 << CS11) |
+        (1 << CS10);
+
+    TIMSK1 |= (1 << OCIE1A);
+}
+
+bool Control_timer::take_tick()
+{
+    bool tickAvailable = false;
+
+    ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
+    {
+        if (pendingTicks > 0)
+        {
+            pendingTicks--;
+            tickAvailable = true;
+        }
+    }
+
+    return tickAvailable;
+}
+
+uint32_t Control_timer::tick_count() const
+{
+    uint32_t ticks;
+
+    ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
+    {
+        ticks = totalTicks;
+    }
+
+    return ticks;
+}
@@ -0,0 +1,26 @@
+#include <digital_in.h>
+#include <avr/io.h>
+
+Digital_in::Digital_in(uint8_t pin)
+{
+    pinMask = static_cast<uint8_t>(1U << pin);
+}
+
+void Digital_in::init()
+{
+    // Input
+    DDRB &= static_cast<uint8_t>(~pinMask);
+
+    // Internal pull-up
+    PORTB |= pinMask;
+}
+
+bool Digital_in::is_hi() const
+{
+    return (PINB & pinMask) != 0;
+}
+
+bool Digital_in::is_lo() const
+{
+    return (PINB & pinMask) == 0;
+}
@@ -0,0 +1,27 @@
+#include <digital_out.h>
+#include <avr/io.h>
+
+Digital_out::Digital_out(uint8_t 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;
+}
@@ -0,0 +1,75 @@
+#include <encoder.h>
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+static Encoder* activeEncoder = nullptr;
+
+ISR(PCINT0_vect)
+{
+    if (activeEncoder != nullptr)
+    {
+        activeEncoder->update();
+    }
+}
+
+Encoder::Encoder(uint8_t c1Pin, uint8_t c2Pin)
+    : c1(c1Pin),
+      c2(c2Pin),
+      c1Pin(c1Pin),
+      previousC1(false),
+      count(0)
+{
+}
+
+void Encoder::init()
+{
+    c1.init();
+    c2.init();
+
+    activeEncoder = this;
+
+    previousC1 = c1.is_hi();
+    count = 0;
+
+    // Clear pending PORTB pin-change interrupt
+    PCIFR |= (1 << PCIF0);
+
+    // Enable PORTB pin-change interrupts
+    PCICR |= (1 << PCIE0);
+
+    // Enable interrupt on C1
+    PCMSK0 |= (1 << c1Pin);
+}
+
+void Encoder::update()
+{
+    bool currentC1 = c1.is_hi();
+
+    if (currentC1 != previousC1)
+    {
+        bool currentC2 = c2.is_hi();
+
+        if (currentC1 == currentC2)
+        {
+            count++;
+        }
+        else
+        {
+            count--;
+        }
+
+        previousC1 = currentC1;
+    }
+}
+
+int32_t Encoder::position() const
+{
+    uint8_t oldSREG = SREG;
+
+    cli();
+    int32_t currentCount = count;
+    SREG = oldSREG;
+
+    return currentCount;
+}
@@ -0,0 +1,57 @@
+#include <Arduino.h>
+
+#include <initialization.h>
+#include <context.h>
+
+Initialization::Initialization(
+    Speed_controller& speedController,
+    Digital_out& statusLED
+)
+    : speedController(speedController),
+      statusLED(statusLED),
+      operationalState(nullptr)
+{
+}
+
+const char* Initialization::name() const
+{
+    return "Initialization";
+}
+
+void Initialization::on_entry()
+{
+    speedController.stop();
+
+    statusLED.set_lo();
+
+    Serial.println();
+    Serial.println("Initialization");
+    Serial.println("Boot-up");
+}
+
+void Initialization::on_do()
+{
+    // No continuous activity required
+    // in Part 1 Initialization.
+}
+
+void Initialization::on_exit()
+{
+}
+
+void Initialization::set_operational()
+{
+    if (operationalState != nullptr)
+    {
+        context_->transition_to(
+            *operationalState
+        );
+    }
+}
+
+void Initialization::set_operational_state(
+    State& state
+)
+{
+    operationalState = &state;
+}
@@ -0,0 +1,172 @@
+#include <Arduino.h>
+
+#include <digital_in.h>
+#include <digital_out.h>
+#include <encoder.h>
+
+#include <control_timer.h>
+#include <speed_estimator.h>
+
+#include <motor_driver.h>
+
+#include <p_controller.h>
+#include <speed_controller.h>
+
+#include <context.h>
+#include <initialization.h>
+#include <operational.h>
+#include <stopped.h>
+
+// --------------------------------------------------
+// Hardware / drivers
+// --------------------------------------------------
+
+// Encoder:
+// PB0 = D8
+// PB1 = D9
+Encoder encoder(0, 1);
+
+// Emergency-stop:
+// PB2 = D10
+// Internal pull-up enabled.
+// LOW = emergency stop pressed.
+Digital_in emergencyStop(2);
+
+// Status LED:
+// PB5 = D13
+Digital_out statusLED(5);
+
+Control_timer controlTimer;
+
+Motor_driver motorDriver;
+
+// 2100 counts/revolution,
+// 4 ms fixed controller period
+Speed_estimator speedEstimator(
+    2100.0,
+    0.004
+);
+
+P_controller pController(40.0);
+
+Speed_controller speedController(
+    encoder,
+    speedEstimator,
+    motorDriver,
+    pController
+);
+
+// --------------------------------------------------
+// State machine
+// --------------------------------------------------
+
+Initialization initialization(
+    speedController,
+    statusLED
+);
+
+Operational operational(
+    speedController,
+    statusLED
+);
+
+Stopped stopped(
+    speedController,
+    statusLED
+);
+
+Context* stateMachine = nullptr;
+
+void setup()
+{
+    Serial.begin(115200);
+
+    statusLED.init();
+
+    emergencyStop.init();
+
+    encoder.init();
+
+    motorDriver.init();
+
+    speedController.set_reference(
+        50.0
+    );
+
+    // --------------------------------------------------
+    // State transitions
+    // --------------------------------------------------
+
+    initialization.set_operational_state(
+        operational
+    );
+
+    operational.set_initialization_state(
+        initialization
+    );
+
+    operational.set_stopped_state(
+        stopped
+    );
+
+    stopped.set_initialization_state(
+        initialization
+    );
+
+    static Context context(
+        initialization
+    );
+
+    stateMachine = &context;
+
+    controlTimer.init();
+
+    Serial.println();
+    Serial.println("Project 3 - Part 2");
+    Serial.println("Commands:");
+    Serial.println("o = set operational");
+    Serial.println("r = reset");
+    Serial.println("D10 LOW = emergency stop");
+}
+
+void loop()
+{
+    // --------------------------------------------------
+    // Emergency-stop fault
+    // --------------------------------------------------
+
+    if (emergencyStop.is_lo())
+    {
+        stateMachine->fault();
+    }
+
+    // --------------------------------------------------
+    // Serial commands
+    // --------------------------------------------------
+
+    if (Serial.available() > 0)
+    {
+        char command =
+            Serial.read();
+
+        if (command == 'o')
+        {
+            stateMachine->
+                set_operational();
+        }
+        else if (command == 'r')
+        {
+            stateMachine->
+                reset();
+        }
+    }
+
+    // --------------------------------------------------
+    // Stable 250 Hz state activity
+    // --------------------------------------------------
+
+    if (controlTimer.take_tick())
+    {
+        stateMachine->do_work();
+    }
+}
@@ -0,0 +1,135 @@
+#include <motor_driver.h>
+
+#include <avr/io.h>
+
+void Motor_driver::init()
+{
+    currentCommand = 0.0;
+
+    // D11 / PB3 / OC2A -> AIN1
+    DDRB |= (1 << PB3);
+
+    // D3 / PD3 / OC2B -> AIN2
+    DDRD |= (1 << PD3);
+
+    PORTB &= ~(1 << PB3);
+    PORTD &= ~(1 << PD3);
+
+    // Timer2 Fast PWM
+    TCCR2A = 0;
+    TCCR2B = 0;
+    TCNT2 = 0;
+
+    OCR2A = 0;
+    OCR2B = 0;
+
+    TCCR2A =
+        (1 << COM2A1) |
+        (1 << COM2B1) |
+        (1 << WGM21) |
+        (1 << WGM20);
+
+    // Prescaler = 8
+    // PWM frequency ~= 7.8 kHz
+    TCCR2B =
+        (1 << CS21);
+}
+
+void Motor_driver::enable_pwm_outputs()
+{
+    // Non-inverting PWM on OC2A and OC2B
+    TCCR2A |=
+        (1 << COM2A1) |
+        (1 << COM2B1);
+
+    TCCR2A &=
+        ~((1 << COM2A0) |
+          (1 << COM2B0));
+}
+
+unsigned char Motor_driver::magnitude(
+    double value
+) const
+{
+    if (value < 0.0)
+    {
+        value = -value;
+    }
+
+    if (value > 255.0)
+    {
+        value = 255.0;
+    }
+
+    return static_cast<unsigned char>(value);
+}
+
+void Motor_driver::set_command(double command)
+{
+    if (command > 255.0)
+    {
+        command = 255.0;
+    }
+
+    if (command < -255.0)
+    {
+        command = -255.0;
+    }
+
+    currentCommand = command;
+
+    enable_pwm_outputs();
+
+    unsigned char pwm =
+        magnitude(command);
+
+    if (command > 0.0)
+    {
+        // Same direction that previously gave positive RPM:
+        // AIN1 = LOW
+        // AIN2 = PWM
+        OCR2A = 0;
+        OCR2B = pwm;
+    }
+    else if (command < 0.0)
+    {
+        OCR2A = pwm;
+        OCR2B = 0;
+    }
+    else
+    {
+        OCR2A = 0;
+        OCR2B = 0;
+    }
+}
+
+void Motor_driver::coast()
+{
+    currentCommand = 0.0;
+
+    enable_pwm_outputs();
+
+    OCR2A = 0;
+    OCR2B = 0;
+}
+
+void Motor_driver::brake()
+{
+    currentCommand = 0.0;
+
+    // Disconnect Timer2 PWM from the pins.
+    TCCR2A &=
+        ~((1 << COM2A1) |
+          (1 << COM2A0) |
+          (1 << COM2B1) |
+          (1 << COM2B0));
+
+    // DRV8833: both inputs HIGH gives braking.
+    PORTB |= (1 << PB3);
+    PORTD |= (1 << PD3);
+}
+
+double Motor_driver::command() const
+{
+    return currentCommand;
+}
@@ -0,0 +1,103 @@
+#include <Arduino.h>
+
+#include <operational.h>
+#include <context.h>
+
+Operational::Operational(
+    Speed_controller& speedController,
+    Digital_out& statusLED
+)
+    : speedController(speedController),
+      statusLED(statusLED),
+      initializationState(nullptr),
+      stoppedState(nullptr),
+      logCounter(0)
+{
+}
+
+const char* Operational::name() const
+{
+    return "Operational";
+}
+
+void Operational::on_entry()
+{
+    Serial.println("Operational");
+
+    statusLED.set_hi();
+
+    logCounter = 0;
+
+    speedController.start();
+}
+
+void Operational::on_do()
+{
+    speedController.update();
+
+    logCounter++;
+
+    // Print every 40 ms
+    if (logCounter >= 10)
+    {
+        logCounter = 0;
+
+        Serial.print("Reference: ");
+        Serial.print(
+            speedController.reference()
+        );
+
+        Serial.print(" RPM, Actual: ");
+        Serial.print(
+            speedController.actual_rpm()
+        );
+
+        Serial.print(" RPM, Control: ");
+        Serial.println(
+            speedController.control_output()
+        );
+    }
+}
+
+void Operational::on_exit()
+{
+    speedController.stop();
+
+    statusLED.set_lo();
+}
+
+void Operational::reset()
+{
+    if (initializationState != nullptr)
+    {
+        context_->transition_to(
+            *initializationState
+        );
+    }
+}
+
+void Operational::fault()
+{
+    if (stoppedState != nullptr)
+    {
+        Serial.println("FAULT detected");
+
+        context_->transition_to(
+            *stoppedState
+        );
+    }
+}
+
+void Operational::set_initialization_state(
+    State& state
+)
+{
+    initializationState = &state;
+}
+
+void Operational::set_stopped_state(
+    State& state
+)
+{
+    stoppedState = &state;
+}
@@ -0,0 +1,55 @@
+#include <p_controller.h>
+
+P_controller::P_controller(double kp)
+    : kp(kp)
+{
+}
+
+double P_controller::update(
+    double reference,
+    double actual
+)
+{
+    double error =
+        reference - actual;
+
+    double output =
+        kp * error;
+
+    return limit(output);
+}
+
+void P_controller::reset()
+{
+    // P controller has no stored dynamic state.
+}
+
+const char* P_controller::name() const
+{
+    return "P";
+}
+
+void P_controller::set_kp(double newKp)
+{
+    kp = newKp;
+}
+
+double P_controller::get_kp() const
+{
+    return kp;
+}
+
+double P_controller::limit(double value) const
+{
+    if (value > 255.0)
+    {
+        return 255.0;
+    }
+
+    if (value < 0.0)
+    {
+        return 0.0;
+    }
+
+    return value;
+}
@@ -0,0 +1,106 @@
+#include <speed_controller.h>
+
+Speed_controller::Speed_controller(
+    Encoder& encoder,
+    Speed_estimator& estimator,
+    Motor_driver& motor,
+    Controller& controller
+)
+    : encoder(encoder),
+      estimator(estimator),
+      motor(motor),
+      controller(&controller),
+      running(false),
+      referenceRPM(0.0),
+      actualRPM(0.0),
+      controlOutput(0.0)
+{
+}
+
+void Speed_controller::start()
+{
+    estimator.reset(
+        encoder.position()
+    );
+
+    controller->reset();
+
+    actualRPM = 0.0;
+    controlOutput = 0.0;
+
+    running = true;
+
+    motor.coast();
+}
+
+void Speed_controller::stop()
+{
+    running = false;
+
+    controlOutput = 0.0;
+
+    motor.coast();
+}
+
+void Speed_controller::brake()
+{
+    running = false;
+
+    controlOutput = 0.0;
+
+    motor.brake();
+}
+
+void Speed_controller::update()
+{
+    if (!running)
+    {
+        return;
+    }
+
+    actualRPM =
+        estimator.update(
+            encoder.position()
+        );
+
+    controlOutput =
+        controller->update(
+            referenceRPM,
+            actualRPM
+        );
+
+    motor.set_command(
+        controlOutput
+    );
+}
+
+void Speed_controller::set_reference(
+    double rpm
+)
+{
+    referenceRPM = rpm;
+}
+
+double Speed_controller::reference() const
+{
+    return referenceRPM;
+}
+
+void Speed_controller::set_controller(
+    Controller& newController
+)
+{
+    controller = &newController;
+
+    controller->reset();
+}
+
+double Speed_controller::actual_rpm() const
+{
+    return actualRPM;
+}
+
+double Speed_controller::control_output() const
+{
+    return controlOutput;
+}
@@ -0,0 +1,76 @@
+#include <speed_estimator.h>
+
+Speed_estimator::Speed_estimator(
+    double countsPerRevolution,
+    double sampleTimeSeconds
+)
+    : countsPerRevolution(countsPerRevolution),
+      sampleTime(sampleTimeSeconds),
+      previousPosition(0),
+      rollingCounts(0),
+      historyIndex(0),
+      samplesFilled(0),
+      currentRPM(0.0)
+{
+    for (uint8_t i = 0; i < WINDOW_SIZE; i++)
+    {
+        countHistory[i] = 0;
+    }
+}
+
+void Speed_estimator::reset(int32_t initialPosition)
+{
+    previousPosition = initialPosition;
+
+    rollingCounts = 0;
+    historyIndex = 0;
+    samplesFilled = 0;
+    currentRPM = 0.0;
+
+    for (uint8_t i = 0; i < WINDOW_SIZE; i++)
+    {
+        countHistory[i] = 0;
+    }
+}
+
+double Speed_estimator::update(int32_t currentPosition)
+{
+    int32_t deltaCount =
+        currentPosition - previousPosition;
+
+    previousPosition = currentPosition;
+
+    rollingCounts -=
+        countHistory[historyIndex];
+
+    countHistory[historyIndex] =
+        deltaCount;
+
+    rollingCounts += deltaCount;
+
+    historyIndex++;
+
+    if (historyIndex >= WINDOW_SIZE)
+    {
+        historyIndex = 0;
+    }
+
+    if (samplesFilled < WINDOW_SIZE)
+    {
+        samplesFilled++;
+    }
+
+    double windowTime =
+        samplesFilled * sampleTime;
+
+    currentRPM =
+        (rollingCounts * 60.0) /
+        (countsPerRevolution * windowTime);
+
+    return currentRPM;
+}
+
+double Speed_estimator::rpm() const
+{
+    return currentRPM;
+}
@@ -0,0 +1,75 @@
+#include <Arduino.h>
+
+#include <stopped.h>
+#include <context.h>
+
+Stopped::Stopped(
+    Speed_controller& speedController,
+    Digital_out& statusLED
+)
+    : speedController(speedController),
+      statusLED(statusLED),
+      initializationState(nullptr),
+      ledCounter(0)
+{
+}
+
+const char* Stopped::name() const
+{
+    return "Stopped";
+}
+
+void Stopped::on_entry()
+{
+    Serial.println("Stopped");
+    Serial.println("Emergency stop - motor brake active");
+
+    // Immediately stop normal controller operation
+    // and actively brake the motor.
+    speedController.brake();
+
+    statusLED.set_hi();
+
+    ledCounter = 0;
+}
+
+void Stopped::on_do()
+{
+    // State activity runs every 4 ms.
+    //
+    // 2 Hz LED:
+    // full period = 500 ms
+    // toggle approximately every 250 ms
+    //
+    // 250 ms / 4 ms = 62.5 ticks
+
+    ledCounter++;
+
+    if (ledCounter >= 63)
+    {
+        ledCounter = 0;
+        statusLED.toggle();
+    }
+}
+
+void Stopped::on_exit()
+{
+    statusLED.set_lo();
+}
+
+void Stopped::reset()
+{
+    if (initializationState != nullptr)
+    {
+        context_->transition_to(
+            *initializationState
+        );
+    }
+}
+
+void Stopped::set_initialization_state(
+    State& state
+)
+{
+    initializationState = &state;
+}
@@ -0,0 +1,11 @@
+
+This directory is intended for PlatformIO Test Runner and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html
@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch
@@ -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
@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch
@@ -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