主要函数:pidtest.c , k230pid.cstatic修饰:作用(1)只在当前.c文件内可见(2)函数退出后仍然保留数值下次调用时还能使用。一、pidtest.c整体功能概览循迹传感器|v计算偏差 trackError8|v循迹 PID / 分段 PID|v生成左右轮目标速度|v左右电机速度 PID|vPWM 输出|v电机运动|v编码器反馈实际速度二、代码架构#include ti_msp_dl_config.h#include Delay.h#include drv_Motor.h#include UART_1.h#include UART_2.h#include pid.h#include M0Gy.h#include encoder.h#include 12xunji.h#include KEY.h#include OLED_SoftI2C.h#include k230pid.h#include Step_Motor.h#include Serial.h#include LED.h模块作用ti_msp_dl_config.hMSPM0 芯片和 SysConfig 生成的配置drv_Motor.h电机驱动pid.hPID 结构体和 PID 算法encoder.h编码器读取12xunji.h12 路循迹传感器KEY.h按键扫描OLED_SoftI2C.hOLED 软件 I2CUART_1.h/UART_2.h串口通信LED.h指示灯k230pid.hK230 或外部视觉模块相关通信三、宏定义和参数配置#define SPEED_TARGET 245.0f //目标速度#define ENCODER_SPEED_SCALE 37.7f //编码器速度转换系数编码器读取的单位时间内的脉冲需要转换成速度速度 编码器计数 × 换算系数#define SPEED_SAMPLE_COUNT 2U //速度采样次数20ms读一次编码器每两次定时器中断统计一次平均速度作用降低编码器瞬间抖动减少速度测量噪声降低PID更新频率#define ENCODER_COUNT_MAX_PER_TICK 20 //编码器异常限制#define SPEED_MEASURE_MAX 500.0f如果某次编码器计数异常过大认为数据不可信是一种常见的输入数据保护#define SPEED_FEEDFORWARD 0.13f //速度前馈系数有一定初始的pwm速度环可以更加平滑加速响应更快四、PID中的重要编程技巧1.积分限幅防止积分饱和电机启动或者负载过大时误差可能长时间存在积分项会不断累加如果不限制可能会出现积分项很大电机已经接近目标但是输出仍然而很大产生严重超调。所以程序需要多层保护PID输出限幅-前馈叠加-pwm输出限幅2.不同电机使用不同PID参数左右电机的机械特性通常不完全一致因此参数可能不同3.三个PID控制对象用于循迹环不过在Track8_UpdateTargets()中程序没有直接调用PID_Update(Track)而是手动计算correction xianfu(-(kp * error ki * Track.ErrorInt kd * derivative),55.0f,-55.0f);循迹环使用动态参数在不同的误差下PID的参数不一样五、复位函数的设计1.循迹PID复位static void TrackPID_Reset(void) { Track.Error0 0.0f; Track.Error1 0.0f; Track.ErrorInt 0.0f; Track.Out 0.0f; }2.速度PID复位static void SpeedPID_Reset(void)作用重置A、B电机PID状态速度变量编码器计数是很重要的工程习惯。如果不清除下一次启动可能会出现刚启动就产生很大的PWM。六、全局变量和volatile代码中如volatile float SpeedA;volatile bool carRunning;volatile bool trackUpdatePending;为什么需要volatile?1.这些变量可能被主循环修改、定时器中断修改、编码器中断修改。例如trackUpdatePending true;在定时器中断中设置然后主循环读取如果没有volatile,编译器可能认为变量不会被异步改变从而优化掉重复读取因此中断和主循环共享的变量应考虑使用volatile七、运行状态机代码定义了运行模式typedef enum { CAR_MODE_KEY1 0, //按键1普通循迹 CAR_MODE_KEY2_TIMED, //定时加减速逐步加速运行到截止时间前减速到时间自动停车 CAR_MODE_KEY3_LAP //跑一圈 } CarMode_t;按键1普通循迹按键2定时加减速逐步加速运行到截止时间前减速到时间自动停车按键3跑一圈根据编码器累计值判断里程接近目标圈数时减速达到目标编码器计数后停止八、按键扫描技巧主循环中按键扫描以系统的Tick为基准每毫秒或者每一个系统节拍执行一次if (now ! lastKeyTick) { lastKeyTick now; Key_Tick(); } key Key_GetNum();优点不使用阻塞时延迟不影响主循环其他任务方便实现消抖读取按键定期扫描 重新更新读取使用扫描和读取分离的设计可以降低模块耦合九、主循环的非阻塞式设计主循环始终运行while (1) { now SysTick_GetTick(); UART2_K230_PollRx(); ... }程序没有使用大量的延时函数来完成周期任务而是使用Tick判断if ((uint32_t)(now - key2LastRampTick) 10U)这属于非阻塞式任务调度非阻塞定时的典型写法这种写法比直接延时更适合嵌入式系统if ((uint32_t)(now - lastTick) interval) { lastTick now; //更新 DoTask();//相应的任务 }十、循迹控制流程函数static void Track8_UpdateTargets(void) { uint8_t lost; uint8_t i; uint8_t rightBlackCount 0U; float error; float absError; float kp; float kd; float ki; float baseSpeed; float curveSlowdown; float minimumBaseSpeed; float correction; trackError8 get_track_err12_with_flag(lost); for (i 6U; i 12U; i) { if (D12[i]) rightBlackCount; } if (!(carMode CAR_MODE_KEY3_LAP) rightBlackCount 3U) { Car_SetRunning(false); return; } trackLost (lost ! 0U); //丢线处理 if (trackLost) { ASpeed.Target 0.0f; BSpeed.Target 0.0f; Track.ErrorInt 0.0f; Track.Out 0.0f; return; } error trackError8; derivative error - lastError; lastError error; absError (error 0.0f) ? error : -error; // if (absError 0.5f) { error 0.0f; absError 0.0f; kp 9.0f; ki 0.0006f; kd 0.01f; Track.ErrorInt * 0.8f; } else if (absError 1.5f) { kp 12.0f; ki 0.013f; kd 0.015f; } else if (absError 2.5f) { kp 16.0f; ki 0.31f; kd 0.015f; }else if (absError 3.5f) { kp 20.0f; ki 0.5f; kd 0.005f; } else { kp 28.0f; ki 0.9f; kd 0.0f; } if (error ! 0.0f) { Track.ErrorInt error; Track.ErrorInt xianfu(Track.ErrorInt, Track.IntMax, Track.IntMin); } //根据不同模式设置弯道减速系数 curveSlowdown (carMode CAR_MODE_KEY3_LAP) ? 8.0f : 15.0f; //设置最低基础速度 minimumBaseSpeed (carMode CAR_MODE_KEY3_LAP) ? keyCurrentSpeed * 0.65f : ((carMode CAR_MODE_KEY2_TIMED) ? 0.0f : 125.0f); baseSpeed (((carMode CAR_MODE_KEY2_TIMED) || (carMode CAR_MODE_KEY3_LAP)) ? keyCurrentSpeed : SPEED_TARGET) - curveSlowdown * absError; baseSpeed xianfu(baseSpeed, ((carMode CAR_MODE_KEY2_TIMED) || (carMode CAR_MODE_KEY3_LAP)) ? keyCurrentSpeed : SPEED_TARGET, minimumBaseSpeed); /* Left line error is positive; a negative correction turns the car left. */ correction xianfu(-(kp * error ki * Track.ErrorInt kd * derivative),55.0f,-55.0f); Track.Actual trackError8; Track.Out correction; if (((carMode CAR_MODE_KEY2_TIMED) || (carMode CAR_MODE_KEY3_LAP)) keyCurrentSpeed ((carMode CAR_MODE_KEY3_LAP) ? KEY3_SPEED_TARGET : KEY2_SPEED_TARGET)) { correction * keyCurrentSpeed / ((carMode CAR_MODE_KEY3_LAP) ? KEY3_SPEED_TARGET : KEY2_SPEED_TARGET); } ASpeed.Target xianfu(baseSpeed correction, 250.0f,((carMode CAR_MODE_KEY2_TIMED) || (carMode CAR_MODE_KEY3_LAP)) ? 0.0f : 80.0f); BSpeed.Target xianfu(baseSpeed - correction, 250.0f,((carMode CAR_MODE_KEY2_TIMED) || (carMode CAR_MODE_KEY3_LAP)) ? 0.0f : 80.0f); }主要步骤1.读取函数误差trackError8 get_track_err12_with_flag(lost);返回当前偏差是否丢线2.检测特殊黑线for (i 6U; i 12U; i){if (D12[i])rightBlackCount;}如果右侧连续监测到多个黑色传感器可能表示终点或特殊标志3.丢线处理if (trackLost){ASpeed.Target 0.0f;BSpeed.Target 0.0f;Track.ErrorInt 0.0f;Track.Out 0.0f;return;}丢线后直接把左右目标速度设为0让小车停车4.计算微分项derivative error - lastError;lastError error;微分项代表误差变化趋势误差快速增大可能即将偏移误差快速减小正在回到轨迹5.根据误差大小动态调整PIDif (absError 0.5f) { kp 9.0f; ki 0.0006f; kd 0.01f; } else if (absError 1.5f) { kp 12.0f; ki 0.013f; kd 0.015f; } ... else { kp 28.0f; ki 0.9f; kd 0.0f; }分段PID增益调度基本思想偏差小时控制柔和避免抖动偏差小时增加比例作用快速纠偏偏差极大时减少微分避免噪声放大6.误差积分Track.ErrorInt error; Track.ErrorInt xianfu( Track.ErrorInt, Track.IntMax, Track.IntMin );同时在误差小时Track.ErrorInt * 0.8f;相当于让积分逐渐衰减减少误差状态下的积分累积十一、弯道减速偏差越大基础速度越低避免高速过弯容易冲出轨迹baseSpeed currentSpeed - curveSlowdown * absError;同时要设置最低速度baseSpeed xianfu(baseSpeed, maximum, minimum);十二、左右轮速度分配差速转向的核心ASpeed.Target xianfu(baseSpeed correction, ...);BSpeed.Target xianfu(baseSpeed - correction, ...);假设correction0,则A轮速度增加B轮速度减少关系左轮目标 基础速度 转向修正右轮目标 基础速度 - 转向修正十三、速度采样和滤波定时器中断读取编码器rawA (float)EncoderA_Get();rawB (float)EncoderB_Get();编码器值先进性异常判断:if (rawA -ENCODER_COUNT_MAX_PER_TICK rawA ENCODER_COUNT_MAX_PER_TICK)累加两次样本rawASum abs(rawA);rawASampleCount;达到采样次数后计算速度measuredA rawASum *(ENCODER_SPEED_SCALE / SPEED_SAMPLE_COUNT);使用一阶低通滤波SpeedA 0.7f * SpeedA 0.3f * measuredA;作用降低编码器计数跳变减少速度噪声让PID输入更加平滑代价响应速度变慢可能引入滞后十四、前馈控制pwmA ASpeed.Target *SPEED_FEEDFORWARD ASpeed.Out;pwmB BSpeed.Target *SPEED_FEEDFORWARD BSpeed.Out;这里将PWM分成两部分pwm 速度前馈 PID修正前馈的作用根据目标速度预先给出大致 PWM减轻 PID 的工作量提高响应速度PID 只负责补偿电机差异摩擦力电池电压变化负载变化十五、定时器中断是实时控制核心void TIMER_0_INST_IRQHandler(void)主要负责判断小车是否运行读取编码器计算速度执行速度 PID计算 PWM控制电机设置主循环任务标志关键代码1.中断只设置标志真正的循迹计算放到主循环trackUpdatePending true;if (carRunning trackUpdatePending){trackUpdatePending false;//先清除标志Track8_UpdateTargets();//循迹计算}总结中断中做快速、确定、必须实时的工作复杂计算、显示、通信放到主循环。十六、中断与主循环的分工中断中完成编码器读取速度计算速度 PIDPWM 输出更新标志位主循环中完成按键处理模式切换加减速调度OLED 显示循迹目标速度更新串口轮询这种分工避免中断执行时间过长。十七、临界区保护static void Car_SetRunning(bool running) { bool wasRunning carRunning; int16_t finalEncoderA; NVIC_DisableIRQ(TIMER_0_INST_INT_IRQN); NVIC_DisableIRQ(Encoder_INT_IRQN); finalEncoderA EncoderA_Get(); //按键3模式的里程计数 if (wasRunning (key1CountActive || (carMode CAR_MODE_KEY3_LAP))) { encoderATotalCount (finalEncoderA 0) ? (uint32_t)(-(int32_t)finalEncoderA) : (uint32_t)finalEncoderA; } //清空标志 carRunning false; DRV8701_StopAll(); //复位 TrackPID_Reset(); SpeedPID_Reset(); rawASum 0; rawBSum 0; rawASampleCount 0U; rawBSampleCount 0U; aSpeedReady false; bSpeedReady false; trackUpdatePending false; ASpeed.Target 0.0f; BSpeed.Target 0.0f; pwmA 0.0f; pwmB 0.0f; (void)EncoderA_Get(); (void)EncoderB_Get(); NVIC_ClearPendingIRQ(Encoder_INT_IRQN); NVIC_EnableIRQ(Encoder_INT_IRQN); if (running) { //按键1直接加速 按键23由其他函数逐渐加速 ASpeed.Target ((carMode CAR_MODE_KEY2_TIMED) || (carMode CAR_MODE_KEY3_LAP)) ? 0.0f : SPEED_TARGET; BSpeed.Target ((carMode CAR_MODE_KEY2_TIMED) || (carMode CAR_MODE_KEY3_LAP)) ? 0.0f : SPEED_TARGET; carRunning true; /* Let the main loop read the sensors immediately after startup. */ trackUpdatePending true; } else if (wasRunning key1CountActive) { key1ElapsedMs SysTick_GetTick() - key1StartTick; key1CountActive false; encoderCountDisplayPending true; } NVIC_ClearPendingIRQ(TIMER_0_INST_INT_IRQN); NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN); oledStateUpdatePending !running; }在Car_SetRunning()中:修改变量前先关闭相关中断NVIC_DisableIRQ(TIMER_0_INST_INT_IRQN);NVIC_DisableIRQ(Encoder_INT_IRQN);完成后再打开中断NVIC_ClearPendingIRQ(...);NVIC_EnableIRQ(...);原因为了防止主循环和中断同时修改同一个变量产生数据竞争导致数据被莫名改为别的值十八、编码器读取设计编码器驱动中A相出发中断B相判断方向正交编码器的单边沿解码if (DL_GPIO_readPins(Encoder_PORT, Encoder_E1B_PIN) 0){EncoderA_cnt_Temp--;}else{EncoderA_cnt_Temp;}读取函数具有读取并清零的效果适合周期性测速int16_t EncoderA_Get(void){int16_t Temp;Temp EncoderA_cnt_Temp;EncoderA_cnt_Temp 0;EncoderA_cnt Temp;return Temp;}十九、运行中的加减速1.按键2模式每10ms调整一次速度利用的是系统定时if ((now - key2LastRampTick) 10U)加速/减速keyCurrentSpeed KEY2_ACCEL_STEP; //加速keyCurrentSpeed - KEY2_DECEL_STEP; //减速限制最大值if (keyCurrentSpeed KEY2_SPEED_TARGET){keyCurrentSpeed KEY2_SPEED_TARGET;}这样避免目标速度瞬间跳变减少电机冲击电池电流突变车体打滑PID 大幅响应二十、无符号Tick的时间计算技巧代码经常使用这种处理可以较好地处理系统Tick溢出(uint32_t)(now - lastTick) interval或者(int32_t)(now - deadline) 0二十一、OLED显示设计OLED 显示被封装成独立函数OLED_ShowRunState()OLED_ShowEncoderCount()OLED_ShowRunningTime()优点主循环逻辑更清晰显示布局集中管理后续修改界面更容易代码还使用了显示标志oledStateUpdatePendingencoderCountDisplayPending避免 OLED 被频繁刷新。因为 OLED 通常使用 I2C刷新过程可能比较慢如果在高频任务中频繁调用可能导致主循环卡顿I2C 阻塞影响实时控制二十二、状态停止函数的作用统一状态管理函数static void Car_SetRunning(bool running) { bool wasRunning carRunning; int16_t finalEncoderA; NVIC_DisableIRQ(TIMER_0_INST_INT_IRQN); NVIC_DisableIRQ(Encoder_INT_IRQN); finalEncoderA EncoderA_Get(); //按键3模式的里程计数 if (wasRunning (key1CountActive || (carMode CAR_MODE_KEY3_LAP))) { encoderATotalCount (finalEncoderA 0) ? (uint32_t)(-(int32_t)finalEncoderA) : (uint32_t)finalEncoderA; } //清空标志 carRunning false; DRV8701_StopAll(); //复位 TrackPID_Reset(); SpeedPID_Reset(); rawASum 0; rawBSum 0; rawASampleCount 0U; rawBSampleCount 0U; aSpeedReady false; bSpeedReady false; trackUpdatePending false; ASpeed.Target 0.0f; BSpeed.Target 0.0f; pwmA 0.0f; pwmB 0.0f; (void)EncoderA_Get(); (void)EncoderB_Get(); NVIC_ClearPendingIRQ(Encoder_INT_IRQN); NVIC_EnableIRQ(Encoder_INT_IRQN); if (running) { //按键1直接加速 按键23由其他函数逐渐加速 ASpeed.Target ((carMode CAR_MODE_KEY2_TIMED) || (carMode CAR_MODE_KEY3_LAP)) ? 0.0f : SPEED_TARGET; BSpeed.Target ((carMode CAR_MODE_KEY2_TIMED) || (carMode CAR_MODE_KEY3_LAP)) ? 0.0f : SPEED_TARGET; carRunning true; /* Let the main loop read the sensors immediately after startup. */ trackUpdatePending true; } else if (wasRunning key1CountActive) { key1ElapsedMs SysTick_GetTick() - key1StartTick; key1CountActive false; encoderCountDisplayPending true; } NVIC_ClearPendingIRQ(TIMER_0_INST_INT_IRQN); NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN); oledStateUpdatePending !running; }停止时完成carRunning false;DRV8701_StopAll();TrackPID_Reset();SpeedPID_Reset();清零速度和采样变量清除中断挂起标志启动时完成设置初始速度目标carRunning true;trackUpdatePending true;二十三、这份代码体现编程技巧1. 使用枚举代替魔法数字CAR_MODE_KEY1CAR_MODE_KEY2_TIMEDCAR_MODE_KEY3_LAP比直接写if (mode 2)可读性更强。2. 使用static限制作用域static void Track8_UpdateTargets(void)表示函数只在当前源文件中使用。这样可以避免命名冲突也体现模块封装。3. 使用const思维配置参数虽然这里主要使用宏但所有控制常量集中定义方便调参。4. 使用统一限幅函数float xianfu(float now, float max, float min)避免各处重复写限幅代码。5. 使用标志位实现任务解耦trackUpdatePendingoledStateUpdatePendingencoderCountDisplayPending中断只产生事件主循环负责处理事件。6. 使用前馈加反馈PWM Feedforward PID相比单独使用 PID动态性能更好。7. 对传感器数据做异常保护编码器计数和速度均做范围判断避免异常数据直接进入控制器。8. 启停时重置控制器状态避免上一次运行的积分项、误差项影响下一次启动。二十四、特别需要注意的问题1.Track的初始化参数没有真正完全使用虽然定义了Track.KpTrack.KiTrack.Kd但循迹函数中又根据误差动态设置局部变量kpkikd所以Track.Kp/Ki/Kd实际上基本没有参与循迹计算。这容易造成误解。更清晰的做法是要么直接使用Track.Kp/Ki/Kd要么删除无效初始化要么将动态参数写回结构体2. 微分项没有除以采样周期当前微分项是error - lastError严格的离散 PID 通常是(error - lastError) / dt如果采样周期发生变化微分项的实际效果也会变化。3. PID 积分也没有乘采样周期当前积分是Integral error × dt更严格的形式通常是Integral error × dt当前写法隐含了“采样周期固定且已经吸收到 Ki 中”的假设。4. 速度环只控制速度大小编码器速度使用绝对值rawASum abs(rawA);rawBSum abs(rawB);因此速度环只控制速度大小不控制方向。方向由DRV8701_SetSpeed(DRV8701_MOTOR_A, pwmA);DRV8701_SetSpeed(DRV8701_MOTOR_B, -pwmB);直接决定。5. 共享变量较多维护成本较高主循环、中断、编码器中断共同访问很多全局变量后续可以考虑更明确的数据结构独立的运行状态结构体更清晰的临界区封装统一的事件队列6. 中断中计算浮点 PID在 Cortex-M0 上浮点运算可能比较耗时。如果定时器频率很高应该确认中断执行时间是否发生中断嵌套PWM 更新是否及时是否需要定点化或降低 PID 更新频率二十五、核心总结这份代码最值得学习的地方有四点1. 双环控制结构循迹环生成左右轮目标速度速度环控制实际电机 PWM2. 中断与主循环分工中断负责实时控制主循环负责模式、按键、显示和调度3. 工程化保护措施包括积分限幅输出限幅编码器异常检测丢线停车启停复位中断临界区保护速度滤波4. 非阻塞任务调度使用系统 Tick 管理按键扫描加速和减速定时停车OLED 刷新运行时间显示整体上这不是一个单纯的 PID 示例而是一份将控制算法、嵌入式中断、状态机、传感器采集和人机交互结合起来的完整小车控制程序。