32343
843447835d
- IPS200 display menu with real-time status and parameter settings - Camera image processing and line tracking - IMU attitude solution and PID control - WiFi SPI oscilloscope data transmission
103 lines
4.2 KiB
C
103 lines
4.2 KiB
C
#ifndef _PID_CONTROL_H_
|
|
#define _PID_CONTROL_H_
|
|
|
|
#include "zf_common_typedef.h"
|
|
|
|
/*============================================================
|
|
* 电机引脚定义 (与 cpu0_main.c 保持一致)
|
|
*============================================================*/
|
|
#define MOTOR_DIR1 (P11_9)
|
|
#define MOTOR_PWM1 (ATOM3_CH5_P11_10)
|
|
#define MOTOR_DIR2 (P11_11)
|
|
#define MOTOR_PWM2 (ATOM2_CH7_P11_12)
|
|
|
|
/*============================================================
|
|
* PID 单环结构体 (支持位置式/增量式, 带积分分离 + 输出限幅)
|
|
*
|
|
* mode: 0=位置式 (output = f(error) 直接输出)
|
|
* 1=增量式 (output += Δf(error) 累积输出, 天然抗饱和)
|
|
*============================================================*/
|
|
typedef struct {
|
|
float kp, ki, kd; /* PID 系数 */
|
|
float setpoint; /* 目标值 (外部写入) */
|
|
float feedback; /* 反馈值 (外部写入) */
|
|
float error; /* 当前误差 (只读) */
|
|
float integral; /* 积分累加值 (位置式用) */
|
|
float prev_error; /* e(k-1) */
|
|
float prev_error2; /* e(k-2), 增量式 D 项用 */
|
|
float derivative; /* 本次微分值 (只读) */
|
|
float output; /* 本次输出 (只读) */
|
|
float out_max, out_min; /* 输出限幅 */
|
|
float integral_limit; /* 积分分离阈值 */
|
|
uint8 mode; /* 0=位置式, 1=增量式 */
|
|
} pid_t;
|
|
|
|
/*============================================================
|
|
* 四轮车速度/转向 PID 全局句柄
|
|
*
|
|
* 前向运动:
|
|
* 目标速度 → 左/右速度环 → 左/右PWM基础值
|
|
*
|
|
* 并行方向 (转向):
|
|
* 转向环 → PWM差速值 (用陀螺仪 yaw rate 做 D 项反馈)
|
|
*
|
|
* 最终输出:
|
|
* 左轮PWM = 左速度环输出 - PWM差速值
|
|
* 右轮PWM = 右速度环输出 + PWM差速值
|
|
*============================================================*/
|
|
extern pid_t g_pid_speed_left; /* 左轮速度环 (内环, 100ms) */
|
|
extern pid_t g_pid_speed_right; /* 右轮速度环 (内环, 100ms) */
|
|
extern pid_t g_pid_steering; /* 转向环 (并行, 帧周期) */
|
|
|
|
/*============================================================
|
|
* 电机输出接口 (每个电机驱动一侧两个轮子, 差速转向)
|
|
*============================================================*/
|
|
#define PWM_DUTY_MAX_MOTOR ((int16)9990)
|
|
#define MOTOR_PWM_DEADBAND ((int16)3000)
|
|
#define SPEED_PID_OUT_MAX ((float)(PWM_DUTY_MAX_MOTOR - MOTOR_PWM_DEADBAND))
|
|
|
|
void motor_set_left (int16 pwm); /* 左电机: 正=前进, 负=后退 */
|
|
void motor_set_right(int16 pwm); /* 右电机: 正=前进, 负=后退 */
|
|
|
|
/*============================================================
|
|
* PID 核心函数
|
|
*============================================================*/
|
|
void pid_init(pid_t *p, float kp, float ki, float kd,
|
|
float out_max, float out_min, float integral_limit, uint8 mode);
|
|
|
|
float pid_compute(pid_t *p); /* 位置式 */
|
|
|
|
float pid_compute_inc(pid_t *p); /* 增量式 */
|
|
|
|
void pid_reset(pid_t *p);
|
|
|
|
/*============================================================
|
|
* 控制环总控
|
|
*============================================================*/
|
|
|
|
/* 速度环 (内环): 每100ms调用, 编码器速度 → PWM基础值
|
|
* target_speed 由 Base Spd 菜单参数设定 (全局变量)
|
|
* 输出: 驱动左右电机 PWM
|
|
*/
|
|
void cascade_speed_loop(int16 enc1_count, int16 enc2_count);
|
|
|
|
/* 转向环 (并行): 每帧调用, 摄像头中线偏差 → PWM差速
|
|
* gyro_z 作为 D 项反馈, 抑制转向过冲和蛇形
|
|
*/
|
|
void cascade_steering_loop(float line_deviation, float gyro_z);
|
|
|
|
/* 控制使能 */
|
|
extern bool g_control_enabled;
|
|
|
|
/* 目标速度 (Base Spd → 速度环输入), 单位: 编码器脉冲/100ms */
|
|
extern float g_target_speed;
|
|
|
|
/* 最终下发到左右电机的 PWM 命令 (含死区补偿和转向差速) */
|
|
extern float g_motor_pwm_left_cmd;
|
|
extern float g_motor_pwm_right_cmd;
|
|
|
|
/* 陀螺仪 z 轴角速度 (°/s), 在 5ms ISR 中更新, 供转向环 D 项使用 */
|
|
extern float g_gyro_z_dps;
|
|
|
|
#endif /* _PID_CONTROL_H_ */
|