PID多环处理，例如速度位置环

例如
typedef struct {
    float Kp, Ki, Kd;
    float integral_term;
    float last_error;
    float pwm_output;
} PID_Controller;

void init_PID(PID_Controller *pid, float kp, float ki, float kd) {
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->integral_term = 0.0f;
    pid->last_error = 0.0f;
}

float pid_update(PID_Controller *pid, float setpoint, float current_value, float dt) {
    float error = setpoint - current_value;
    pid->integral_term += error * dt;
    float derivative = (error - pid->last_error) / dt;
    pid->last_error = error;

    float output = pid->Kp * error + pid->Ki * pid->integral_term + pid->Kd * derivative;

    // 这里仅作为示例，实际应用中应根据需求限制输出范围
    return constrain(output, -100.0f, 100.0f);
}

// 假设的编码器位置读取函数
float read_encoder() {
    // 实际代码需要替换为读取编码器数据的逻辑
    return 0.0f;
}

// 假设的当前速度读取函数
float read_motor_speed() {
    // 实际代码需要替换为读取电机当前速度的逻辑
    return 0.0f;
}

// 假设的PWM信号更新函数
void update_motor_pwm(float pwm_signal) {
    // 实际代码需要替换为向电机驱动电路发送PWM信号的逻辑
}

// 主程序
int main() {
    MotorController motor_ctrl;
    PID_Controller pos_pid, speed_pid;
    init_PID(&pos_pid, KP_POS, KI_POS, KD_POS);
    init_PID(&speed_pid, KP_SPEED, KI_SPEED, KD_SPEED);
    motor_ctrl.setpoint_pos = DESIRED_POSITION;
    
    while (1) {
        motor_ctrl.current_pos = read_encoder();
        float pos_error = motor_ctrl.setpoint_pos - motor_ctrl.current_pos;
        float desired_speed_change = pid_update(&pos_pid, 0, pos_error, COMPUTATION_INTERVAL_SECONDS);//重点利用让位置的误差往0处理，算出每次增加的目标速度多少

        // 更新速度设定值并约束在合理范围内
        motor_ctrl.speed_pid.setpoint += desired_speed_change;
        motor_ctrl.speed_pid.setpoint = constrain(motor_ctrl.speed_pid.setpoint, MIN_SPEED, MAX_SPEED);//约束最大值，最小值

        motor_ctrl.current_speed = read_motor_speed();
        float speed_error = motor_ctrl.speed_pid.setpoint - motor_ctrl.current_speed;
        float pwm_signal = pid_update(&speed_pid, motor_ctrl.speed_pid.setpoint, motor_ctrl.current_speed, COMPUTATION_INTERVAL_SECONDS);

        update_motor_pwm(pwm_signal);
        delay(COMPUTATION_INTERVAL_SECONDS);

        if (abs(motor_ctrl.current_pos - motor_ctrl.setpoint_pos) < POSITION_TOLERANCE) {
            // 到达目标位置后的处理（如停止或进入位置保持模式）
            break;
        }
    }

    stop_motor();
}
首先通过位置PID控制器计算出期望速度的变化量，并将这个变化量叠加到速度设定值上。然后，通过速度PID控制器根据实际速度与更新后的速度设定值的差值来调整PWM输出，从而实现对电机速度的精确控制，进而使得电机向目标位置移动。这样做的好处是可以更好地平衡位置精度和响应速度，同时避免过冲和振荡。

位置保持模式例子

typedef struct {
    PID_Controller pos_pid;
    float setpoint_pos;
    float current_pos;
    bool is_in_position_hold_mode;
    float position_hold_pid_params[Kp, Ki, Kd]; // 位置保持模式下的PID参数
} MotorController;

void enter_position_hold_mode(MotorController *mc) {
    mc->is_in_position_hold_mode = true;
    mc->pos_pid.Kp = mc->position_hold_pid_params[Kp];
    mc->pos_pid.Ki = mc->position_hold_pid_params[Ki];
    mc->pos_pid.Kd = mc->position_hold_pid_params[Kd];
}

void exit_position_hold_mode(MotorController *mc) {
    mc->is_in_position_hold_mode = false;
    // 恢复正常位置跟踪时的PID参数
    // ... 这里根据实际情况设置 ...
}

bool new_position_command_arrived() {
    // 实际代码需要替换为检查是否有新位置命令到来的逻辑
    static uint32_t last_check_time = 0;
    if (millis() - last_check_time > NEW_POSITION_CHECK_INTERVAL) {
        last_check_time = millis();
        // 检查新位置命令是否存在并返回结果
        return check_new_position_command(); // 实际调用获取新位置命令的方法
    }
    return false;
}

float get_new_setpoint_position() {
    // 实际代码需要替换为获取新位置设定点的逻辑
    return retrieve_new_setpoint_position(); // 实际调用获取新位置设定点的方法
}

int main() {
    MotorController motor_ctrl;
    init_MotorController(&motor_ctrl, KP_POS, KI_POS, KD_POS);
    motor_ctrl.setpoint_pos = DESIRED_POSITION;
    
    while (1) {
        motor_ctrl.current_pos = read_encoder();

        if (!motor_ctrl.is_in_position_hold_mode) {
            // 在未进入位置保持模式下，执行正常的位置跟踪控制
            float pos_error = motor_ctrl.setpoint_pos - motor_ctrl.current_pos;
            float pwm_signal = pid_update(&motor_ctrl.pos_pid, pos_error, COMPUTATION_INTERVAL_SECONDS);

            update_motor_pwm(pwm_signal);
            
            if (abs(motor_ctrl.current_pos - motor_ctrl.setpoint_pos) < POSITION_HOLD_TOLERANCE) {
                enter_position_hold_mode(&motor_ctrl);
            }
        } else {
            // 在位置保持模式下，仅使用比例或比例微分控制
            float pos_error = motor_ctrl.setpoint_pos - motor_ctrl.current_pos;
            float pwm_signal = pid_update(&motor_ctrl.pos_pid, pos_error, 0.0f /* 不使用速度反馈 */, COMPUTATION_INTERVAL_SECONDS);

            update_motor_pwm(pwm_signal);

            if (new_position_command_arrived() || abs(pos_error) > POSITION_HOLD_EXIT_THRESHOLD) {
                motor_ctrl.setpoint_pos = get_new_setpoint_position();
                exit_position_hold_mode(&motor_ctrl);
            }
        }

        delay(COMPUTATION_INTERVAL_SECONDS);
    }

    stop_motor();
}

 大概实现实现思路

typedef struct {
    PID_Controller pos_pid; // 位置PID控制器
    PID_Controller speed_pid; // 速度PID控制器
    float setpoint_pos;
    float setpoint_speed;
    float current_pos;
    float current_speed;
} MotorController;

// 初始化位置和速度PID控制器
void init_MotorController(MotorController *mc, 
                           float kp_pos, float ki_pos, float kd_pos,
                           float kp_speed, float ki_speed, float kd_speed) {
    init_PID(&mc->pos_pid, kp_pos, ki_pos, kd_pos);
    init_PID(&mc->speed_pid, kp_speed, ki_speed, kd_speed);
}

// 更新电机控制器状态并计算PWM信号
float update_motor_controller(MotorController *mc, float dt, float max_pwm) {
    mc->current_pos = read_encoder(); // 获取当前位置
    mc->current_speed = read_motor_speed(); // 获取当前速度

    // 位置误差
    float pos_error = mc->setpoint_pos - mc->current_pos;

    // 计算期望速度，基于位置误差（可能需要加入速度限制）
    float desired_speed_change = pid_update(&mc->pos_pid, pos_error, dt);

    // 结合现有的速度与期望的速度变化，得到新的期望速度
    mc->setpoint_speed += constrain(desired_speed_change, -MAX_SPEED_CHANGE, MAX_SPEED_CHANGE);

    // 使用速度PID控制电机实际速度接近期望速度
    float speed_error = mc->setpoint_speed - mc->current_speed;
    float pwm_signal = pid_update(&mc->speed_pid, speed_error, dt, max_pwm);

    return pwm_signal;
}

int main() {
    MotorController motor_ctrl;
    init_MotorController(&motor_ctrl, Kp_pos, Ki_pos, Kd_pos, Kp_speed, Ki_speed, Kd_speed);
    motor_ctrl.setpoint_pos = DESIRED_POSITION;
    motor_ctrl.setpoint_speed = INITIAL_SPEED;

    while (1) {
        float pwm_output = update_motor_controller(&motor_ctrl, COMPUTATION_INTERVAL_SECONDS, MAX_PWM_VALUE);
        update_motor_pwm(pwm_output);
        delay(COMPUTATION_INTERVAL_SECONDS);
        
        // 当达到目标位置时，停止位置跟踪或调整为保持位置模式
        if (motor_ctrl.current_pos >= motor_ctrl.setpoint_pos) {
            // 调整位置PID设置或者直接切换到位置保持模式
        }
    }

    stop_motor();
}
这个例子中，首先根据位置误差计算出期望速度的变化量，然后通过速度PID控制器来调整电机的实际速度，使之逐步逼近目标速度，从而间接地使得电机向目标位置移动。当接近目标位置时，可以通过适当调整位置PID参数使其进入位置保持模式，或者直接冻结位置误差避免进一步加速。注意，这样的设计依然需要考虑实际硬件的限制和具体应用场景的需求进行适应性调整