Completed part 4

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/docs/diagrams/part1_state.wsd

@@ -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

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/docs/diagrams/part2_state.wsd

@@ -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

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/docs/part3_test_plan.md

@@ -0,0 +1,23 @@
+# Part 3 - PI Controller TDD
+
+## Normal flows
+
+1. Zero error produces zero output.
+2. Positive error produces positive control output.
+3. Negative error produces negative control output.
+4. Constant error accumulates integral action.
+5. The PI controller operates through the common Controller interface.
+
+## Alternate flows
+
+1. Positive output is limited to +255.
+2. Negative output is limited to -255.
+3. Positive saturation does not cause integral windup.
+4. Negative saturation does not cause integral windup.
+5. The controller recovers correctly after saturation.
+6. Reset clears the accumulated integral.
+
+The tests were written and executed before the PI control law was
+implemented. The initial run produced the expected failing tests.
+After implementing the PI control law and anti-windup behavior,
+the same test suite passed all 11 tests.

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/README

@@ -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

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/context.h

@@ -0,0 +1,32 @@
+#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();
+
+    void select_p();
+    void select_pi();
+
+    void set_kp(double value);
+    void set_ti(double value);
+
+    const char* current_state_name() const;
+
+private:
+    State* state;
+};
+
+#endif

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/control_timer.h

@@ -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

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/controller.h

@@ -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

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/digital_in.h

@@ -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

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/digital_out.h

@@ -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

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/encoder.h

@@ -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

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/initialization.h

@@ -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_pre_operational_state(
+        State& preOperationalState
+    );
+
+private:
+    Speed_controller& speedController;
+    Digital_out& statusLED;
+
+    State* preOperationalState;
+
+    bool transitionPending;
+};
+
+#endif

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/motor_driver.h

@@ -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

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/operational.h

@@ -0,0 +1,52 @@
+#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 set_pre_operational() override;
+    void fault() override;
+
+    void set_initialization_state(
+        State& initializationState
+    );
+
+    void set_pre_operational_state(
+        State& preOperationalState
+    );
+
+    void set_stopped_state(
+        State& stoppedState
+    );
+
+private:
+    Speed_controller& speedController;
+    Digital_out& statusLED;
+
+    State* initializationState;
+    State* preOperationalState;
+    State* stoppedState;
+
+    uint8_t logCounter;
+    uint32_t sampleCounter;
+};
+
+#endif

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/p_controller.h

@@ -0,0 +1,36 @@
+#ifndef P_CONTROLLER_H
+#define P_CONTROLLER_H
+
+#include <controller.h>
+
+class P_controller : public Controller
+{
+public:
+    explicit P_controller(
+        double kp
+    );
+
+    double update(
+        double reference,
+        double actual
+    ) override;
+
+    void reset() override;
+
+    const char* name() const override;
+
+    void set_kp(
+        double value
+    );
+
+    double get_kp() const;
+
+private:
+    double kp;
+
+    double limit(
+        double value
+    ) const;
+};
+
+#endif

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/pi_controller.h

@@ -0,0 +1,42 @@
+#ifndef PI_CONTROLLER_H
+#define PI_CONTROLLER_H
+
+#include <controller.h>
+
+class PI_controller : public Controller
+{
+public:
+    PI_controller(
+        double kp,
+        double ti,
+        double timestep
+    );
+
+    double update(
+        double reference,
+        double actual
+    ) override;
+
+    void reset() override;
+
+    const char* name() const override;
+
+    void set_kp(double kp);
+    double get_kp() const;
+
+    void set_ti(double ti);
+    double get_ti() const;
+
+    double get_timestep() const;
+
+private:
+    double kp;
+    double ti;
+    double timestep;
+
+    double integral;
+
+    double limit(double value) const;
+};
+
+#endif

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/pre_operational.h

@@ -0,0 +1,63 @@
+#ifndef PRE_OPERATIONAL_H
+#define PRE_OPERATIONAL_H
+
+#include <stdint.h>
+
+#include <state.h>
+#include <speed_controller.h>
+#include <digital_out.h>
+
+#include <p_controller.h>
+#include <pi_controller.h>
+
+class PreOperational : public State
+{
+public:
+    PreOperational(
+        Speed_controller& speedController,
+        Digital_out& statusLED,
+        P_controller& pController,
+        PI_controller& piController
+    );
+
+    const char* name() const override;
+
+    void on_entry() override;
+    void on_do() override;
+    void on_exit() override;
+
+    void reset() override;
+    void set_operational() override;
+
+    void select_p() override;
+    void select_pi() override;
+
+    void set_kp(double value) override;
+    void set_ti(double value) override;
+
+    void set_initialization_state(
+        State& initializationState
+    );
+
+    void set_operational_state(
+        State& operationalState
+    );
+
+private:
+    Speed_controller& speedController;
+    Digital_out& statusLED;
+
+    P_controller& pController;
+    PI_controller& piController;
+
+    State* initializationState;
+    State* operationalState;
+
+    uint8_t ledCounter;
+
+    bool piSelected;
+
+    void print_configuration();
+};
+
+#endif

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/speed_controller.h

@@ -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

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/speed_estimator.h

@@ -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

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/state.h

@@ -0,0 +1,45 @@
+#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;
+
+    // State-machine events
+    virtual void reset() {}
+    virtual void set_operational() {}
+    virtual void set_pre_operational() {}
+    virtual void fault() {}
+
+    // Pre-operational configuration events
+    virtual void select_p() {}
+    virtual void select_pi() {}
+    virtual void set_kp(double value)
+    {
+        (void)value;
+    }
+
+    virtual void set_ti(double value)
+    {
+        (void)value;
+    }
+};
+
+#endif

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/include/stopped.h

@@ -0,0 +1,51 @@
+#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_pre_operational() override;
+    void set_operational() override;
+
+    void set_initialization_state(
+        State& initializationState
+    );
+
+    void set_pre_operational_state(
+        State& preOperationalState
+    );
+
+    void set_operational_state(
+        State& operationalState
+    );
+
+private:
+    Speed_controller& speedController;
+    Digital_out& statusLED;
+
+    State* initializationState;
+    State* preOperationalState;
+    State* operationalState;
+
+    uint8_t ledCounter;
+};
+
+#endif

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/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

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/platformio.ini

@@ -8,8 +8,25 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
+[platformio]
+default_envs = nanoatmega328new
+
+
 [env:nanoatmega328new]
 platform = atmelavr
 board = nanoatmega328new
 framework = arduino
 monitor_speed = 115200
+
+
+[env:native]
+platform = native
+test_framework = unity
+test_build_src = yes
+
+build_flags =
+    -D UNITY_INCLUDE_DOUBLE
+
+build_src_filter =
+    -<*>
+    +<pi_controller.cpp>

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/context.cpp

@@ -0,0 +1,109 @@
+#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();
+    }
+}
+
+void Context::select_p()
+{
+    if (state != nullptr)
+    {
+        state->select_p();
+    }
+}
+
+void Context::select_pi()
+{
+    if (state != nullptr)
+    {
+        state->select_pi();
+    }
+}
+
+void Context::set_kp(double value)
+{
+    if (state != nullptr)
+    {
+        state->set_kp(value);
+    }
+}
+
+void Context::set_ti(double value)
+{
+    if (state != nullptr)
+    {
+        state->set_ti(value);
+    }
+}
+
+const char* Context::current_state_name() const
+{
+    if (state == nullptr)
+    {
+        return "None";
+    }
+
+    return state->name();
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/control_timer.cpp

@@ -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;
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/digital_in.cpp

@@ -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;
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/digital_out.cpp

@@ -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;
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/encoder.cpp

@@ -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;
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/initialization.cpp

@@ -0,0 +1,61 @@
+#include <Arduino.h>
+
+#include <initialization.h>
+#include <context.h>
+
+Initialization::Initialization(
+    Speed_controller& speedController,
+    Digital_out& statusLED
+)
+    : speedController(speedController),
+      statusLED(statusLED),
+      preOperationalState(nullptr),
+      transitionPending(false)
+{
+}
+
+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");
+
+    // CANopen-style autonomous transition
+    // into PreOperational.
+    transitionPending = true;
+}
+
+void Initialization::on_do()
+{
+    if (
+        transitionPending &&
+        preOperationalState != nullptr
+    )
+    {
+        transitionPending = false;
+
+        context_->transition_to(
+            *preOperationalState
+        );
+    }
+}
+
+void Initialization::on_exit()
+{
+}
+
+void Initialization::set_pre_operational_state(
+    State& state
+)
+{
+    preOperationalState = &state;
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/main.cpp

@@ -0,0 +1,416 @@
+#include <Arduino.h>
+#include <stdlib.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 <pi_controller.h>
+#include <speed_controller.h>
+
+#include <context.h>
+#include <initialization.h>
+#include <pre_operational.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;
+
+// Encoder resolution = 2100 counts/revolution
+// Controller period = 4 ms = 250 Hz
+Speed_estimator speedEstimator(
+    2100.0,
+    0.004
+);
+
+// --------------------------------------------------
+// Controllers
+// --------------------------------------------------
+
+P_controller pController(
+    40.0
+);
+
+PI_controller piController(
+    40.0,   // Kp
+    0.50,   // Ti [s]
+    0.004   // timestep [s]
+);
+
+Speed_controller speedController(
+    encoder,
+    speedEstimator,
+    motorDriver,
+    pController
+);
+
+// --------------------------------------------------
+// State machine states
+// --------------------------------------------------
+
+Initialization initialization(
+    speedController,
+    statusLED
+);
+
+PreOperational preOperational(
+    speedController,
+    statusLED,
+    pController,
+    piController
+);
+
+Operational operational(
+    speedController,
+    statusLED
+);
+
+Stopped stopped(
+    speedController,
+    statusLED
+);
+
+Context* stateMachine = nullptr;
+
+// --------------------------------------------------
+// Serial command buffer
+// --------------------------------------------------
+
+char commandBuffer[32];
+uint8_t commandIndex = 0;
+
+// --------------------------------------------------
+// Process one complete serial command
+// --------------------------------------------------
+
+void processCommand(const char* command)
+{
+    if (command[0] == '\0')
+    {
+        return;
+    }
+
+    // --------------------------------------------------
+    // Single-character commands
+    // --------------------------------------------------
+
+    if (
+        command[0] == 'o' &&
+        command[1] == '\0'
+    )
+    {
+        stateMachine->set_operational();
+    }
+
+    else if (
+        command[0] == 'c' &&
+        command[1] == '\0'
+    )
+    {
+        stateMachine->set_pre_operational();
+    }
+
+    else if (
+        command[0] == 'r' &&
+        command[1] == '\0'
+    )
+    {
+        stateMachine->reset();
+    }
+
+    else if (
+        command[0] == 'p' &&
+        command[1] == '\0'
+    )
+    {
+        stateMachine->select_p();
+    }
+
+    else if (
+        command[0] == 'i' &&
+        command[1] == '\0'
+    )
+    {
+        stateMachine->select_pi();
+    }
+
+    // --------------------------------------------------
+    // k<number> = set Kp
+    // Example:
+    // k30.625
+    // --------------------------------------------------
+
+    else if (command[0] == 'k')
+    {
+        char* endPointer;
+
+        double value =
+            strtod(
+                command + 1,
+                &endPointer
+            );
+
+        if (
+            endPointer == command + 1 ||
+            *endPointer != '\0'
+        )
+        {
+            Serial.println(
+                "Invalid Kp command."
+            );
+
+            Serial.println(
+                "Example: k30.625"
+            );
+
+            return;
+        }
+
+        stateMachine->set_kp(
+            value
+        );
+    }
+
+    // --------------------------------------------------
+    // t<number> = set Ti
+    // Example:
+    // t0.4
+    // --------------------------------------------------
+
+    else if (command[0] == 't')
+    {
+        char* endPointer;
+
+        double value =
+            strtod(
+                command + 1,
+                &endPointer
+            );
+
+        if (
+            endPointer == command + 1 ||
+            *endPointer != '\0'
+        )
+        {
+            Serial.println(
+                "Invalid Ti command."
+            );
+
+            Serial.println(
+                "Example: t0.4"
+            );
+
+            return;
+        }
+
+        stateMachine->set_ti(
+            value
+        );
+    }
+
+    else
+    {
+        Serial.print(
+            "Unknown command: "
+        );
+
+        Serial.println(
+            command
+        );
+    }
+}
+
+// --------------------------------------------------
+// Non-blocking serial command reader
+// --------------------------------------------------
+
+void readSerialCommands()
+{
+    while (Serial.available() > 0)
+    {
+        char character =
+            Serial.read();
+
+        // Ignore carriage return
+        if (character == '\r')
+        {
+            continue;
+        }
+
+        // Newline = end of command
+        if (character == '\n')
+        {
+            commandBuffer[
+                commandIndex
+            ] = '\0';
+
+            processCommand(
+                commandBuffer
+            );
+
+            commandIndex = 0;
+
+            continue;
+        }
+
+        // Add character to buffer
+        if (
+            commandIndex <
+            sizeof(commandBuffer) - 1
+        )
+        {
+            commandBuffer[
+                commandIndex
+            ] = character;
+
+            commandIndex++;
+        }
+        else
+        {
+            // Buffer overflow
+            commandIndex = 0;
+
+            Serial.println(
+                "Command too long."
+            );
+        }
+    }
+}
+
+// --------------------------------------------------
+// Setup
+// --------------------------------------------------
+
+void setup()
+{
+    Serial.begin(115200);
+
+    statusLED.init();
+
+    emergencyStop.init();
+
+    encoder.init();
+
+    motorDriver.init();
+
+    // Initial target speed
+    speedController.set_reference(
+        50.0
+    );
+
+    // --------------------------------------------------
+    // Wire state transitions
+    // --------------------------------------------------
+
+    initialization.set_pre_operational_state(
+        preOperational
+    );
+
+    preOperational.set_initialization_state(
+        initialization
+    );
+
+    preOperational.set_operational_state(
+        operational
+    );
+
+    operational.set_initialization_state(
+        initialization
+    );
+
+    operational.set_pre_operational_state(
+        preOperational
+    );
+
+    operational.set_stopped_state(
+        stopped
+    );
+
+    stopped.set_initialization_state(
+        initialization
+    );
+
+    stopped.set_pre_operational_state(
+        preOperational
+    );
+
+    stopped.set_operational_state(
+        operational
+    );
+
+    // --------------------------------------------------
+    // Create state machine
+    // --------------------------------------------------
+
+    static Context context(
+        initialization
+    );
+
+    stateMachine = &context;
+
+    // Fixed 250 Hz timing
+    controlTimer.init();
+
+    Serial.println();
+    Serial.println(
+        "Project 3 - Part 4"
+    );
+}
+
+// --------------------------------------------------
+// Main loop
+// --------------------------------------------------
+
+void loop()
+{
+    // --------------------------------------------------
+    // Emergency stop
+    // --------------------------------------------------
+
+    if (emergencyStop.is_lo())
+    {
+        stateMachine->fault();
+    }
+
+    // --------------------------------------------------
+    // Serial interface
+    // --------------------------------------------------
+
+    readSerialCommands();
+
+    // --------------------------------------------------
+    // Fixed 250 Hz state-machine activity
+    // --------------------------------------------------
+
+    if (controlTimer.take_tick())
+    {
+        stateMachine->do_work();
+    }
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/motor_driver.cpp

@@ -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;
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/operational.cpp

@@ -0,0 +1,143 @@
+#include <Arduino.h>
+
+#include <operational.h>
+#include <context.h>
+
+Operational::Operational(
+    Speed_controller& speedController,
+    Digital_out& statusLED
+)
+    : speedController(speedController),
+      statusLED(statusLED),
+      initializationState(nullptr),
+      preOperationalState(nullptr),
+      stoppedState(nullptr),
+      logCounter(0),
+      sampleCounter(0)
+{
+}
+
+const char* Operational::name() const
+{
+    return "Operational";
+}
+
+void Operational::on_entry()
+{
+    Serial.println();
+    Serial.println("Operational");
+
+    statusLED.set_hi();
+
+    logCounter = 0;
+    sampleCounter = 0;
+
+    speedController.start();
+
+    Serial.println(
+        "DATA,time_ms,reference_rpm,actual_rpm,control"
+    );
+}
+
+void Operational::on_do()
+{
+    speedController.update();
+
+    sampleCounter++;
+    logCounter++;
+
+    // 5 controller ticks × 4 ms = 20 ms
+    // => 50 Hz logging rate.
+    if (logCounter >= 5)
+    {
+        logCounter = 0;
+
+        const uint32_t time_ms =
+            sampleCounter * 4UL;
+
+        Serial.print("DATA,");
+        Serial.print(time_ms);
+        Serial.print(",");
+
+        Serial.print(
+            speedController.reference(),
+            2
+        );
+
+        Serial.print(",");
+
+        Serial.print(
+            speedController.actual_rpm(),
+            2
+        );
+
+        Serial.print(",");
+
+        Serial.println(
+            speedController.control_output(),
+            2
+        );
+    }
+}
+
+void Operational::on_exit()
+{
+    speedController.stop();
+
+    statusLED.set_lo();
+}
+
+void Operational::reset()
+{
+    if (initializationState != nullptr)
+    {
+        context_->transition_to(
+            *initializationState
+        );
+    }
+}
+
+void Operational::set_pre_operational()
+{
+    if (preOperationalState != nullptr)
+    {
+        context_->transition_to(
+            *preOperationalState
+        );
+    }
+}
+
+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_pre_operational_state(
+    State& state
+)
+{
+    preOperationalState = &state;
+}
+
+void Operational::set_stopped_state(
+    State& state
+)
+{
+    stoppedState = &state;
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/p_controller.cpp

@@ -0,0 +1,63 @@
+#include <p_controller.h>
+
+P_controller::P_controller(
+    double kp
+)
+    : kp(kp)
+{
+}
+
+double P_controller::update(
+    double reference,
+    double actual
+)
+{
+    const double error =
+        reference - actual;
+
+    const double output =
+        kp * error;
+
+    return limit(output);
+}
+
+void P_controller::reset()
+{
+}
+
+const char* P_controller::name() const
+{
+    return "P";
+}
+
+void P_controller::set_kp(
+    double value
+)
+{
+    if (value > 0.0)
+    {
+        kp = value;
+    }
+}
+
+double P_controller::get_kp() const
+{
+    return kp;
+}
+
+double P_controller::limit(
+    double value
+) const
+{
+    if (value > 255.0)
+    {
+        return 255.0;
+    }
+
+    if (value < -255.0)
+    {
+        return -255.0;
+    }
+
+    return value;
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/pi_controller.cpp

@@ -0,0 +1,141 @@
+#include <pi_controller.h>
+
+PI_controller::PI_controller(
+    double kp,
+    double ti,
+    double timestep
+)
+    : kp(kp),
+      ti(ti),
+      timestep(timestep),
+      integral(0.0)
+{
+}
+
+double PI_controller::update(
+    double reference,
+    double actual
+)
+{
+    const double error =
+        reference - actual;
+
+    // Candidate integral:
+    // E(k) = E(k-1) + e(k)T
+    const double candidateIntegral =
+        integral + error * timestep;
+
+    // Candidate PI output:
+    // u = Kp * (e + E/Ti)
+    const double candidateOutput =
+        kp *
+        (
+            error +
+            candidateIntegral / ti
+        );
+
+    // -----------------------------------------
+    // Conditional integration / anti-windup
+    // -----------------------------------------
+
+    // Integrate normally if the candidate
+    // controller output is not saturated.
+    if (
+        candidateOutput <= 255.0 &&
+        candidateOutput >= -255.0
+    )
+    {
+        integral =
+            candidateIntegral;
+    }
+
+    // If positively saturated, only integrate
+    // if the error is driving the output back down.
+    else if (
+        candidateOutput > 255.0 &&
+        error < 0.0
+    )
+    {
+        integral =
+            candidateIntegral;
+    }
+
+    // If negatively saturated, only integrate
+    // if the error is driving the output back up.
+    else if (
+        candidateOutput < -255.0 &&
+        error > 0.0
+    )
+    {
+        integral =
+            candidateIntegral;
+    }
+
+    const double output =
+        kp *
+        (
+            error +
+            integral / ti
+        );
+
+    return limit(output);
+}
+
+void PI_controller::reset()
+{
+    integral = 0.0;
+}
+
+const char* PI_controller::name() const
+{
+    return "PI";
+}
+
+void PI_controller::set_kp(
+    double value
+)
+{
+    kp = value;
+}
+
+double PI_controller::get_kp() const
+{
+    return kp;
+}
+
+void PI_controller::set_ti(
+    double value
+)
+{
+    if (value > 0.0)
+    {
+        ti = value;
+    }
+}
+
+double PI_controller::get_ti() const
+{
+    return ti;
+}
+
+double PI_controller::get_timestep() const
+{
+    return timestep;
+}
+
+double PI_controller::limit(
+    double value
+) const
+{
+    if (value > 255.0)
+    {
+        return 255.0;
+    }
+
+    if (value < -255.0)
+    {
+        return -255.0;
+    }
+
+    return value;
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/pre_operational.cpp

@@ -0,0 +1,225 @@
+#include <Arduino.h>
+
+#include <pre_operational.h>
+#include <context.h>
+
+PreOperational::PreOperational(
+    Speed_controller& speedController,
+    Digital_out& statusLED,
+    P_controller& pController,
+    PI_controller& piController
+)
+    : speedController(speedController),
+      statusLED(statusLED),
+      pController(pController),
+      piController(piController),
+      initializationState(nullptr),
+      operationalState(nullptr),
+      ledCounter(0),
+      piSelected(false)
+{
+}
+
+const char* PreOperational::name() const
+{
+    return "PreOperational";
+}
+
+void PreOperational::on_entry()
+{
+    // Motor must not run while configuring.
+    speedController.stop();
+
+    statusLED.set_lo();
+
+    ledCounter = 0;
+
+    Serial.println();
+    Serial.println("PreOperational");
+    Serial.println("Motor control disabled.");
+
+    print_configuration();
+
+    Serial.println();
+    Serial.println("Commands:");
+    Serial.println("p = select P controller");
+    Serial.println("i = select PI controller");
+    Serial.println("k<number> = set Kp");
+    Serial.println("t<number> = set Ti");
+    Serial.println("o = set operational");
+    Serial.println("r = reset");
+}
+
+void PreOperational::on_do()
+{
+    // State activity runs every 4 ms.
+    //
+    // 1 Hz LED:
+    // full period = 1000 ms
+    // toggle every 500 ms
+    //
+    // 500 ms / 4 ms = 125 ticks
+
+    ledCounter++;
+
+    if (ledCounter >= 125)
+    {
+        ledCounter = 0;
+
+        statusLED.toggle();
+    }
+}
+
+void PreOperational::on_exit()
+{
+    statusLED.set_lo();
+}
+
+void PreOperational::reset()
+{
+    if (initializationState != nullptr)
+    {
+        context_->transition_to(
+            *initializationState
+        );
+    }
+}
+
+void PreOperational::set_operational()
+{
+    if (operationalState != nullptr)
+    {
+        context_->transition_to(
+            *operationalState
+        );
+    }
+}
+
+void PreOperational::select_p()
+{
+    piSelected = false;
+
+    speedController.set_controller(
+        pController
+    );
+
+    Serial.println(
+        "Controller selected: P"
+    );
+
+    print_configuration();
+}
+
+void PreOperational::select_pi()
+{
+    piSelected = true;
+
+    speedController.set_controller(
+        piController
+    );
+
+    Serial.println(
+        "Controller selected: PI"
+    );
+
+    print_configuration();
+}
+
+void PreOperational::set_kp(
+    double value
+)
+{
+    if (value <= 0.0)
+    {
+        Serial.println(
+            "Invalid Kp."
+        );
+
+        return;
+    }
+
+    if (piSelected)
+    {
+        piController.set_kp(
+            value
+        );
+    }
+    else
+    {
+        pController.set_kp(
+            value
+        );
+    }
+
+    Serial.print("Kp set to: ");
+    Serial.println(value, 4);
+}
+
+void PreOperational::set_ti(
+    double value
+)
+{
+    if (value <= 0.0)
+    {
+        Serial.println(
+            "Invalid Ti."
+        );
+
+        return;
+    }
+
+    piController.set_ti(
+        value
+    );
+
+    Serial.print("Ti set to: ");
+    Serial.print(value, 4);
+    Serial.println(" s");
+}
+
+void PreOperational::set_initialization_state(
+    State& state
+)
+{
+    initializationState = &state;
+}
+
+void PreOperational::set_operational_state(
+    State& state
+)
+{
+    operationalState = &state;
+}
+
+void PreOperational::print_configuration()
+{
+    Serial.print("Controller: ");
+
+    if (piSelected)
+    {
+        Serial.println("PI");
+
+        Serial.print("Kp = ");
+        Serial.println(
+            piController.get_kp(),
+            4
+        );
+
+        Serial.print("Ti = ");
+        Serial.print(
+            piController.get_ti(),
+            4
+        );
+        Serial.println(" s");
+    }
+    else
+    {
+        Serial.println("P");
+
+        Serial.print("Kp = ");
+        Serial.println(
+            pController.get_kp(),
+            4
+        );
+    }
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/speed_controller.cpp

@@ -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;
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/speed_estimator.cpp

@@ -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;
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/src/stopped.cpp

@@ -0,0 +1,112 @@
+#include <Arduino.h>
+
+#include <stopped.h>
+#include <context.h>
+
+Stopped::Stopped(
+    Speed_controller& speedController,
+    Digital_out& statusLED
+)
+    : speedController(speedController),
+      statusLED(statusLED),
+      initializationState(nullptr),
+      preOperationalState(nullptr),
+      operationalState(nullptr),
+      ledCounter(0)
+{
+}
+
+const char* Stopped::name() const
+{
+    return "Stopped";
+}
+
+void Stopped::on_entry()
+{
+    Serial.println();
+    Serial.println("Stopped");
+    Serial.println(
+        "Emergency stop - motor brake active"
+    );
+
+    speedController.brake();
+
+    statusLED.set_hi();
+
+    ledCounter = 0;
+}
+
+void Stopped::on_do()
+{
+    // State activity runs every 4 ms.
+    //
+    // 2 Hz LED:
+    // toggle every ~250 ms
+    //
+    // 250 / 4 = 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_pre_operational()
+{
+    if (preOperationalState != nullptr)
+    {
+        context_->transition_to(
+            *preOperationalState
+        );
+    }
+}
+
+void Stopped::set_operational()
+{
+    if (operationalState != nullptr)
+    {
+        context_->transition_to(
+            *operationalState
+        );
+    }
+}
+
+void Stopped::set_initialization_state(
+    State& state
+)
+{
+    initializationState = &state;
+}
+
+void Stopped::set_pre_operational_state(
+    State& state
+)
+{
+    preOperationalState = &state;
+}
+
+void Stopped::set_operational_state(
+    State& state
+)
+{
+    operationalState = &state;
+}

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/test/README

@@ -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

Project_3_Dynamic_Controller/Part_4_PreOperational_Final/test/test_pi_controller/test_main.cpp

@@ -0,0 +1,438 @@
+#include <unity.h>
+
+#include <controller.h>
+#include <pi_controller.h>
+
+// --------------------------------------------------
+// Unity setup
+// --------------------------------------------------
+
+void setUp()
+{
+}
+
+void tearDown()
+{
+}
+
+// --------------------------------------------------
+// Normal-flow tests
+// --------------------------------------------------
+
+void test_zero_error_gives_zero_output()
+{
+    PI_controller controller(
+        10.0,
+        1.0,
+        0.1
+    );
+
+    double output =
+        controller.update(
+            50.0,
+            50.0
+        );
+
+    TEST_ASSERT_DOUBLE_WITHIN(
+        0.001,
+        0.0,
+        output
+    );
+}
+
+void test_positive_error_gives_positive_output()
+{
+    PI_controller controller(
+        10.0,
+        1.0,
+        0.1
+    );
+
+    // error = 2
+    // E = 2 * 0.1 = 0.2
+    // u = 10 * (2 + 0.2 / 1)
+    //   = 22
+
+    double output =
+        controller.update(
+            52.0,
+            50.0
+        );
+
+    TEST_ASSERT_DOUBLE_WITHIN(
+        0.001,
+        22.0,
+        output
+    );
+}
+
+void test_negative_error_gives_negative_output()
+{
+    PI_controller controller(
+        10.0,
+        1.0,
+        0.1
+    );
+
+    // error = -2
+    // E = -2 * 0.1 = -0.2
+    // u = 10 * (-2 - 0.2)
+    //   = -22
+
+    double output =
+        controller.update(
+            48.0,
+            50.0
+        );
+
+    TEST_ASSERT_DOUBLE_WITHIN(
+        0.001,
+        -22.0,
+        output
+    );
+}
+
+void test_integral_accumulates_over_time()
+{
+    PI_controller controller(
+        10.0,
+        1.0,
+        0.1
+    );
+
+    double first =
+        controller.update(
+            51.0,
+            50.0
+        );
+
+    double second =
+        controller.update(
+            51.0,
+            50.0
+        );
+
+    double third =
+        controller.update(
+            51.0,
+            50.0
+        );
+
+    // After first update:
+    // E = 0.1
+    // u = 10 * (1 + 0.1) = 11
+
+    TEST_ASSERT_DOUBLE_WITHIN(
+        0.001,
+        11.0,
+        first
+    );
+
+    // After second update:
+    // E = 0.2
+    // u = 12
+
+    TEST_ASSERT_DOUBLE_WITHIN(
+        0.001,
+        12.0,
+        second
+    );
+
+    // After third update:
+    // E = 0.3
+    // u = 13
+
+    TEST_ASSERT_DOUBLE_WITHIN(
+        0.001,
+        13.0,
+        third
+    );
+}
+
+// --------------------------------------------------
+// Saturation tests
+// --------------------------------------------------
+
+void test_positive_output_is_limited()
+{
+    PI_controller controller(
+        100.0,
+        1.0,
+        0.1
+    );
+
+    double output =
+        controller.update(
+            60.0,
+            50.0
+        );
+
+    TEST_ASSERT_DOUBLE_WITHIN(
+        0.001,
+        255.0,
+        output
+    );
+}
+
+void test_negative_output_is_limited()
+{
+    PI_controller controller(
+        100.0,
+        1.0,
+        0.1
+    );
+
+    double output =
+        controller.update(
+            40.0,
+            50.0
+        );
+
+    TEST_ASSERT_DOUBLE_WITHIN(
+        0.001,
+        -255.0,
+        output
+    );
+}
+
+// --------------------------------------------------
+// Anti-windup tests
+// --------------------------------------------------
+
+void test_positive_saturation_does_not_wind_up()
+{
+    PI_controller controller(
+        100.0,
+        1.0,
+        0.1
+    );
+
+    // Repeatedly demand an unreachable positive output.
+    for (int i = 0; i < 20; i++)
+    {
+        controller.update(
+            60.0,
+            50.0
+        );
+    }
+
+    // With anti-windup the large saturated error
+    // should not have accumulated.
+    //
+    // New error = +1
+    // E = 1 * 0.1 = 0.1
+    // u = 100 * (1 + 0.1) = 110
+
+    double output =
+        controller.update(
+            51.0,
+            50.0
+        );
+
+    TEST_ASSERT_DOUBLE_WITHIN(
+        0.001,
+        110.0,
+        output
+    );
+}
+
+void test_negative_saturation_does_not_wind_up()
+{
+    PI_controller controller(
+        100.0,
+        1.0,
+        0.1
+    );
+
+    // Repeatedly demand an unreachable negative output.
+    for (int i = 0; i < 20; i++)
+    {
+        controller.update(
+            40.0,
+            50.0
+        );
+    }
+
+    // New error = -1
+    // E = -0.1
+    // u = 100 * (-1 - 0.1) = -110
+
+    double output =
+        controller.update(
+            49.0,
+            50.0
+        );
+
+    TEST_ASSERT_DOUBLE_WITHIN(
+        0.001,
+        -110.0,
+        output
+    );
+}
+
+void test_controller_recovers_after_saturation()
+{
+    PI_controller controller(
+        100.0,
+        1.0,
+        0.1
+    );
+
+    // Cause positive saturation.
+    controller.update(
+        60.0,
+        50.0
+    );
+
+    // Return to a small error.
+    // Anti-windup should allow immediate recovery.
+    double output =
+        controller.update(
+            51.0,
+            50.0
+        );
+
+    TEST_ASSERT_DOUBLE_WITHIN(
+        0.001,
+        110.0,
+        output
+    );
+}
+
+// --------------------------------------------------
+// Reset test
+// --------------------------------------------------
+
+void test_reset_clears_integral()
+{
+    PI_controller controller(
+        10.0,
+        1.0,
+        0.1
+    );
+
+    // Build up integral contribution.
+    controller.update(
+        51.0,
+        50.0
+    );
+
+    controller.update(
+        51.0,
+        50.0
+    );
+
+    // Clear stored integral.
+    controller.reset();
+
+    // This should now behave exactly like
+    // the first +1 error sample again.
+    //
+    // E = 0.1
+    // u = 10 * (1 + 0.1) = 11
+
+    double output =
+        controller.update(
+            51.0,
+            50.0
+        );
+
+    TEST_ASSERT_DOUBLE_WITHIN(
+        0.001,
+        11.0,
+        output
+    );
+}
+
+// --------------------------------------------------
+// Polymorphism test
+// --------------------------------------------------
+
+void test_pi_controller_works_through_controller_interface()
+{
+    PI_controller pi(
+        10.0,
+        1.0,
+        0.1
+    );
+
+    // Access PI controller only through
+    // the common Controller interface.
+    Controller* controller =
+        &pi;
+
+    double output =
+        controller->update(
+            52.0,
+            50.0
+        );
+
+    TEST_ASSERT_DOUBLE_WITHIN(
+        0.001,
+        22.0,
+        output
+    );
+
+    TEST_ASSERT_EQUAL_STRING(
+        "PI",
+        controller->name()
+    );
+}
+
+// --------------------------------------------------
+// Test runner
+// --------------------------------------------------
+
+int main(int argc, char** argv)
+{
+    (void)argc;
+    (void)argv;
+
+    UNITY_BEGIN();
+
+    RUN_TEST(
+        test_zero_error_gives_zero_output
+    );
+
+    RUN_TEST(
+        test_positive_error_gives_positive_output
+    );
+
+    RUN_TEST(
+        test_negative_error_gives_negative_output
+    );
+
+    RUN_TEST(
+        test_integral_accumulates_over_time
+    );
+
+    RUN_TEST(
+        test_positive_output_is_limited
+    );
+
+    RUN_TEST(
+        test_negative_output_is_limited
+    );
+
+    RUN_TEST(
+        test_positive_saturation_does_not_wind_up
+    );
+
+    RUN_TEST(
+        test_negative_saturation_does_not_wind_up
+    );
+
+    RUN_TEST(
+        test_controller_recovers_after_saturation
+    );
+
+    RUN_TEST(
+        test_reset_clears_integral
+    );
+
+    RUN_TEST(
+        test_pi_controller_works_through_controller_interface
+    );
+
+    return UNITY_END();
+}