目前程序没有完全调好需要再改进会在从5cm回到-5cm路上停下一、程序总体功能k230pid.c实现了一套基于 K230 视觉检测、MSPM0 单片机控制和步进电机执行的小球位置闭环控制系统。系统的基本控制链路为K230视觉识别 ↓ 获得小球相对目标点的位置误差 ↓ MSPM0通过串口接收误差 ↓ 状态观测器估计小球位置和速度 ↓ 位置—速度串级控制器计算目标倾斜量 ↓ 步进电机改变导轨/圆管倾角 ↓ 小球在重力作用下移动 ↓ K230再次检测形成闭环从文件名看它是一个 PID 程序但从当前实际执行路径看它已经发展成一个组合控制系统包含定时中断闭环控制K230视觉误差接收α-β状态观测器位置—速度串级控制两阶段目标切换识别小球静止/运动状态判断静摩擦与卡滞补偿步进电机位置限制分段运动与斜率限制数据丢失保护最终到位保持文件中还保留了一套传统 PID 和轨迹控制算法但目前定时中断实际调用的是BallCascade_Control()而不是传统的BallTrajectory_Control()。二、核心参数及含义2.1基础控制参数#define BALL_ERROR_DEADBAND 7误差死区避免误差和视觉噪声导致电机频繁动作#define BALL_STEPPER_MIN_HZ 650#define BALL_STEPPER_MAX_HZ 2000步进电机最低和最高脉冲频率其中最大值用于初始化PID输出范围在当前串级控制实际使用固定的4200这两个参数主要服务于旧pid控制路径#define BALL_DATA_TIMEOUT_TICKS 50Uk230数据超时门限为50个控制周期超时后就停止电机#define BALL_MOTOR_DIRECTION 1用于适配机械安装方向为1控制方向不变为0电机运动方向取反。#define BALL_Y_POSITION_LIMIT 800步进电机相对于启动零点允许的累计位置范围(-800~800)#define BALL_OPUT_PER_STEP 50.0f旧PID输出到步进电机步数的比例系数例如:PID输出为500时对应10步。该参数只在旧函数BallStepper_SetOutput()和BallStepper_Shake()中使用当前串级控制直接输出目标电机位置。#define BALL_CONTROL_PERIOD_S 0.01f控制周期为0.01s真正配置取决于定时器周期2.2串级控制参数#define BALL_POSITION_KP 3.5f位置环比例系数 根据位置误差生成目标速度v -kp*△x(负号表示偏移某一侧死后,目标速度指向相反方向)#define BALL_TARGET_SPEED_MAX 200.0f目标速度限幅(-200~200)#define BALL_SPEED_KP 2.6f速度环比例系数根据速度误差计算电机目标位置(目标倾斜角度)#define BALL_CASCADE_POS_LIMIT 800串级输出限幅#define BALL_CASCADE_DELTA_MAX 28单周期最大调整步数优点:避免导轨角度突变降低机械冲击减少步进电机丢步风险让控制输出更加平滑#define BALL_CASCADE_SPEED_HZ 4200U串级控制实际使用的最大限制频率三、全局变量与嵌入式变量修饰1.volatile变量表示这些变量可能被中断或其他执行上下文异步修改编译器每次访问时都应重新从内训读取不能随意缓存或优化掉但是只能防止编译器错误优化并以保证多字节变量的原子性不等价于线程安全或者中断安全。变量含义Ball_ErrorXK230发送的小球位置误差Ball_PID_Output控制器输出或速度误差调试值Ball_NoDataTicks未收到新数据的周期数Ball_PID_Active是否已经收到有效数据并进入控制状态Ball_PID_Enabled控制功能是否允许运行Ball_Y_Position软件记录的电机累计位置Ball_AdjustSteps最近一次调整的步数Ball_IsMoving小球是否被判断为正在运动Ball_TaskStage当前任务阶段1或2Ball_ErrorSpeed误差接近速度的滤波估计Ball_Y_TargetPosition电机目标位置Ball_ControlPhase控制阶段Ball_StepCommandCount已发送的步进运动命令数Ball_StuckTiltBoost克服静摩擦的附加倾斜量Ball_FinalHold是否已进入最终保持状态Ball_PositionEstimate观测器估计的小球位置Ball_VelocityEstimate观测器估计的小球速度Ball_TargetVelocity位置环产生的目标速度四、小球运动状态识别static void Ball_UpdateMotionState(int32_t error) { int32_t change; if (!s_errorSampleValid) { s_lastBallError error; s_errorSampleValid 1U; return; } change error - s_lastBallError; if (change 0) change -change; s_lastBallError error; //判断小球是否在运动阈值原本为4 if (change 3) //运动 { s_stillConfirmCount 0U; if (s_moveConfirmCount 3U) s_moveConfirmCount; if (s_moveConfirmCount 3U) Ball_IsMoving 1U; } else //静止 { s_moveConfirmCount 0U; if (s_stillConfirmCount 4U) s_stillConfirmCount; if (s_stillConfirmCount 4U) Ball_IsMoving 0U; } }判断规则若连续3次满足 \(\Delta e\geq4\)认为小球正在运动若连续4次满足 \(\Delta e4\)认为小球静止。这种方法不是单帧立即切换状态而是使用连续确认计数。其优点是避免视觉坐标抖动导致状态在“运动”和“静止”之间频繁跳变。这是一种数字去抖、时间滤波或滞回确认思想。这里需要区分阈值4是空间变化阈值连续3次和4次是时间确认阈值二者结合增强了状态判断稳定性。五、电机执行static void BallStepper_SetOutput(float output, int32_t maxSteps, uint16_t speedHz) { int32_t adjustSteps; //等待上一次运动完成再执行下一次 if (!Servo_Y_IsDone()) { Ball_AdjustSteps 0; return; } adjustSteps (int32_t)(output / BALL_OUTPUT_PER_STEP); if (adjustSteps 0 output ! 0.0f) adjustSteps (output 0.0f) ? 1 : -1; if (adjustSteps maxSteps) adjustSteps maxSteps; if (adjustSteps -maxSteps) adjustSteps -maxSteps; if (BALL_MOTOR_DIRECTION 0) adjustSteps -adjustSteps; if (Ball_Y_Position adjustSteps BALL_Y_POSITION_LIMIT) adjustSteps BALL_Y_POSITION_LIMIT - Ball_Y_Position; else if (Ball_Y_Position adjustSteps -BALL_Y_POSITION_LIMIT) adjustSteps -BALL_Y_POSITION_LIMIT - Ball_Y_Position; Ball_AdjustSteps adjustSteps; if (adjustSteps 0) return; Servo_Y_SetSpeed(speedHz); Servo_Y_Stepper_Run(adjustSteps); Ball_Y_Position adjustSteps; }1.非阻塞异步执行不在中断等待电机2. PID输出到步数的转换adjustSteps (int32_t)(output / BALL_OUTPUT_PER_STEP);3.位置式电机控制/*位置式电机控制 不是每次执行完整目标控制逐段靠近目标位置 使用位置目标 分段跟踪的结构 */ static void BallStepper_MoveToPosition(int32_t targetPosition, int32_t maxSegmentSteps, uint16_t speedHz) { int32_t moveSteps; //限幅 if (targetPosition BALL_Y_POSITION_LIMIT) targetPosition BALL_Y_POSITION_LIMIT; if (targetPosition -BALL_Y_POSITION_LIMIT) targetPosition -BALL_Y_POSITION_LIMIT; Ball_Y_TargetPosition targetPosition; //等待上一次电机动作完成异步非阻塞 if (!Servo_Y_IsDone()) return; moveSteps targetPosition - Ball_Y_Position; //误差机制 //限幅 if (moveSteps maxSegmentSteps) moveSteps maxSegmentSteps; if (moveSteps -maxSegmentSteps) moveSteps -maxSegmentSteps; if (moveSteps 0) { Ball_AdjustSteps 0; return; } Servo_Y_SetSpeed(speedHz); Servo_Y_Stepper_Run(moveSteps); if (!Servo_Y_IsDone()) { Ball_Y_Position moveSteps; Ball_AdjustSteps moveSteps; Ball_StepCommandCount; } }4.软件限位目标位置和运动结果都会被限制在[-800,800]这属于嵌入式系统中的安全保护设计。但是Ball_Y_Position是通过程序累计步数得到的开环位置不是编码器反馈。如果发生以下情况软件位置可能与真实位置不一致步进电机丢步电机堵转机械滑动系统启动时机构不在真实零位电源异常导致状态丢失。因此严格来说这不是绝对位置而是“从启动零点开始累计的估计位置”。更可靠的系统可以增加零位限位开关上电回零编码器反馈电机堵转检测。六、α-β状态观测器K230发送的是离散且可能存在噪声的位置误差系统还需要知道小球速度。直接用相邻位置相减会产生较大噪声因此程序设计了α-β观测器。1.观测器状态分别对应小球位置估计Ball_PositionEstimate小球速度估计Ball_VelocityEstimate2.预测过程每次定时中断先执行预测公式对应代码static void BallObserver_Predict(void) { if (s_observerInited) { Ball_PositionEstimate Ball_VelocityEstimate * BALL_CONTROL_PERIOD_S; } if (s_observerFrameTicks 65535U) s_observerFrameTicks; }这使用了匀速运动模型假设再很短的10ms周期内小球速度基本不变。即使在这一周期没有收到k230新数据观测器仍可根据上一次速度继续预测位置。3.校正过程收到视觉误差后计算残差其中r:测量与预测偏差预测位置zk230发送来的位置误差校正α参数作用α越大位置估计越相信当前视觉测量响应快但是噪声更容易进入估计结果α越小位置更加平滑抗噪声能力强但是会增加滞后性。β参数作用β控制残差对速度估计的修正强度β越大速度估计灵敏但容易抖动。β越小速度平滑但跟踪加速度变化较慢。4.不等间隔数据处理代码没有假设k230每个控制周期都一定发送数据而是通过参数s_observerFrameTicks记录两次视觉帧之间经历了多少个N10ms的周期参数含义Ts​控制周期MCU微控制器控制环的固定运行周期。文中明确提到“MCU控制环可以运行在100Hz”因此Ts1/1000.01Ts​1/1000.01 秒即10ms。这是系统最基础的时间基准。(定时器N或 NticksNticks​两次成功接收到的视觉帧之间所经历的 TsTs​ 控制周期的个数。它是一个整数计数器例如如果隔了5个周期才来一帧则 N5N5。T实际视觉帧间隔两次视觉帧之间真实的物理时间间隔。它通过公式 TN×TsTN×Ts​ 计算得出。关于取值限制 0.01s≤T≤0.20s0.01s≤T≤0.20s 的含义下限0.01s防止 N0N0即同一周期内重复处理同一帧数据确保至少经过一个完整控制周期才更新一次视觉数据。上限0.20s设定一个超时阈值即最多等待20个控制周期。如果超过0.2秒还没收到新帧系统会认为视觉数据已过期或丢帧严重从而停止使用该数据避免用“过时”的信息去控制当前的电机或执行器。总结这个机制的本质是用固定高频的时钟节拍Ts​去“丈量”不规则到达的视觉数据将非周期的视觉输入转化为带有时间戳T的可用信息从而实现多速率系统的协同。速度限幅速度估计限制为-500 V 500用于防止视觉误识别、坐标跳变或丢帧后恢复产生异常大的速度估计从而导致控制器突然输出极端值。七、位置速度串级控制钢珠控制核心static void BallCascade_Control(void)static void BallCascade_Control(void) { float positionError; //位置相对目标误差 float velocityError; //速度相对误差 float targetPositionFloat; //目标位置浮点 float absPosition; //绝对位置误差值 float absVelocity; //绝对速度误差值 int32_t targetPosition; //目标位置 if (!s_observerInited) return; //目标位置定义为0阴恻观测到位置本身则为相对目标的误差 positionError Ball_PositionEstimate; absPosition (positionError 0.0f) ? positionError : -positionError; absVelocity (Ball_VelocityEstimate 0.0f) ? Ball_VelocityEstimate : -Ball_VelocityEstimate; if (Ball_TaskStage 2U absPosition 7.0f absVelocity BALL_SETTLE_SPEED) { Ball_FinalHold 1U; } if (Ball_FinalHold) { Ball_ControlPhase 3U; Ball_TargetVelocity 0.0f; Ball_PID_Output 0.0f; BallStepper_MoveToPosition(0, BALL_CASCADE_DELTA_MAX, BALL_CASCADE_SPEED_HZ); return; } //目标速度-3.5*相对目标位置误差限幅200 Ball_TargetVelocity Ball_ClampFloat( -BALL_POSITION_KP * positionError, -BALL_TARGET_SPEED_MAX, BALL_TARGET_SPEED_MAX); //速度误差 //Ball_VelocityEstimate在β观测器中得到 velocityError Ball_TargetVelocity - Ball_VelocityEstimate; targetPositionFloat BALL_SPEED_KP * velocityError; /* A stuck ball needs extra tilt, but the boost disappears after it moves. */ if (absPosition 12.0f absVelocity 10.0f) { if (s_stuckControlFrames 1000U) s_stuckControlFrames; Ball_StuckTiltBoost 120 (int32_t)(s_stuckControlFrames / 6U) * 35; if (Ball_StuckTiltBoost 500) Ball_StuckTiltBoost 500; targetPositionFloat (targetPositionFloat 0.0f) ? (float)Ball_StuckTiltBoost : -(float)Ball_StuckTiltBoost; } else { s_stuckControlFrames 0U; Ball_StuckTiltBoost 0; } targetPositionFloat Ball_ClampFloat( targetPositionFloat, -(float)BALL_CASCADE_POS_LIMIT, (float)BALL_CASCADE_POS_LIMIT); targetPosition (int32_t)targetPositionFloat; if (BALL_MOTOR_DIRECTION 0) targetPosition -targetPosition; Ball_PID_Output velocityError; if ((positionError 0.0f Ball_VelocityEstimate 0.0f) || (positionError 0.0f Ball_VelocityEstimate 0.0f)) Ball_ControlPhase 2U; else Ball_ControlPhase 0U; BallStepper_MoveToPosition(targetPosition, BALL_CASCADE_DELTA_MAX, BALL_CASCADE_SPEED_HZ); }串级控制分为内外环1.外环位置误差生成目标速度程序吧目标位置定义位0因此观测到的位子本身就是相对目标的误差如果小球离目标较远目标速度较大接近目标后目标速度自然减小。与直接用位置误差控制倾角相比这种方式明确规定了“希望小球以多块的速度接近目标”。2.内环速度误差产生目标倾角因为电机位置与导轨倾角近似相关所以目标电机位置实际上代表目标倾斜程度。内环会根据实际速度调整倾角小球运动太慢增加倾角小球运动太快减小倾角小球已有较大冲向目标的速度可能输出反向倾角进行制动。3.串级控制的优势相较于单一位置PID它具有以下优点能显式控制小球接近目标的速度更容易提前减速降低冲过目标点的概率能利用速度反馈增加系统阻尼对小球这种具有惯性的对象更合适。该控制器严格来说是位置外环P控制器 速度内环P控制器当前实际路径并不是传统的PID控制器。八、控制阶段判断变量Ball_Controlphase用于表示当前控制阶段0正常驱动/加速阶段1旧轨迹控制中的水平滑行阶段2反向制动或速度方向异常阶段3最终到位保持阶段判断条件if ((positionError 0 velocity 0) ||(positionError 0 velocity 0))表示位置误差和速度方向相反即小球正在向目标点运动。不过代码把这一状态名为阶段2更准确的说它代表“正在接近目标/存在制动需求”并不一定意味着此刻已经执行反向制动。九、最终到位保持待优化满足条件Ball_TaskStage 2absPosition 7 //绝对位置absVelocity 8 //绝对速度系统进入Ball_FinalHold 1;优点这比只判断位置更加合理。如果判断位置小球可能只是高速经过目标点并未真正的稳定。加入速度条件后要求小球不仅靠近目标而且运动速度足够小。进入最终保持状态后将目标速度设置为钢球PID控制系统输出设置为制动电机调整位置并且实时更新逻辑位置。代码如下Ball_TargetVelocity 0;Ball_PID_Output 0;BallStepper_MoveToPosition(0, ...);系统将导轨逐步恢复到标定水平位置。需要注意Ball_FinalHold是锁存状态。一旦置1本次任务中不会因为误差再次增大而自动退出只能通过停止、重新使能或阶段切换清除。这能避免在目标附近反复切换但也可能使小球受扰动后无法自动重新捕获目标。这里需要优化十、静摩擦与卡滞补偿导轨上存在静摩擦导致启动或者低俗运动时候小球出现卡滞现象。判断条件absPosition 12absVelocity 10小球明显未到达目标但是出现停止或者低速时候触发触发后逐渐增加在static void BallCascade_Control(void))Ball_StuckTiltBoostif (absPosition 12.0f absVelocity 10.0f) { if (s_stuckControlFrames 1000U) s_stuckControlFrames; Ball_StuckTiltBoost 120 (int32_t)(s_stuckControlFrames / 6U) * 35; if (Ball_StuckTiltBoost 500) Ball_StuckTiltBoost 500; targetPositionFloat (targetPositionFloat 0.0f) ? (float)Ball_StuckTiltBoost : -(float)Ball_StuckTiltBoost; } else { s_stuckControlFrames 0U; Ball_StuckTiltBoost 0; } targetPositionFloat Ball_ClampFloat( targetPositionFloat, -(float)BALL_CASCADE_POS_LIMIT, (float)BALL_CASCADE_POS_LIMIT); targetPosition (int32_t)targetPositionFloat; if (BALL_MOTOR_DIRECTION 0) targetPosition -targetPosition; Ball_PID_Output velocityError; if ((positionError 0.0f Ball_VelocityEstimate 0.0f) || (positionError 0.0f Ball_VelocityEstimate 0.0f)) Ball_ControlPhase 2U; else Ball_ControlPhase 0U;基本表示小球开始运动后就将补偿清0具体代码s_stuckControlFrames 0;Ball_StuckTiltBoost 0;这属于自适应静摩擦补偿或渐增式启动力矩补偿。它和积分控制有一定相似性二者都会在误差长期不能消除时逐渐增强控制量。但该方法只在“误差较大且速度很低”时工作比普通积分更加有针对性也不容易在高速运动时继续累积十一、两段轨迹识别0-5cm;5cm--5cm)使用变量Ball_TaskStage表示两个任务阶段通过误差变化推断目标是否发生变化判断条件具体代码Ball_TaskStage 1 //当前在第一阶段previousAbs 420 //上一帧误差处于一定范围内currentAbs 120 //当前两帧误差发生明显跳变jump 30 //相邻两帧误差发生明显跳变条件满足后切换为Ball_TaskStage 2;同时清除旧阶段状态清空最终保持标志清空速度估计相关变量清空制动阈值清空防卡滞补偿清空PID历史误差和积分重置观测器。这是典型的状态机和模式切换设计。切换时重置观测器非常重要因为目标点变化会让“误差坐标”瞬间改变。如果把该跳变当成小球真实运动就会估计出一个极大的虚假速度。十二、误差接近速度的低通滤波旧轨迹通过以下计算公式接近速度对应代码approachSpeed previousAbs - currentAbs;Ball_ErrorSpeed Ball_ErrorSpeed * 0.7f approachSpeed * 0.3f;含义大于0绝对值误差正在减少小球接近目标。小于0绝对值误差正在增大小球远离目标。这是一阶指数低通滤波器可以降低视觉坐标噪声对速度计算的影响。当前串级控制主要使用α-β观测器中的Ball_VelocityEstimateBall_ErrorSpeed主要服务于保留的旧轨迹控制。十三、传统轨迹控制算法目前虽然没有被定时中断调用BallTrajectory_Control()仍包含大量值得学习的控制思想。它把小球控制过程划分为以下阶段1.加速驱动阶段根据绝对误差大小选择不同倾斜量。误差越大倾斜步数越大误差越小倾斜步数越小。这属于分段非线性控制或增益调度。2.水平滑行阶段当小球接近目标且具有一定速度时BallStepper_MoveToPosition(0, ...);让导轨恢复到水平让小球依旧保持惯性滑行。3.反向制动阶段当小球更接近目标且速度仍较大时向相反方向倾斜导轨以抵消小球动量。这是一种基于状态机的预测制动思想4.最终保持阶段误差小于等于7时把导轨回复水平并停止继续调整。这套方法不完全依赖数字模型而是把人的控制经验写成分段规则属于经验控制或混合控制十四、PID增益调度static void BallPID_SetProfile(int32_t error, int32_t *maxSteps, uint16_t *speedHz) { int32_t absError (error 0) ? error : -error; /* Faster pulses improve response; smaller step segments limit tube angle. */ if(absError 260) { step.Kp 26.0f; step.Ki 0.0f; step.Kd 25.0f; step.OutMax 1800.0f; step.OutMin -1600.0f; *maxSteps 3; *speedHz 3700U; } else if(absError 230) { step.Kp 23.0f; step.Ki 0.0f; step.Kd 23.0f; step.OutMax 1600.0f; step.OutMin -1600.0f; *maxSteps 3; *speedHz 3500U; } else if (absError 200) { step.Kp 23.0f; step.Ki 0.0f; step.Kd 23.0f; step.OutMax 1600.0f; step.OutMin -1600.0f; *maxSteps 3; *speedHz 3200U; } else if (absError 180) { step.Kp 21.0f; step.Ki 0.0f; step.Kd 27.0f; step.OutMax 800.0f; step.OutMin -800.0f; *maxSteps 1; *speedHz 2700U; } else if (absError 160) { step.Kp 19.0f; step.Ki 5.0f; step.Kd 26.0f; step.OutMax 900.0f; step.OutMin -900.0f; step.IntMax 150;step.IntMin -150; *maxSteps 1; *speedHz 2600U; }else if (absError 130) { step.Kp 20.0f; step.Ki 6.0f; step.Kd 26.0f; step.OutMax 800.0f; step.OutMin -800.0f; *maxSteps 1; *speedHz 2600U; }else if (absError 100) { step.Kp 20.0f; step.Ki 6.0f; step.Kd 25.0f; step.OutMax 900.0f; step.OutMin -900.0f; *maxSteps 1; *speedHz 2500U; } else if (absError 80) { step.Kp 17.0f; step.Ki 2.0f; step.Kd 22.0f; step.OutMax 1000.0f; step.OutMin -1000.0f; *maxSteps 2; *speedHz 2800U; } else if (absError 60) { step.Kp 17.0f; step.Ki 3.0f; step.Kd 22.0f; step.OutMax 900.0f; step.OutMin -900.0f; *maxSteps 3; *speedHz 2800U; } else if (absError 40) { step.Kp 16.0f; step.Ki 3.0f; step.Kd 22.0f; step.OutMax 800.0f; step.OutMin -800.0f; *maxSteps 2; *speedHz 2800U; } else if (absError 30) { step.Kp 16.0f; step.Ki 2.0f; step.Kd 21.0f; step.OutMax 700.0f; step.OutMin -700.0f; *maxSteps 2; *speedHz 2800U; } else { step.Kp 14.0f; step.Ki 2.0f; step.Kd 18.0f; step.OutMax 600.0f; step.OutMin -600.0f; *maxSteps 2; *speedHz 2600U; } }根据绝对误差的不同区间动态修改KpKiKdPID输出限幅单次最大步数电机脉冲频率这种方法叫做增益调度。基本思想是距离目标较远时强调快速响应接近目标时降低输出并增强阻尼某些区间关闭积分防止大误差下积分累积不同误差区间采用不同执行速度。不过该函数当前没有在中断执行路径中被调用所以它是保留算法不是当前实际运行参数。另一个值得注意的问题是某些分支修改了IntMax和IntMin其他分支没有重新赋值。这意味着积分限幅可能保留上一次分支的值形成隐含状态。如果重新启用该算法最好在每个分支中都完整设置相关参数。十五、抖动脱困、回平和反向制动辅助函数以下函数目前也没有被实际主控制路径调用BallStepper_Shake()通过正向、反向和再次正向的周期动作让机构振动以克服小球卡滞。这是振动脱困策略。BallStepper_Level()每次最多向零位移动4步让导轨缓慢回平。BallStepper_ReverseBrake()根据当前电机倾斜方向执行反向运动且制动步数限制在624步。这些函数体现了代码从早期PID控制逐步演化到轨迹控制、再演化到串级控制的过程。十六、初始化、使能和停止流程1.初始化void K230PID_Init(void)void K230PID_Init(void) { UART2_K230_Init(); StepMotor_Init(); K230PID_Stop(); Ball_NoDataTicks 0U; Ball_PID_Active 0U; Ball_PID_Enabled 1U; NVIC_ClearPendingIRQ(TIMER_1_INST_INT_IRQN); NVIC_EnableIRQ(TIMER_1_INST_INT_IRQN); DL_TimerA_startCounter(TIMER_1_INST); }依次完成初始化K230串口初始化步进电机清除控制器历史状态清除无数据计数设置控制使能清除定时器中断挂起标志开启定时器中断启动定时器。说明整个控制器是由周期定时器驱动的。2.使能void K230PID_Enable(void)void K230PID_Enable(void) { Ball_PID_Enabled 1U; Ball_PID_Active 0U; Ball_NoDataTicks 0U; Ball_Y_Position 0; K230PID_Stop(); }执行设置使能标志控制状态暂时设为未激活清空超时计数把当前电机软件位置设为零调用停止函数清空历史状态。需要注意设置Ball_Y_Position0只是把当前机械位置当作软件零点并没有真正执行机械回零。3.停止void K230PID_Stop(void)void K230PID_Stop(void) { Servo_Y_Stop(); step.Error0 0.0f; step.Error1 0.0f; step.ErrorInt 0.0f; step.Out 0.0f; Ball_PID_Output 0.0f; Ball_AdjustSteps 0; Ball_IsMoving 0U; s_errorSampleValid 0U; s_moveConfirmCount 0U; s_stillConfirmCount 0U; s_shakePhase 0U; Ball_TaskStage 1U; Ball_ErrorSpeed 0.0f; Ball_BrakeThreshold 0; Ball_Y_TargetPosition 0; Ball_ControlPhase 0U; Ball_StuckTiltBoost 0; Ball_FinalHold 0U; s_stuckControlFrames 0U; s_trajectorySampleValid 0U; BallObserver_Reset(); }停止时会立即停止步进电机清空PID误差和积分清空输出清空运动状态判断回到第一任务阶段清除速度、制动和防卡滞状态清除轨迹采样状态重置α-β观测器。Ball_PID_Enabled清零所以它更接近“停止当前控制并等待新的有效视觉数据”而不是彻底禁用模块。十七、定时器中断控制流程核心控制位于void TIMER_1_INST_IRQHandler(void)void TIMER_1_INST_IRQHandler(void) { if (DL_TimerA_getPendingInterrupt(TIMER_1_INST) DL_TIMER_IIDX_ZERO) { int32_t error; uint8_t hasNewFrame 0U; if (!Ball_PID_Enabled) { DL_TimerA_clearInterruptStatus( TIMER_1_INST, DL_TIMERA_INTERRUPT_ZERO_EVENT); return; } BallObserver_Predict(); if (K230_GetFrame(error) K230_FRAME_ERROR) { hasNewFrame 1U; Ball_PID_Active 1U; Ball_NoDataTicks 0U; Ball_ErrorX error; if (BallTrajectory_Update(error)) { BallObserver_Reset(); } Ball_UpdateMotionState(error); BallObserver_Correct(error); } if (!hasNewFrame) { if (!Ball_PID_Active) { DL_TimerA_clearInterruptStatus( TIMER_1_INST, DL_TIMERA_INTERRUPT_ZERO_EVENT); return; } if (Ball_NoDataTicks 65535U) Ball_NoDataTicks; if (Ball_NoDataTicks BALL_DATA_TIMEOUT_TICKS) { K230PID_Stop(); Ball_PID_Active 0U; } } if (Ball_PID_Active) BallCascade_Control(); DL_TimerA_clearInterruptStatus(TIMER_1_INST, DL_TIMERA_INTERRUPT_ZERO_EVENT); } }定时器执行顺序如下1.判断是否使能如果没有使能不执行控制。if (!Ball_PID_Enabled)return;2.观测器预测无论有没有新视觉数据都先根据当前速度预测下一时刻位置。BallObserver_Predict();3.读取k230数据if (K230_GetFrame(error) K230_FRAME_ERROR)收到误差帧后标记有新数据激活控制器清零超时计数保存误差判断任务阶段是否切换更新运动/静止状态用视觉测量校正观测器。4.处理丢帧如果本周期没有收到新数据若系统从未收到过有效数据直接返回若已经激活则增加无数据计数达到50次后停止控制。5.执行串级控制即使某一周期没有新视觉帧只要没有达到超时门限仍会根据观测器预测结果继续控制。if (Ball_PID_Active)BallCascade_Control();6.清除中断标志保证下一个定时周期能够正常进入中断。十八、数据丢失保护该程序区分了两个状态Ball_PID_Enabled模块是否允许运行Ball_PID_Active是否已经收到过有效误差帧。系统启动后不会在没有视觉数据的情况下盲目控制。只有收到第一帧有效误差后才进入活动状态。活动后如果临时丢一两帧观测器继续预测控制器仍然工作避免偶发丢帧造成电机频繁启停。如果连续约0.5秒没有数据K230PID_Stop(); Ball_PID_Active 0;系统停止电机并重置控制状态。这是通信看门狗或数据有效性监控机制。十九、该程序体现的主要知识点可将本文件中的知识点概括为闭环反馈控制通过视觉误差持续修正执行器输出。位置式PID包含比例、积分、微分、积分限幅和输出限幅。串级控制位置外环生成目标速度速度内环生成目标倾角。状态观测器使用α-β观测器融合位置测量并估计小球速度。预测—校正结构无数据时预测有新数据时利用残差校正。多速率系统MCU控制频率和K230视觉帧率可以不同。有限状态机使用任务阶段、控制阶段、最终保持状态组织控制逻辑。数字滤波使用指数低通滤波处理误差变化速度。防抖判断连续多帧确认小球是运动还是静止。增益调度根据误差区间改变PID参数和执行器参数。非线性分段控制不同误差区间使用不同倾斜量和制动力度。静摩擦补偿小球卡滞时逐渐增加倾角。反向制动通过反向倾斜消除小球动量。执行器饱和限制目标位置、速度估计、输出和单次步数。控制量变化率限制每周期最多调整一定步数降低机械冲击。软件限位防止电机累计位置超过安全范围。非阻塞电机控制控制器只在电机完成上一段动作后发送下一段命令。通信超时保护长时间无视觉数据时自动停止。中断驱动控制使用硬件定时器保证近似固定的控制周期。嵌入式变量管理使用volatile处理主程序与中断共享变量使用static封装内部状态。二十、程序优点该程序的主要优点包括控制链路完整形成视觉闭环使用速度估计优于只根据位置误差控制串级结构适合具有惯性的小球系统考虑了视觉噪声、丢帧和帧率不稳定有位置、速度、输出和增量等多层限幅具有卡滞补偿适应真实机械摩擦使用分段步进控制降低机械冲击目标切换时重置观测器避免虚假速度最终到位同时判断位置和速度保留了大量调试变量便于串口、OLED或调试器观察。二十一、存在的问题和改进方向1 .文件名与实际算法不完全一致虽然文件名为k230pid.c当前实际控制函数是串级P控制不是传统完整PID。建议在报告中写成系统早期使用位置式PID当前版本以位置—速度串级比例控制为主并保留原PID与轨迹控制代码。2. 存在未调用函数和未使用宏当前未被实际路径调用的函数包括BallStepper_SetOutput()BallStepper_Shake()BallStepper_Level()BallStepper_ReverseBrake()BallTrajectory_Control()BallPID_SetProfile()未实际使用的宏包括BALL_ERROR_DEADBANDBALL_STEPPER_MIN_HZ这会增加代码阅读难度。可以删除旧代码或者通过编译选项明确选择控制模式。3. 注释存在乱码文件中部分中文注释因编码不一致出现乱码可能是UTF-8、GBK和编辑器设置不一致造成的。建议统一为UTF-8编码。4. 中断函数工作量较大中断中包含串口帧读取浮点观测器运算控制算法电机命令处理。在资源有限的MCU上中断时间过长可能影响其他实时任务。更合理的结构是定时中断只设置控制标志主循环或实时任务执行复杂控制计算串口中断只接收并缓存数据。如果当前100Hz周期内运算时间充足也可以继续使用但最好通过GPIO翻转或定时器测量中断执行时间。5 .软件电机位置可能漂移Ball_Y_Position是命令累计值不是真实反馈位置。建议增加上电回零限位开关编码器定期零点校准。6. 任务切换采用误差跳变推断通过误差变化判断K230是否切换目标可能受到误检、丢帧和坐标突变影响。更可靠的方法是让K230数据帧直接携带当前目标编号当前任务阶段帧序号校验信息。MCU收到明确阶段字段后再切换状态。7. 最终保持状态不能自动退出Ball_FinalHold一旦置位就持续让导轨回平。如果小球后来受到扰动离开目标系统不会重新控制。可以增加退出条件例如持续若干帧后清除Ball_FinalHold形成带滞回的重新捕获机制。8. 串级参数缺少单位说明代码中的位置、速度和步数单位没有明确标注。例如K230误差是像素还是毫米速度单位是像素/秒还是毫米/秒电机步数和导轨角度之间的换算关系目标点±5cm如何换算为图像坐标。报告中最好明确写出当前控制参数属于实验整定参数其数值与视觉坐标尺度、摄像头安装、步进电机细分数及机械传动比有关。9. 观测模型只采用匀速模型α-β观测器假设短周期内速度保持不变但真实小球受到倾角和摩擦影响存在加速度。后续可以考虑α-β-γ观测器卡尔曼滤波将导轨倾角作为模型输入建立小球动力学模型。10. 参数散布在代码中大量经验参数直接写在条件分支中例如7、12、10、120、500等。建议集中定义为宏或配置结构体便于参数说明在线调参不同机械平台复用记录实验版本。二十二、报告结论的总结k230pid.c构建了一套以K230视觉识别为传感器、MSPM0为控制器、步进电机为执行器的小球位置闭环控制系统。系统以10ms定时周期运行通过串口接收小球相对目标点的位置误差并利用α-β状态观测器对位置进行滤波、对速度进行估计。在实际控制路径中程序采用位置外环和速度内环组成的串级控制结构位置环根据位置偏差产生目标速度速度环再根据目标速度与估计速度之差生成导轨的目标倾斜位置。为了适应真实机械系统程序还设计了目标速度限幅、电机位置限位、单周期最大步数限制、视觉数据超时保护、运动状态多帧确认、静摩擦补偿和最终稳定判断等机制。此外程序能够通过视觉误差的跳变识别两阶段目标切换并在切换时清除旧控制状态及重置观测器避免目标坐标变化被错误解释为小球高速运动。该程序体现了PID控制、串级控制、状态观测、数字滤波、状态机、执行器饱和、通信看门狗和嵌入式中断编程等多方面知识。其主要特点不是单纯依靠一个固定参数PID而是通过观测器、串级反馈及多种经验补偿共同提高系统在视觉噪声、静摩擦、惯性和通信丢帧条件下的稳定性。