meira nýtt

src/motor_controller.cpp

@@ -0,0 +1,110 @@
+#include "motor_controller.h"
+#include "state.h"
+
+#include <Arduino.h>
+
+
+MotorController::MotorController(
+    RotaryEncoderDriver& encoder,
+    MotorDriver& motor,
+    P_controller& controller
+)
+    : current_state(nullptr),
+      initialization_state(nullptr),
+      operational_state(nullptr),
+      encoder(encoder),
+      motor(motor),
+      controller(controller),
+      reference_rpm(30.0)
+{
+}
+
+
+void MotorController::set_states(
+    State* initialization,
+    State* operational
+)
+{
+    initialization_state = initialization;
+    operational_state = operational;
+}
+
+
+void MotorController::begin()
+{
+    transition_to(initialization_state);
+}
+
+
+void MotorController::transition_to(State* new_state)
+{
+    if (current_state != nullptr)
+    {
+        current_state->exit();
+    }
+
+    current_state = new_state;
+
+    current_state->set_context(this);
+
+    current_state->enter();
+}
+
+
+void MotorController::update()
+{
+    if (current_state != nullptr)
+    {
+        current_state->update();
+    }
+}
+
+
+void MotorController::command(char c)
+{
+    if (current_state != nullptr)
+    {
+        current_state->command(c);
+    }
+}
+
+
+void MotorController::to_initialization()
+{
+    transition_to(initialization_state);
+}
+
+
+void MotorController::to_operational()
+{
+    transition_to(operational_state);
+}
+
+
+void MotorController::run_speed_controller()
+{
+    if (encoder.new_speed_available())
+    {
+        double actual = encoder.rpm();
+
+        double pwm_percent =
+            controller.update(reference_rpm, actual);
+
+        double pwm =
+            pwm_percent * 2.55;
+
+        motor.set(pwm);
+    }
+}
+
+
+void MotorController::stop_motor()
+{
+    motor.stop();
+}
+
+
+void MotorController::reset_speed_measurement()
+{
+    encoder.reset_speed_measurement();
+}