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();
}


位置保持模式例子

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();
}