147 lines
3.3 KiB
C
147 lines
3.3 KiB
C
/*
|
|
* 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;
|
|
|
|
int32_t error = setpoint - position;
|
|
|
|
// Deadband: if within ±3 counts, hold position and reset integrator
|
|
if (error >= -3 && error <= 3) {
|
|
pid->integrator = 0;
|
|
pid->prev_error = 0;
|
|
pid->last_output = 0;
|
|
cmd.pwm = 0;
|
|
cmd.dir = 1;
|
|
return cmd;
|
|
}
|
|
|
|
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_ */
|