Byrjaði á kóðum fyrir part 3 en lenti í veseni með test kóðann

include/controller.h

@@ -0,0 +1,18 @@
+#ifndef CONTROLLER_H
+#define CONTROLLER_H
+
+class Controller
+{
+public:
+    virtual ~Controller()
+    {
+    }
+
+    virtual double update(double ref, double actual) = 0;
+
+    virtual void reset()
+    {
+    }
+};
+
+#endif

include/initialization_state.h

@@ -7,7 +7,7 @@
 class InitializationState : public State
 {
 private:
-    static const unsigned long WAIT_MS = 2000;
+    static const unsigned long WAIT_MS = 2000UL;
 
     unsigned long enter_time_ms = 0;
 

include/motor_controller.h

@@ -3,7 +3,9 @@
 
 #include "rotary_encoder_driver.h"
 #include "motor_driver.h"
-#include "p_controller.h"
+//#include "p_controller.h"
+//#include "pi_controller.h"
+#include "controller.h"
 #include "digital_out.h"
 
 class State;
@@ -18,13 +20,15 @@ private:
 
     State* current_state;
 
-    State* initialization_state;
+    State& initialization_state;
-    State* operational_state;
+    State& operational_state;
-    State* stopped_state;
+    State& stopped_state;
 
     RotaryEncoderDriver& encoder;
     MotorDriver& motor;
-    P_controller& controller;
+    //P_controller& controller;
+    //PI_controller& controller;
+    Controller& controller;
 
     Digital_out led;
 
@@ -35,15 +39,19 @@ public:
     MotorController(
         RotaryEncoderDriver& encoder,
         MotorDriver& motor,
-        P_controller& controller
+        //P_controller& controller,
+        Controller& controller,
+        State& initialization,
+        State& operational,
+        State& stopped
     );
 
 
-    void set_states(
+    // void set_states(
-        State* initialization,
+    //     State* initialization,
-        State* operational,
+    //     State* operational,
-        State* stopped
+    //     State* stopped
-    );
+    // );
 
 
     void begin();
@@ -70,9 +78,17 @@ public:
 
     void reset_speed_measurement();
 
+    void reset_controller();
+
+
+
     void led_on();
 
     void led_off();
+
+    
 };
 
+
+
 #endif

include/p_controller.h

@@ -1,7 +1,9 @@
 #ifndef P_CONTROLLER_H
 #define P_CONTROLLER_H
 
-class P_controller
+#include "controller.h"
+
+class P_controller : public Controller
 {
 private:
     double Kp;
@@ -9,7 +11,10 @@ private:
 public:
     P_controller(double Kp);
 
-    double update(double ref, double actual);
+    double update(double ref, double actual) override;
+
+
+    
 };
 
 #endif

include/pi_controller.h

@@ -0,0 +1,27 @@
+#ifndef PI_CONTROLLER_H
+#define PI_CONTROLLER_H
+
+#include "controller.h"
+
+class PI_controller : public Controller
+{
+private:
+    double Kp;
+    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

platformio.ini

@@ -14,4 +14,5 @@ board = nanoatmega328new
 framework = arduino
 
 monitor_speed = 115200
-
+test_framework = unity
+test_build_src = yes

src/initialization_state.cpp

@@ -15,7 +15,10 @@ void InitializationState::enter()
 
     context->reset_speed_measurement();
 
+    context->reset_controller();
+
     enter_time_ms = millis();
+
 }
 
 

src/main.cpp

@@ -3,7 +3,8 @@
 
 #include "rotary_encoder_driver.h"
 #include "motor_driver.h"
-#include "p_controller.h"
+//#include "p_controller.h"
+#include "pi_controller.h"
 
 #include "motor_controller.h"
 
@@ -16,10 +17,15 @@ RotaryEncoderDriver encoder;
 
 MotorDriver motor;
 
-const double KP = 12.5;
+//const double KP = 12.5;
 
-P_controller controller(KP);
+//P_controller controller(KP);
 
+PI_controller controller(
+    12.5,           // Kp
+    1.0,            // Ti
+    0.004           // T = 4 ms
+);
 
 InitializationState initialization_state;
 
@@ -33,7 +39,8 @@ MotorController motor_controller(
     motor,
     controller,
     initialization_state,
-    operational_state
+    operational_state,
+    stopped_state
 );
 
 
@@ -53,11 +60,11 @@ int main()
     sei();
 
 
-    motor_controller.set_states(
+    // motor_controller.set_states(
-        &initialization_state,
+    //     &initialization_state,
-        &operational_state,
+    //     &operational_state,
-        &stopped_state
+    //     &stopped_state
-    );
+    // );
 
 
     motor_controller.begin();

src/motor_controller.cpp

@@ -7,35 +7,39 @@
 MotorController::MotorController(
     RotaryEncoderDriver& encoder,
     MotorDriver& motor,
-    P_controller& controller,
+    //P_controller& controller,
+    //PI_controller& controller;
+    Controller& controller,
     State& initialization,
-    State& operational
+    State& operational,
+    State& stopped
 )
     : current_state(nullptr),
-      initialization_state(nullptr),
+      initialization_state(initialization),
-      operational_state(nullptr),
+      operational_state(operational),
-      stopped_state(nullptr),
+      stopped_state(stopped),
       encoder(encoder),
       motor(motor),
       controller(controller),
       led(LED_PIN),
       reference_rpm(DEFAULT_REFERENCE_RPM)
 {
+    initialization_state.set_context(this);
+    operational_state.set_context(this);
+    stopped_state.set_context(this);
 }
 
 
-void MotorController::set_states(
+// void MotorController::set_states(
-    State* initialization,
+//     State* initialization,
-    State* operational,
+//     State* operational,
-    State* stopped
+//     State* stopped
-)
+// )
-{
+
-    initialization_state = initialization;
+    // initialization_state = initialization;
-    operational_state = operational;
+    // operational_state = operational;
-    stopped_state = stopped;
+    // stopped_state = stopped;
-    //initialization_state.set_context(this);
+
-    //operational_state.set_context(this);
-}
 
 
 void MotorController::begin()
@@ -56,6 +60,8 @@ void MotorController::transition_to(State& new_state)
 
     current_state = &new_state;
 
+    current_state->set_context(this);
+
     current_state->enter();
 }
 
@@ -128,13 +134,19 @@ bool MotorController::fault_active()
     return motor.fault_active();
 }
 
-//void MotorController::led_on()
+void MotorController::reset_controller()
-//{
+{
-//    led.set_hi();
+    controller.reset();
-//}
+}
+
+void MotorController::led_on()
+{
+   led.set_hi();
+}
 
 
-//void MotorController::led_off()
+void MotorController::led_off()
-//{
+{
-//    led.set_lo();
+   led.set_lo();
-//}
+}
+

src/motor_driver.cpp

@@ -126,5 +126,5 @@ void MotorDriver::set(double pwm)
 
 bool MotorDriver::fault_active()
 {
-    return !(PIND & (1 << PIND4));p
+    return !(PIND & (1 << PIND4));
 }

src/pi_controller.cpp

@@ -0,0 +1,57 @@
+#include "pi_controller.h"
+
+
+PI_controller::PI_controller(
+    double Kp,
+    double Ti,
+    double T
+)
+    : Kp(Kp),
+      Ti(Ti),
+      T(T),
+      integral(0.0)
+{
+}
+
+double PI_controller::update(double ref, double actual)
+{
+    return 0.0;
+}
+
+// double PI_controller::update(double ref, double actual)
+// {
+//     double error = ref - actual;
+
+//     double new_integral =
+//         integral + error * T;
+
+//     double output =
+//         Kp * (error + new_integral / Ti);
+
+
+//     // Positive saturation
+//     if (output > 100.0)
+//     {
+//         return 100.0;
+//     }
+
+
+//     // Negative saturation
+//     if (output < -100.0)
+//     {
+//         return -100.0;
+//     }
+
+
+//     // Only save the integral if the controller
+//     // is not saturated
+//     integral = new_integral;
+
+//     return output;
+// }
+
+
+void PI_controller::reset()
+{
+    integral = 0.0;
+}

test/test_pi_controller/test_main.cpp

@@ -0,0 +1,28 @@
+#include <unity.h>
+
+//#include "p_controller.h"
+#include "pi_controller.h"
+
+static const double KP = 2.0;
+static const double KI = 1.0;
+static const double T = 0.1;
+static const double TOL = 1e-4; // Tolerance
+
+void test_positive_error()
+{
+    PI_controller controller(KP, KI, T);
+
+    double output = controller.update(10.0, 8.0);
+
+    TEST_ASSERT_DOUBLE_WITHIN(TOL, 4.4, output);
+}
+
+
+int main()
+{
+    UNITY_BEGIN();
+
+    RUN_TEST(test_positive_error);
+
+    return UNITY_END();
+}