added lpf filter, pwm output

2022-09-10 16:23:33 +02:00
parent 12347b2df2
commit 8c1c99f27f
6 changed files with 264 additions and 31 deletions
@@ -31,13 +31,12 @@ double pidcont_next(pidcont_t* p, double target, double actual, double dt) {
    double integ  = 0.0;
    double output = 0.0;

-    const double kk = 100;
+    error = target - actual;
+    integ += error * dt;

-    error = (target - actual);
-    integ += (error * dt);
-    deriv = (error - p->perror) * dt;
+    deriv = error - p->perror;

-    output = (p->kp * error / kk) + (p->ki * integ / kk) + (p->kd * deriv / kk);
+    output = (p->kp * error) + (p->ki * integ) + (p->kd * deriv);

    p->perror = error;
    p->integ  = integ;