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

.gitignore

@@ -4,6 +4,8 @@ __pycache__
 *.csv
 *.typ
 *.pdf
+scripts
+*.bak
 
 # Vélarsértækar skrár sem PlatformIO býr til sjálft
 .vscode/c_cpp_properties.json

docs/diagrams/src/motor_state.wsd

@@ -20,6 +20,7 @@ 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 
@@ -32,17 +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
+' Initialization -d-> Operational : initialization\n complete
 
-PreOperational --> Operational : set operational [o]
+PreOperational -d-> Operational : set operational [o]
-PreOperational --> Initialization : reset [r]
+PreOperational -l--> Initialization : reset [r]
 
-Operational --> PreOperational : set pre-operational [p]
+Operational -u-> PreOperational : set pre-operational [p]
-Operational --> Initialization : reset [r]
+Operational -> Initialization : reset [r]
 Operational --> Stopped : fault detected
 
-Stopped --> Initialization : reset [r]
+Stopped -l-> Initialization : reset [r]
-Stopped --> Operational : set operational [o]
+Stopped -> Operational : set operational [o]
-Stopped --> PreOperational : set pre-operational [p]
+Stopped -u-> PreOperational : set pre-operational [p]
 
 @enduml

include/motor_controller.h

@@ -25,12 +25,16 @@ private:
 
     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(
@@ -64,7 +68,12 @@ public:
     bool fault_active();
 
 
-    void run_speed_controller();
+    // Skilar true þegar nýtt sýni var keyrt í gegnum stýringuna
+    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();
 
@@ -76,6 +85,8 @@ public:
 
     void set_Ti(double value);
 
+    void set_controller(P_controller& new_controller);
+
     void led_on();
 
     void led_off();

include/operational_state.h

@@ -2,10 +2,20 @@
 #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:
+    uint32_t sample_count = 0;
+
 public:
     void enter() override;
 

include/pre_operational_state.h

@@ -4,9 +4,15 @@
 #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;
 
@@ -26,7 +32,7 @@ private:
     uint8_t input_index;
 
 public:
-    PreOperationalState();
+    PreOperationalState(P_controller& p_controller, P_controller& pi_controller);
 
     void enter() override;
     void update() override;

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>

src/initialization_state.cpp

@@ -25,6 +25,7 @@ void InitializationState::update()
     if (millis() - enter_time_ms >= WAIT_MS)
     {
         context->to_pre_operational();
+        // context->to_operational();
     }
 }
 

src/main.cpp

@@ -22,27 +22,27 @@ RotaryEncoderDriver encoder;
 
 MotorDriver motor;
 
-const double Kp = 12.5;
+const double Kp_P = 10.0;     // P-controller from Project 2
-const double Ti = 1.0;
+
+const double Kp = 10.125;     // Ziegler-Nichols PI
+const double Ti = 0.0334;
 const double T = 0.004;
 
-// P_controller controller(Kp);
+P_controller p_controller(Kp_P);
-PI_controller controller(Kp, Ti, T);
+PI_controller pi_controller(Kp, Ti, T);
 
 
 InitializationState initialization_state;
-
+// Pre-operational velur á milli stýringanna með 'c'
-PreOperationalState pre_operational_state;
+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,
@@ -53,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')  // Tók þetta út svo það
+            motor_controller.command(c);   
-            //{                            // svo það væri hægt að setja 
+
-            motor_controller.command(c);   // inn gildi fyrir Kp og Ti
-            //}
         }
 
 
@@ -95,7 +81,4 @@ int main()
     return 0;
 }
 
-// int main(){
+#endif 
-//     return 0;
-// }
-#endif // PIO_UNIT_TESTING

src/motor_controller.cpp

@@ -20,9 +20,11 @@ MotorController::MotorController(
       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);
@@ -100,20 +102,37 @@ void MotorController::to_operational()
 }
 
 
-void MotorController::run_speed_controller()
+bool MotorController::run_speed_controller()
 {
-    if (encoder.new_speed_available())
+    if (!encoder.new_speed_available())
     {
-        double actual = encoder.rpm();
+        return false;
-
-        double pwm_percent =
-            controller.update(reference_rpm, actual);
-
-        double pwm =
-            pwm_percent * PWM_PER_PERCENT;
-
-        motor.set(pwm);
     }
+
+    double actual = encoder.rpm();
+
+    double pwm_percent =
+        controller->update(reference_rpm, actual);
+
+    double pwm =
+        pwm_percent * PWM_PER_PERCENT;
+
+    // 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)
+    {
+        pwm = -255.0;
+    }
+
+    motor.set(pwm);
+
+    last_actual_rpm = actual;
+    last_pwm = pwm;
+
+    return true;
 }
 
 
@@ -130,17 +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);
+    controller->set_Kp(value);
 }
 
 void MotorController::set_Ti(double value)
 {
-    controller.set_Ti(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

@@ -11,6 +11,12 @@ void OperationalState::enter()
     context->reset_controller();
 
     context->led_on();
+
+    sample_count = 0;
+
+#if LOG_CSV
+    Serial.println("time_ms,ref_rpm,actual_rpm,pwm");
+#endif
 }
 
 // Part 1 
@@ -33,7 +39,21 @@ void OperationalState::update()
         return;
     }
 
-    context->run_speed_controller();
+    if (context->run_speed_controller())
+    {
+        sample_count++;
+
+#if LOG_CSV
+        // 4 ms á milli sýna, tíminn byrjar á 0 við hverja innkomu í Operational
+        Serial.print(sample_count * 4UL);
+        Serial.print(',');
+        Serial.print(context->get_reference_rpm(), 1);
+        Serial.print(',');
+        Serial.print(context->get_actual_rpm(), 2);
+        Serial.print(',');
+        Serial.println(context->get_pwm(), 1);
+#endif
+    }
 }
 
 

src/pre_operational_state.cpp

@@ -4,8 +4,11 @@
 #include <Arduino.h>
 #include <stdlib.h>
 
-PreOperationalState::PreOperationalState()
+PreOperationalState::PreOperationalState(P_controller& p_controller, P_controller& pi_controller)
-    : previous_time(0),
+    : 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),
@@ -133,6 +136,23 @@ void PreOperationalState::command(char c)
 
         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()

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();
+        }
     }
 }
 
@@ -73,5 +77,5 @@ void StoppedState::command(char c)
 
 void StoppedState::exit()
 {
-    digitalWrite(LED_BUILTIN, LOW);
+    context->led_off();
 }