基于 STM32+FreeRTOS 的智能平衡小车完整设计

// 任务优先级定义
#define TASK_PRIORITY_CONTROL     5  // 最高
#define TASK_PRIORITY_SENSOR      4
#define TASK_PRIORITY_COMM        3
#define TASK_PRIORITY_DISPLAY     2

// 任务栈大小
#define TASK_STACK_CONTROL        512
#define TASK_STACK_SENSOR         256
#define TASK_STACK_COMM           256
#define TASK_STACK_DISPLAY        128

void vControlTask(void *pvParameters) {
    TickType_t xLastWakeTime = xTaskGetTickCount();
    
    for (;;) {
        // 1. 读取姿态数据
        get_attitude(&roll, &pitch, &yaw);
        
        // 2. 计算平衡控制量 (串级 PID)
        float angle_out = pid_angle(&pid_angle_ctrl, target_angle, roll);
        float speed_out = pid_speed(&pid_speed_ctrl, target_speed, motor_speed);
        
        // 3. 合成最终 PWM 输出
        float pwm_l = angle_out + speed_out;
        float pwm_r = angle_out - speed_out;
        
        // 4. 限制 PWM 范围
        pwm_l = constrain(pwm_l, -MAX_PWM, MAX_PWM);
        pwm_r = constrain(pwm_r, -MAX_PWM, MAX_PWM);
        
        // 5. 更新电机
        motor_set_pwm(pwm_l, pwm_r);
        
        // 精确延时 1ms
        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(1));
    }
}

typedef struct {
    float Kp, Ki, Kd;
    float integral;
    float last_error;
    float output_max;
} PID_TypeDef;

float pid_calc(PID_TypeDef *pid, float target, float measure) {
    float error = target - measure;
    
    // 比例项
    float p_term = pid->Kp * error;
    
    // 积分项 (带限幅)
    pid->integral += error;
    pid->integral = constrain(pid->integral, -100, 100);
    float i_term = pid->Ki * pid->integral;
    
    // 微分项
    float d_term = pid->Kd * (error - pid->last_error);
    
    pid->last_error = error;
    
    float output = p_term + i_term + d_term;
    return constrain(output, -pid->output_max, pid->output_max);
}

#define ALPHA 0.98f  // 滤波系数

void complementary_filter(float *roll, float *pitch) {
    // 加速度计解算角度
    float accel_roll = atan2f(ay, az) * RAD2DEG;
    float accel_pitch = atan2f(-ax, sqrtf(ay*ay + az*az)) * RAD2DEG;
    
    // 陀螺仪积分角度
    gyro_roll += gx * dt;
    gyro_pitch += gy * dt;
    
    // 互补滤波融合
    *roll = ALPHA * (gyro_roll) + (1.0f - ALPHA) * accel_roll;
    *pitch = ALPHA * (gyro_pitch) + (1.0f - ALPHA) * accel_pitch;
}

typedef struct {
    float Q_angle;    // 过程噪声协方差
    float Q_gyro;     // 陀螺仪噪声协方差
    float R_angle;    // 加速度计噪声协方差
    float P[2][2];    // 误差协方差矩阵
    float K[2];       // 卡尔曼增益
} Kalman_TypeDef;

float kalman_filter(Kalman_TypeDef *k, float newAngle, float newRate, float dt) {
    // 预测
    k->P[0][0] += dt * (dt*k->P[1][1] - k->P[0][1] - k->P[1][0] + k->Q_angle);
    k->P[0][1] -= dt * k->P[1][1];
    k->P[1][0] -= dt * k->P[1][1];
    k->P[1][1] += k->Q_gyro * dt;
    
    // 更新
    float S = k->P[0][0] + k->R_angle;
    k->K[0] = k->P[0][0] / S;
    k->K[1] = k->P[1][0] / S;
    
    float y = newAngle - k->P[0][0];
    k->P[0][0] -= k->K[0] * k->P[0][0];
    k->P[0][1] -= k->K[0] * k->P[0][1];
    k->P[1][0] -= k->K[1] * k->P[0][0];
    k->P[1][1] -= k->K[1] * k->P[0][1];
    
    return k->K[0] * y;
}