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

include/motor_controller.h

@@ -66,6 +66,8 @@ public:
 
     void reset_speed_measurement();
 
+    void reset_controller();
+
     void led_on();
 
     void led_off();

include/p_controller.h

@@ -3,13 +3,14 @@
 
 class P_controller
 {
-private:
+protected:
     double Kp;
 
 public:
     P_controller(double Kp);
-
-    double update(double ref, double actual);
+    virtual ~P_controller() {}
+    virtual double update(double ref, double actual);
+    virtual void reset() {}
 };
 
-#endif
+#endif

include/pi_controller.h

@@ -0,0 +1,21 @@
+#ifndef PI_CONTROLLER_H
+#define PI_CONTROLLER_H
+
+#include "p_controller.h"
+
+class PI_controller : public P_controller
+{
+    private:
+        static constexpr double U_MAX = 100.0;  // %
+        static constexpr double U_MIN = -100.0; // %
+
+        double Ti;
+        double T;
+        double integral;
+    public:
+        PI_controller(double Kp, double Ti, double T);
+        double update(double ref, double actual) override;
+        void reset() override;
+};
+
+#endif