Bætti við test-um, pi controller með polymorphism og bætti við virkni í operational mode

src/main.cpp

@@ -3,7 +3,7 @@
 
 #include "rotary_encoder_driver.h"
 #include "motor_driver.h"
-#include "p_controller.h"
+#include "pi_controller.h"
 
 #include "motor_controller.h"
 
@@ -16,9 +16,12 @@ RotaryEncoderDriver encoder;
 
 MotorDriver motor;
 
-const double KP = 12.5;
+const double Kp = 12.5;
+const double Ti = 1.0;
+const double T = 0.004;
 
-P_controller controller(KP);
+// P_controller controller(Kp);
+PI_controller controller(Kp, Ti, T);
 
 
 InitializationState initialization_state;

src/motor_controller.cpp

@@ -120,6 +120,11 @@ void MotorController::reset_speed_measurement()
     encoder.reset_speed_measurement();
 }
 
+void MotorController::reset_controller()
+{
+    controller.reset();
+}
+
 void MotorController::to_stopped()
 {
     transition_to(stopped_state);

src/operational_state.cpp

@@ -8,6 +8,8 @@ void OperationalState::enter()
 {
     Serial.println("State: Operational");
 
+    context->reset_controller();
+
     context->led_on();
 }
 

src/pi_controller.cpp

@@ -0,0 +1,31 @@
+#include "pi_controller.h"
+
+PI_controller::PI_controller(double Kp, double Ti, double T) : P_controller(Kp), Ti(Ti), T(T), integral(0.0)
+{
+}
+
+double PI_controller::update(double ref, double actual)
+{
+    double e = ref - actual;
+    double new_integral = integral + e * T;
+    double u = Kp * (e + new_integral / Ti);
+
+    if (u > U_MAX)
+    {
+        u = U_MAX;
+    }
+    else if (u < U_MIN)
+    {
+        u = U_MIN;
+    }
+    else
+    {
+        integral = new_integral;
+    }
+    return u;
+}
+
+void PI_controller::reset()
+{
+    integral = 0.0;
+}