Leggja lokahönd á verkefnið og plot-a fleiri myndir

.gitignore

@@ -4,12 +4,10 @@ __pycache__
 *.csv
 *.typ
 *.pdf
-*.py
-*.png
-*.csv
 scripts
+*.bak
 
-# Skrár sem PlatformIO býr til sjálft
+# Vélarsértækar skrár sem PlatformIO býr til sjálft
 .vscode/c_cpp_properties.json
 .vscode/launch.json
 compile_commands.json

docs/diagrams/src/motor_state.wsd

@@ -16,6 +16,13 @@ state Initialization
     Initialization : Do / Wait 2 seconds
     Initialization : Exit / Send Boot-up message
 
+state PreOperational
+    PreOperational : Entry / Stop motor
+    PreOperational : Do / Blink LED at 1 Hz
+    PreOperational : Do / Accept Kp and Ti configuration
+    PreOperational : Do / Select P or PI controller [c]
+    PreOperational : Exit / LED off
+
 state Operational 
     Operational : Entry / LED on
     Operational : Do / Run speed controller
@@ -26,11 +33,18 @@ state Stopped
     Stopped : Do / Blink LED at 2 Hz
     Stopped : Exit / LED off
 
-Initialization --> Operational : initialization complete
+Initialization -r-> PreOperational : initialization\n complete
-Operational --> Initialization : reset
+' Initialization -d-> Operational : initialization\n complete
+
+PreOperational -d-> Operational : set operational [o]
+PreOperational -l--> Initialization : reset [r]
+
+Operational -u-> PreOperational : set pre-operational [p]
+Operational -> Initialization : reset [r]
 Operational --> Stopped : fault detected
 
-Stopped --> Initialization : reset
+Stopped -l-> Initialization : reset [r]
-Stopped --> Operational : set operational
+Stopped -> Operational : set operational [o]
+Stopped -u-> PreOperational : set pre-operational [p]
 
 @enduml

include/motor_controller.h

@@ -9,36 +9,32 @@
 class State;
 
 
-// Gildi úr einni keyrslu á hraðastýringunni, til útprentunar
-struct ControlSample
-{
-    double ref_rpm;
-    double actual_rpm;
-    double pwm_actual;
-};
-
-
 class MotorController
 {
 private:
-    static constexpr double DEFAULT_REFERENCE_RPM = 25.0;
+    static constexpr double DEFAULT_REFERENCE_RPM = 40.0;
     static constexpr double PWM_PER_PERCENT = 255.0 / 100.0;
     static const uint8_t LED_PIN = 5;   // PB5 = D13 = LED_BUILTIN
 
     State* current_state;
 
     State& initialization_state;
+    State& pre_operational_state;
     State& operational_state;
     State& stopped_state;
 
     RotaryEncoderDriver& encoder;
     MotorDriver& motor;
-    P_controller& controller;
+    P_controller* controller;
 
     Digital_out led;
 
     double reference_rpm;
 
+    // Síðasta sýni, notað fyrir CSV-útprentun
+    double last_actual_rpm;
+    double last_pwm;
+
 
 public:
     MotorController(
@@ -46,6 +42,7 @@ public:
         MotorDriver& motor,
         P_controller& controller,
         State& initialization,
+        State& pre_operational,
         State& operational,
         State& stopped
     );
@@ -62,6 +59,8 @@ public:
 
     void to_initialization();
 
+    void to_pre_operational();
+
     void to_operational();
 
     void to_stopped();
@@ -69,8 +68,12 @@ public:
     bool fault_active();
 
 
-    // Skilar true ef ný mæling var tekin og sample fyllt út
+    // Skilar true þegar nýtt sýni var keyrt í gegnum stýringuna
-    bool run_speed_controller(ControlSample& sample);
+    bool run_speed_controller();
+
+    double get_reference_rpm() const { return reference_rpm; }
+    double get_actual_rpm() const { return last_actual_rpm; }
+    double get_pwm() const { return last_pwm; }
 
     void stop_motor();
 
@@ -78,6 +81,12 @@ public:
 
     void reset_controller();
 
+    void set_Kp(double value);
+
+    void set_Ti(double value);
+
+    void set_controller(P_controller& new_controller);
+
     void led_on();
 
     void led_off();

include/operational_state.h

@@ -2,16 +2,18 @@
 #define OPERATIONAL_STATE_H
 
 #include "state.h"
-
 #include <stdint.h>
 
+// Sett á 1 til að prenta CSV (time_ms,ref_rpm,actual_rpm,pwm) í Operational,
+// t.d. fyrir step og load response. Einnig hægt með build_flags = -D LOG_CSV=1
+#ifndef LOG_CSV
+#define LOG_CSV 0
+#endif
+
 
 class OperationalState : public State
 {
 private:
-    static constexpr double SAMPLE_PERIOD_MS = 4.0;
-
-    // uint32_t svo teljarinn flæði ekki yfir eftir ~4 mín (eins og uint16_t)
     uint32_t sample_count = 0;
 
 public:

include/p_controller.h

@@ -11,6 +11,16 @@ public:
     virtual ~P_controller() {}
     virtual double update(double ref, double actual);
     virtual void reset() {}
+
+    virtual void set_Kp(double value)
+    {
+        Kp = value;
+    }
+
+    virtual void set_Ti(double value)
+    {
+        // P controller does not use Ti
+    }
 };
 
 #endif

include/pi_controller.h

@@ -16,6 +16,8 @@ class PI_controller : public P_controller
         PI_controller(double Kp, double Ti, double T);
         double update(double ref, double actual) override;
         void reset() override;
+
+        void set_Ti(double value) override;
 };
 
 #endif

include/pre_operational_state.h

@@ -0,0 +1,43 @@
+#ifndef PRE_OPERATIONAL_STATE_H
+#define PRE_OPERATIONAL_STATE_H
+
+#include "state.h"
+#include "stdint.h"
+
+class P_controller;
+
+class PreOperationalState : public State
+{
+private:
+    P_controller& p_controller;
+    P_controller& pi_controller;
+    bool pi_selected;
+
+    unsigned long previous_time;
+    bool led_state;
+
+    //bool waiting_for_kp;
+    //bool waiting_for_ti;
+
+    enum InputMode
+    {
+        NONE,
+        ENTER_KP,
+        ENTER_TI
+    };
+
+    InputMode input_mode;
+
+    char input_buffer[16];
+    uint8_t input_index;
+
+public:
+    PreOperationalState(P_controller& p_controller, P_controller& pi_controller);
+
+    void enter() override;
+    void update() override;
+    void command(char c) override;
+    void exit() override;
+};
+
+#endif

platformio.ini

@@ -17,9 +17,5 @@ monitor_speed = 115200
 test_framework = unity
 test_build_src = yes
 
-
+; CSV-útprentun í Operational fyrir step/load response (sjá capture_serial.py)
-; [env:native]
+; build_flags = -D LOG_CSV=1
-; platform = native
-; test_framework = unity
-; test_build_src = yes
-; build_src_filter = -<*> +<p_controller.cpp> +<pi_controller.cpp>

scripts/plot_step_response.py

@@ -1,113 +1,46 @@
 #!/usr/bin/env python3
 """
 Teiknar step response-plott ur CSV-gognum sem Arduino-verkefnid prentar ut
-i Operational state (dalkar: time_ms, ref_rpm, actual_rpm, pwm_actual).
+(dalkar: time_ms, ref_rpm, actual_rpm, pwm_requested, pwm_actual).
 
-pwm_actual = stýrimerkid klippt vid +-255, þ.e. þad sem raunverulega
+pwm_requested = Kp * error * 2.55, oklippt (getur farid langt yfir +-255)
+pwm_actual    = sama gildi en klippt vid +-255, þ.e. þad sem raunverulega
                 var sent a motor.set() / vélbúnaðinn
 
-Hausinn er prentadur i hvert skipti sem farid er inn i Operational, svo ein
-skra getur innihaldid nokkrar keyrslur. Adeins sidasta keyrslan er teiknud.
-
-Skriptan metur lika sveiflutima (Tu) og utslag ef kerfid sveiflast, sem
-nytist til ad finna Ku/Tu fyrir Ziegler-Nichols.
-
 Notkun:
-    python plot_step_response.py [inntak.csv] [uttak.png] [titill] [--no-pwm]
+    python plot_step_response.py [inntak.csv] [uttak.png] [titill]
-
---no-pwm teiknar bara RPM (PWM-dalkurinn ma lika vanta i CSV-skranni)
 
 Sjalfgefid: data/step_response.csv -> data/step_response.png
 """
 import sys
 import csv
-import statistics
 import matplotlib.pyplot as plt
 
 
-COLUMNS = ["time_ms", "ref_rpm", "actual_rpm", "pwm_actual"]
-REQUIRED = ["time_ms", "ref_rpm", "actual_rpm"]
-
-# Onnur nofn sem eldri utgafur firmware-sins hafa notad fyrir sama dalk
-ALIASES = {"pwm": "pwm_actual"}
-
-
-def empty_cols():
-    return {key: [] for key in COLUMNS}
-
-
 def load_csv(path):
-    cols = empty_cols()
+    cols = {"time_ms": [], "ref_rpm": [], "actual_rpm": [],
-    header = None
+            "pwm_requested": [], "pwm_actual": []}
     with open(path, newline="") as f:
-        for row in csv.reader(f):
+        reader = csv.DictReader(f)
-            if row and row[0].strip() == "time_ms":
+        for row in reader:
-                header = [name.strip() for name in row]
-                header = [ALIASES.get(name, name) for name in header]
-                missing = [key for key in REQUIRED if key not in header]
-                if missing:
-                    sys.exit(f"Dalka vantar i haus {row}: {missing}")
-                continue
-            if header is None:
-                # rusl a undan fyrsta haus (t.d. "State: Initialization")
-                continue
             try:
-                values = {key: float(row[header.index(key)])
+                for key in cols:
-                          for key in COLUMNS if key in header}
+                    cols[key].append(float(row[key]))
-            except (ValueError, IndexError):
+            except (ValueError, KeyError):
-                # sleppir linum sem eru ekki gild data (t.d. "State: ...")
+                # sleppir linum sem eru ekki gild data (t.d. rusl fyrir/eftir)
+                for key in cols:
+                    if len(cols[key]) > len(cols["time_ms"]):
+                        cols[key].pop()
                 continue
-
-            # Timinn byrjar aftur a 0 -> ny keyrsla, hendum theirri gomlu
-            if cols["time_ms"] and values["time_ms"] <= cols["time_ms"][-1]:
-                cols = empty_cols()
-
-            for key in values:
-                cols[key].append(values[key])
     return cols
 
 
-def estimate_oscillation(time_s, actual_rpm):
+def plot(cols, out_path, title_suffix=""):
-    """
-    Metur sveiflutima ut fra uppleid i gegnum medaltal seinni helmings
-    gagnanna (fyrri helmingur er transient). Hysteresis kemur i veg fyrir
-    ad sudid i rpm-maelingunni gefi falskar skurdpunkta.
-    Skilar (Tu [s], utslag [rpm]) eda None ef engin skyr sveifla.
-    """
-    half = len(actual_rpm) // 2
-    t = time_s[half:]
-    y = actual_rpm[half:]
-    if len(y) < 20:
-        return None
-
-    mean = statistics.fmean(y)
-    band = 0.25 * statistics.pstdev(y)
-    if band == 0:
-        return None
-
-    crossings = []
-    below = y[0] < mean - band
-    for ti, yi in zip(t, y):
-        if below and yi > mean + band:
-            crossings.append(ti)
-            below = False
-        elif not below and yi < mean - band:
-            below = True
-
-    if len(crossings) < 3:
-        return None
-
-    periods = [b - a for a, b in zip(crossings, crossings[1:])]
-    amplitude = (max(y) - min(y)) / 2.0
-    return statistics.fmean(periods), amplitude
-
-
-def plot(cols, out_path, title_suffix="", show_pwm=True):
     time_s = [t / 1000.0 for t in cols["time_ms"]]
     ref_rpm = cols["ref_rpm"]
     actual_rpm = cols["actual_rpm"]
+    pwm_requested = cols["pwm_requested"]
     pwm_actual = cols["pwm_actual"]
-    show_pwm = show_pwm and len(pwm_actual) == len(time_s)
 
     fig, ax1 = plt.subplots(figsize=(10, 5))
 
@@ -119,17 +52,14 @@ def plot(cols, out_path, title_suffix="", show_pwm=True):
     ax1.set_ylabel("Angular velocity [RPM]")
     ax1.grid(True, alpha=0.3)
 
-    lines, labels = ax1.get_legend_handles_labels()
-    if show_pwm:
     ax2 = ax1.twinx()
+    ax2.plot(time_s, pwm_requested, label="PWM requested (unclamped)",
+              color="#d62728", linewidth=1.0, alpha=0.5, linestyle=":")
     ax2.plot(time_s, pwm_actual, label="PWM actual (clamped)",
               color="#d62728", linewidth=1.4)
     ax2.axhline(255, color="#d62728", linewidth=0.6, alpha=0.3)
     ax2.axhline(-255, color="#d62728", linewidth=0.6, alpha=0.3)
     ax2.set_ylabel("PWM [-255, 255]")
-        lines2, labels2 = ax2.get_legend_handles_labels()
-        lines += lines2
-        labels += labels2
 
     # Merkjum stokkpunktinn (fyrsta stadin thar sem ref hoppar fra 0)
     step_idx = next((i for i, r in enumerate(ref_rpm) if r > 0), None)
@@ -139,23 +69,12 @@ def plot(cols, out_path, title_suffix="", show_pwm=True):
         ax1.text(time_s[step_idx], max(actual_rpm) * 1.02, " step",
                   fontsize=9, va="bottom")
 
-    osc = estimate_oscillation(time_s, actual_rpm)
+    lines1, labels1 = ax1.get_legend_handles_labels()
-    # if osc is not None:
+    lines2, labels2 = ax2.get_legend_handles_labels()
-    #     tu, amplitude = osc
+    ax1.legend(lines1 + lines2, labels1 + labels2, loc="lower right",
-    #     print(f"Sveifla: Tu = {tu * 1000:.1f} ms, utslag = +-{amplitude:.2f} RPM")
+               fontsize=8)
-    #     ax1.text(0.99, 0.98, f"Tu ≈ {tu * 1000:.1f} ms\n"
-    #                           f"amplitude ≈ ±{amplitude:.2f} RPM",
-    #               transform=ax1.transAxes, fontsize=9, va="top", ha="right",
-    #               bbox=dict(facecolor="white", alpha=0.8, edgecolor="#cccccc"))
-    # else:
-    #     print("Engin skyr sveifla fannst i seinni helmingi gagnanna")
 
-    ax1.legend(lines, labels, loc="lower right", fontsize=8)
+    title = "Double step response: RPM and PWM over time"
-
-    if show_pwm:
-        title = "Step response: RPM and PWM over time"
-    else:
-        title = "Step response: RPM over time"
     if title_suffix:
         title += f" ({title_suffix})"
     fig.suptitle(title)
@@ -165,12 +84,8 @@ def plot(cols, out_path, title_suffix="", show_pwm=True):
 
 
 if __name__ == "__main__":
-    show_pwm = "--no-pwm" not in sys.argv
+    in_path = sys.argv[1] if len(sys.argv) > 1 else "data/twostep.csv"
-    args = [a for a in sys.argv[1:] if a != "--no-pwm"]
+    out_path = sys.argv[2] if len(sys.argv) > 2 else "data/twostep.png"
-    in_path = args[0] if len(args) > 0 else "data/step_response.csv"
+    title_suffix = sys.argv[3] if len(sys.argv) > 3 else ""
-    out_path = args[1] if len(args) > 1 else "data/step_response.png"
-    title_suffix = args[2] if len(args) > 2 else ""
     data = load_csv(in_path)
-    if not data["time_ms"]:
+    plot(data, out_path, title_suffix)
-        sys.exit(f"Engin gogn fundust i {in_path}")
-    plot(data, out_path, title_suffix, show_pwm)

src/initialization_state.cpp

@@ -24,7 +24,8 @@ void InitializationState::update()
     // Non-blocking bið svo aðallykkjan haldi áfram að keyra
     if (millis() - enter_time_ms >= WAIT_MS)
     {
-        context->to_operational();
+        context->to_pre_operational();
+        // context->to_operational();
     }
 }
 

src/main.cpp

@@ -11,6 +11,9 @@
 #include "motor_controller.h"
 
 #include "initialization_state.h"
+
+#include "pre_operational_state.h"
+
 #include "operational_state.h"
 #include "stopped_state.h"
 
@@ -19,26 +22,29 @@ RotaryEncoderDriver encoder;
 
 MotorDriver motor;
 
-const double Kp = 10.125; // Ku * 0.45
+const double Kp_P = 10.0;     // P-controller from Project 2
-const double Ti = 0.0334; //  Tu * 0.83 37
-const double T = 0.004; // 0.004
 
-// P_controller controller(Kp);
+const double Kp = 10.125;     // Ziegler-Nichols PI
-PI_controller controller(Kp, Ti, T);
+const double Ti = 0.0334;
+const double T = 0.004;
+
+P_controller p_controller(Kp_P);
+PI_controller pi_controller(Kp, Ti, T);
 
 
 InitializationState initialization_state;
-
+// Pre-operational velur á milli stýringanna með 'c'
+PreOperationalState pre_operational_state(p_controller, pi_controller);
 OperationalState operational_state;
-
 StoppedState stopped_state;
 
 // Stöðurnar verða að vera skilgreindar á undan motor_controller
 MotorController motor_controller(
     encoder,
     motor,
-    controller,
+    pi_controller,      // Sjálfgefin stýring
     initialization_state,
+    pre_operational_state,
     operational_state,
     stopped_state
 );
@@ -47,38 +53,24 @@ MotorController motor_controller(
 int main()
 {
     init();
-
     Serial.begin(115200);
-
-
     cli();
 
     encoder.begin();
-
     motor.begin();
 
     sei();
 
-    // motor_controller.set_states(
-    //     &initialization_state,
-    //     &operational_state,
-    //     &stopped_state
-    // );
-
-
     motor_controller.begin();
 
-
     while (true)
     {
         if (Serial.available() > 0)
         {
             char c = Serial.read();
 
-            if (c != '\n' && c != '\r')
-            {
             motor_controller.command(c);   
-            }
+
         }
 
 
@@ -89,7 +81,4 @@ int main()
     return 0;
 }
 
-// int main(){
+#endif 
-//     return 0;
-// }
-#endif // PIO_UNIT_TESTING

src/motor_controller.cpp

@@ -9,20 +9,25 @@ MotorController::MotorController(
     MotorDriver& motor,
     P_controller& controller,
     State& initialization,
+    State& pre_operational,
     State& operational,
     State& stopped
 )
     : current_state(nullptr),
       initialization_state(initialization),
+      pre_operational_state(pre_operational),
       operational_state(operational),
       stopped_state(stopped),
       encoder(encoder),
       motor(motor),
-      controller(controller),
+      controller(&controller),
       led(LED_PIN),
-      reference_rpm(DEFAULT_REFERENCE_RPM)
+      reference_rpm(DEFAULT_REFERENCE_RPM),
+      last_actual_rpm(0.0),
+      last_pwm(0.0)
 {
     initialization_state.set_context(this);
+    pre_operational_state.set_context(this);
     operational_state.set_context(this);
     stopped_state.set_context(this);
 }
@@ -85,6 +90,11 @@ void MotorController::to_initialization()
     transition_to(initialization_state);
 }
 
+void MotorController::to_pre_operational()
+{
+    transition_to(pre_operational_state);
+}
+
 
 void MotorController::to_operational()
 {
@@ -92,7 +102,7 @@ void MotorController::to_operational()
 }
 
 
-bool MotorController::run_speed_controller(ControlSample& sample)
+bool MotorController::run_speed_controller()
 {
     if (!encoder.new_speed_available())
     {
@@ -102,27 +112,25 @@ bool MotorController::run_speed_controller(ControlSample& sample)
     double actual = encoder.rpm();
 
     double pwm_percent =
-        controller.update(reference_rpm, actual);
+        controller->update(reference_rpm, actual);
 
     double pwm =
         pwm_percent * PWM_PER_PERCENT;
 
-    // Klippt hér svo pwm_actual sé nákvæmlega það sem fer á mótorinn
+    // Klippt hér svo last_pwm sé nákvæmlega það sem fer á mótorinn
     if (pwm > 255.0)
     {
         pwm = 255.0;
     }
-
+    else if (pwm < -255.0)
-    if (pwm < -255.0)
     {
         pwm = -255.0;
     }
 
     motor.set(pwm);
 
-    sample.ref_rpm = reference_rpm;
+    last_actual_rpm = actual;
-    sample.actual_rpm = actual;
+    last_pwm = pwm;
-    sample.pwm_actual = pwm;
 
     return true;
 }
@@ -141,7 +149,24 @@ void MotorController::reset_speed_measurement()
 
 void MotorController::reset_controller()
 {
-    controller.reset();
+    controller->reset();
+}
+
+void MotorController::set_Kp(double value)
+{
+    controller->set_Kp(value);
+}
+
+void MotorController::set_Ti(double value)
+{
+    controller->set_Ti(value);
+}
+
+void MotorController::set_controller(P_controller& new_controller)
+{
+    controller = &new_controller;
+
+    controller->reset();
 }
 
 void MotorController::to_stopped()

src/operational_state.cpp

@@ -12,10 +12,11 @@ void OperationalState::enter()
 
     context->led_on();
 
-    // Tíminn byrjar á 0 í hvert skipti sem farið er inn í Operational
     sample_count = 0;
 
-    Serial.println("time_ms,ref_rpm,actual_rpm");
+#if LOG_CSV
+    Serial.println("time_ms,ref_rpm,actual_rpm,pwm");
+#endif
 }
 
 // Part 1 
@@ -38,19 +39,20 @@ void OperationalState::update()
         return;
     }
 
-    ControlSample sample;
+    if (context->run_speed_controller())
-
-    if (context->run_speed_controller(sample))
     {
         sample_count++;
 
-        Serial.print(sample_count * SAMPLE_PERIOD_MS, 1);
+#if LOG_CSV
-        Serial.print(",");
+        // 4 ms á milli sýna, tíminn byrjar á 0 við hverja innkomu í Operational
-        Serial.print(sample.ref_rpm, 1);
+        Serial.print(sample_count * 4UL);
-        Serial.print(",");
+        Serial.print(',');
-        Serial.println(sample.actual_rpm, 2);
+        Serial.print(context->get_reference_rpm(), 1);
-        // Serial.print(",");
+        Serial.print(',');
-        // Serial.println(sample.pwm_actual, 1);
+        Serial.print(context->get_actual_rpm(), 2);
+        Serial.print(',');
+        Serial.println(context->get_pwm(), 1);
+#endif
     }
 }
 
@@ -63,6 +65,13 @@ void OperationalState::command(char c)
 
         context->to_initialization();
     }
+
+    else if (c == 'p' || c == 'P')
+    {
+        Serial.println("Command: set pre-operational");
+
+        context->to_pre_operational();
+    }
 }
 
 

src/pi_controller.cpp

@@ -30,3 +30,8 @@ void PI_controller::reset()
 {
     integral = 0.0;
 }
+
+void PI_controller::set_Ti(double value)
+{
+    Ti = value;
+}

src/pre_operational_state.cpp

@@ -0,0 +1,161 @@
+#include "pre_operational_state.h"
+#include "motor_controller.h"
+
+#include <Arduino.h>
+#include <stdlib.h>
+
+PreOperationalState::PreOperationalState(P_controller& p_controller, P_controller& pi_controller)
+    : p_controller(p_controller),
+      pi_controller(pi_controller),
+      pi_selected(true),
+      previous_time(0),
+      led_state(false),
+      //waiting_for_kp(false),
+      //waiting_for_ti(false),
+      input_mode(NONE),
+      input_index(0)
+{
+}
+
+void PreOperationalState::enter()
+{
+    Serial.println("State: Pre-operational");
+
+    context->stop_motor();
+
+    context->led_off();
+
+    led_state = false;
+
+    previous_time = millis();
+}
+
+void PreOperationalState::update()
+{
+    unsigned long current_time = millis();
+
+    if (current_time - previous_time >= 500)
+    {
+        previous_time = current_time;
+
+        led_state = !led_state;
+
+        if (led_state)
+        {
+            context->led_on();
+        }
+        else
+        {
+            context->led_off();
+        }
+    }
+}
+
+void PreOperationalState::command(char c)
+{
+    // We are currently entering a Kp or Ti value
+    if (input_mode != NONE)
+    {
+        // Enter pressed
+        if (c == '\n' || c == '\r')
+        {
+            if (input_index > 0)
+            {
+                // End the string
+                input_buffer[input_index] = '\0';
+
+                // Convert text to a number
+                double value = atof(input_buffer);
+
+                if (input_mode == ENTER_KP)
+                {
+                    context->set_Kp(value);
+
+                    Serial.print("Kp set to: ");
+                    Serial.println(value);
+                }
+                else if (input_mode == ENTER_TI)
+                {
+                    context->set_Ti(value);
+
+                    Serial.print("Ti set to: ");
+                    Serial.println(value);
+                }
+            }
+
+            // Finished entering a value
+            input_index = 0;
+            input_mode = NONE;
+
+            return;
+        }
+
+        // Store number characters
+        if ((c >= '0' && c <= '9') ||
+            c == '.' ||
+            c == '-')
+        {
+            if (input_index < sizeof(input_buffer) - 1)
+            {
+                input_buffer[input_index] = c;
+                input_index++;
+            }
+        }
+
+        return;
+    }
+
+
+    // Normal state commands
+    if (c == 'o' || c == 'O')
+    {
+        Serial.println("Command: set operational");
+
+        context->to_operational();
+    }
+
+    else if (c == 'r' || c == 'R')
+    {
+        Serial.println("Command: reset");
+
+        context->to_initialization();
+    }
+
+    else if (c == 'k' || c == 'K')
+    {
+        input_mode = ENTER_KP;
+        input_index = 0;
+
+        Serial.println("Enter Kp and press Enter:");
+    }
+
+    else if (c == 't' || c == 'T')
+    {
+        input_mode = ENTER_TI;
+        input_index = 0;
+
+        Serial.println("Enter Ti and press Enter:");
+    }
+
+    // Select controller algorithm, the controller is passed to the motor controller
+    else if (c == 'c' || c == 'C')
+    {
+        pi_selected = !pi_selected;
+
+        if (pi_selected)
+        {
+            context->set_controller(pi_controller);
+            Serial.println("Controller: PI");
+        }
+        else
+        {
+            context->set_controller(p_controller);
+            Serial.println("Controller: P");
+        }
+    }
+}
+
+void PreOperationalState::exit()
+{
+    context->led_off();
+}

src/stopped_state.cpp

@@ -19,7 +19,7 @@ void StoppedState::enter()
 
     led_state = false;
 
-    digitalWrite(LED_BUILTIN, LOW);
+    context->led_off();
 
     previous_time = millis();
 }
@@ -36,10 +36,14 @@ void StoppedState::update()
 
         led_state = !led_state;
 
-        digitalWrite(
+        if (led_state)
-            LED_BUILTIN,
+        {
-            led_state ? HIGH : LOW
+            context->led_on();
-        );
+        }
+        else
+        {
+            context->led_off();
+        }
     }
 }
 
@@ -61,10 +65,17 @@ void StoppedState::command(char c)
 
         context->to_operational();
     }
+
+    else if (c == 'p' || c == 'P')
+    {
+        Serial.println("Command: set pre-operational");
+
+        context->to_pre_operational();
+    }
 }
 
 
 void StoppedState::exit()
 {
-    digitalWrite(LED_BUILTIN, LOW);
+    context->led_off();
 }