bætti við prentskipunum til þess að geta plottað step response

src/main.cpp

@@ -19,9 +19,9 @@ RotaryEncoderDriver encoder;
 
 MotorDriver motor;
 
-const double Kp = 12.5;
-const double Ti = 1.0;
-const double T = 0.004;
+const double Kp = 45.0; // Ziegler-Nichols Method 45.0
+const double Ti = 0.0; // 0.226
+const double T = 0.0; // 0.004
 
 // P_controller controller(Kp);
 PI_controller controller(Kp, Ti, T);

src/motor_controller.cpp

@@ -92,20 +92,39 @@ void MotorController::to_operational()
 }
 
 
-void MotorController::run_speed_controller()
+bool MotorController::run_speed_controller(ControlSample& sample)
 {
-    if (encoder.new_speed_available())
+    if (!encoder.new_speed_available())
     {
-        double actual = encoder.rpm();
-
-        double pwm_percent =
-            controller.update(reference_rpm, actual);
-
-        double pwm =
-            pwm_percent * PWM_PER_PERCENT;
-
-        motor.set(pwm);
+        return false;
     }
+
+    double actual = encoder.rpm();
+
+    double pwm_percent =
+        controller.update(reference_rpm, actual);
+
+    double pwm =
+        pwm_percent * PWM_PER_PERCENT;
+
+    // Klippt hér svo pwm_actual sé nákvæmlega það sem fer á mótorinn
+    if (pwm > 255.0)
+    {
+        pwm = 255.0;
+    }
+
+    if (pwm < -255.0)
+    {
+        pwm = -255.0;
+    }
+
+    motor.set(pwm);
+
+    sample.ref_rpm = reference_rpm;
+    sample.actual_rpm = actual;
+    sample.pwm_actual = pwm;
+
+    return true;
 }
 
 

src/operational_state.cpp

@@ -11,6 +11,11 @@ void OperationalState::enter()
     context->reset_controller();
 
     context->led_on();
+
+    // Tíminn byrjar á 0 í hvert skipti sem farið er inn í Operational
+    sample_count = 0;
+
+    Serial.println("time_ms,ref_rpm,actual_rpm,pwm");
 }
 
 // Part 1 
@@ -33,7 +38,20 @@ void OperationalState::update()
         return;
     }
 
-    context->run_speed_controller();
+    ControlSample sample;
+
+    if (context->run_speed_controller(sample))
+    {
+        sample_count++;
+
+        Serial.print(sample_count * SAMPLE_PERIOD_MS, 1);
+        Serial.print(",");
+        Serial.print(sample.ref_rpm, 1);
+        Serial.print(",");
+        Serial.print(sample.actual_rpm, 2);
+        Serial.print(",");
+        Serial.println(sample.pwm_actual, 1);
+    }
 }