基于STM32的智能分拣机器人系统设计#

1
typedef struct {
2
    float kp;           // 比例系数
3
    float ki;           // 积分系数
4
    float kd;           // 微分系数
5
    float integral;     // 积分项累积值
6
    float prev_error;   // 上一次误差
7
    float output_limit; // 输出限幅值
8
    float integral_limit; // 积分限幅值
9
    float dead_zone;    // 死区范围
10
} PID_Controller;

1
float calculate_path_error(uint8_t sensor_values[8]) {
2
    float weighted_sum = 0;
3
    float total_weight = 0;
4

5
    for (int i = 0; i < 8; i++) {
6
        if (sensor_values[i] > DETECTION_THRESHOLD) {
7
            weighted_sum += (i - 3.5) * sensor_values[i];
8
            total_weight += sensor_values[i];
9
        }
10
    }
11

12
    return (total_weight > 0) ? (weighted_sum / total_weight) : 0;
13
}

1
帧头(2字节) | 命令字(1字节) | 数据长度(1字节) | 数据(N字节) | 校验和(1字节) | 帧尾(1字节)
2
0xAA 0x55   | CMD           | LEN            | DATA[LEN]   | CHECKSUM      | 0x0D