more code

code/Core/Inc/pid.h

@@ -0,0 +1,137 @@
+/*
+ * pid.h
+ *
+ *  Created on: 19 Dec 2025
+ *      Author: janik
+ */
+
+#ifndef INC_PID_H_
+#define INC_PID_H_
+
+// pid_motor_wrap_i32.h
+typedef struct
+{
+    // Gains
+    int32_t kp;
+    int32_t ki;
+    int32_t kd;
+
+    // State
+    int32_t integrator;
+    int32_t prev_error;
+
+    // Limits
+    int32_t integrator_min;
+    int32_t integrator_max;
+    int32_t out_max;         // max |u|, e.g. 2400
+
+    // Slew limit
+    int32_t max_output_step; // max |u(k) - u(k-1)| per control step
+    int32_t last_output;     // u(k-1)
+} pid_i32_t;
+
+typedef struct
+{
+    uint16_t pwm;  // 0..out_max
+    uint8_t  dir;  // 0 or 1
+} pid_motor_cmd_t;
+
+static inline int32_t clamp_i32(int32_t x, int32_t lo, int32_t hi)
+{
+    if (x < lo) return lo;
+    if (x > hi) return hi;
+    return x;
+}
+
+static inline void pid_init(pid_i32_t *pid,
+                            int32_t kp,
+                            int32_t ki,
+                            int32_t kd,
+                            int32_t integrator_min,
+                            int32_t integrator_max,
+                            int32_t out_max,
+                            int32_t max_output_step)
+{
+    pid->kp = kp;
+    pid->ki = ki;
+    pid->kd = kd;
+
+    pid->integrator = 0;
+    pid->prev_error = 0;
+
+    pid->integrator_min = integrator_min;
+    pid->integrator_max = integrator_max;
+    pid->out_max        = out_max;
+
+    pid->max_output_step = max_output_step;
+    pid->last_output     = 0;
+}
+
+static inline void pid_reset(pid_i32_t *pid)
+{
+    pid->integrator = 0;
+    pid->prev_error = 0;
+    pid->last_output = 0;
+}
+
+static inline pid_motor_cmd_t pid_update_motor(pid_i32_t *pid,
+                                               int32_t setpoint,
+                                               int32_t position)
+{
+    pid_motor_cmd_t cmd;
+
+    // Basic PID (no overflow protection, as requested)
+    int32_t error = setpoint - position;
+
+    int32_t p = pid->kp * error;
+
+    int32_t i = pid->integrator + pid->ki * error;
+    i = clamp_i32(i, pid->integrator_min, pid->integrator_max);
+    pid->integrator = i;
+
+    int32_t d_error = error - pid->prev_error;
+    int32_t d = pid->kd * d_error;
+    pid->prev_error = error;
+
+    // Raw control effort
+    int32_t u_raw = p + i + d;
+
+    // Clamp to [-out_max, +out_max]
+    if (u_raw >  pid->out_max) u_raw =  pid->out_max;
+    if (u_raw < -pid->out_max) u_raw = -pid->out_max;
+
+    // ---------- Slew limit: avoid instant full reverse ----------
+    int32_t u_prev = pid->last_output;
+    int32_t u = u_raw;
+
+    int32_t max_step = pid->max_output_step;
+    if (max_step > 0) {
+        int32_t du = u_raw - u_prev;
+
+        if (du > max_step) {
+            u = u_prev + max_step;
+        } else if (du < -max_step) {
+            u = u_prev - max_step;
+        } else {
+            u = u_raw;
+        }
+    }
+
+    // Save for next time
+    pid->last_output = u;
+
+    // Map signed u to dir + pwm
+    if (u >= 0) {
+        cmd.dir = 1;
+        cmd.pwm = (uint16_t)u;
+    } else {
+        cmd.dir = 0;
+        cmd.pwm = (uint16_t)(-u);
+    }
+
+    if (cmd.pwm > pid->out_max)
+        cmd.pwm = (uint16_t)pid->out_max;
+
+    return cmd;
+}
+#endif /* INC_PID_H_ */