双环pid控制3510转设定圈后自锁，实现精准控距（保姆级）

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

首先要了解什么是双环pid，煮播下面使用的是角度-速度双环PID。外环pid负责控制系统的宏观目标（角度控制），其输出作为内环的设定值。而内环pid响应更快，控制微观变量（速度控制），直接作用于电机，快速消除扰动。

一、双环pid

ANGLE[1].MARK = ANGLE[1].POS_ABS - ANGLE[1].First_POS_ABS;
SingleSpeed=pid_calc(&ANGLEPID,ANGLE[1].MARK,position);
Motor[2].Motorout = -pid_calc(&Motor[2].MotorPID,Motor[2].Speed,SingleSpeed);

解释上述代码：通过PID计算将角度误差（ANGLE[1].MARK为get值，position为set值）转换为目标转速（SingleSpeed）。将外环输出的目标转速（SingleSpeed）与电机当前转（Motor[2].Speed）比较，通过PID计算输出电机控制信号（Motorout）。

注：还有不太懂的同学可以查查其他煮播写的，或者问问强大的ai。

二、编码器计数

下面继续聊聊3510电机，其实做这个项目使用步进电机要简单很多，是其性能体现的，步进电机通过脉冲数精确控制旋转圈数，无需编码器反馈。且步进电机在停止供电后，依靠磁阻转矩保持位置，适合断电自锁的场景。3510无刷电机，使用霍尔信号或编码器获取转子角度来进行位置检测。在很多情况下编码器计数容易丢圈，其中在控制层面，煮播换了3~4种计数方法，下面放一个计数有问题的代码，一起讨论一下。

1.错误计数

代码如下（示例）：

void update_motor_count(motor_measure_t *motor) {
    int32_t ecd_diff = motor->ecd - motor->last_ecd;
    if (ecd_diff > HALF_ECD_RANGE) {
        ecd_diff -= ENCODER_RESOLUTION;
    } else if (ecd_diff < -HALF_ECD_RANGE) {
        ecd_diff += ENCODER_RESOLUTION;
    }

    motor->ecd_count += ecd_diff / ENCODER_RESOLUTION;

  // 电机圈数重置， 因为输出轴旋转一圈， 电机轴旋转 36圈，将电机轴数据处理成输出轴数据，用于控制输出轴角度
    if (motor->ecd-motor->last_ecd > HALF_ECD_RANGE)
    {
        motor->ecd_count--;
    }
    else if (motor->ecd-motor->last_ecd < -HALF_ECD_RANGE)
	
	{
        motor->ecd_count++;
    }

    if (motor->ecd_count == FULL_COUNT)
    {
        motor->ecd_count = -(FULL_COUNT - 1);
    }
    else if (motor->ecd_count == -FULL_COUNT)
    {
        motor->ecd_count = FULL_COUNT - 1;
    }
	
    motor->last_ecd = motor->ecd; // 更新上一次的编码器值
}

举个栗子：

初始状态：motor->last_ecd = 4000，motor->ecd = 8096（下一次采样值）。

真实运动：电机正向旋转，编码器值从 4000 → 8096（实际差值为 +4096）。

ecd_diff：8096-4000=4096。

if (ecd_diff > HALF_ECD_RANGE)，4096-8191=-4095。

错误：真实正向旋转（差值为 +4096）被误判为反向溢出，错误修正为 -4095。

motor->ecd_count += ecd_diff / ENCODER_RESOLUTION：-4095/8181
≈ 0（整数除法） so：ecd_count无变化，丢失了本次旋转的计数！！！

2.正确计数

假如使用HALF_ECD_RANGE,无法区分高速正反转，导致编码器计数不准确，难控制精准距离，不妨换个思路。

代码如下（示例）：

void Motor_Angle_Cal(unsigned short int motor_num,float T)
{
	float  res1, res2;
	static float pos[MOTOR_MAX], pos_old[MOTOR_MAX];//
	
	if(cnt==1)
	{
		pos_old[motor_num]=MOTOR_FEEDBACK[motor_num];
		cnt=0;
	}	
	pos[motor_num] =MOTOR_FEEDBACK[motor_num];
	ANGLE[motor_num].eer=pos[motor_num] - pos_old[motor_num];
	
	if(ANGLE[motor_num].eer>0) 	
	{
		res1=ANGLE[motor_num].eer-T;//反转，自减
		res2=ANGLE[motor_num].eer;
	}
	else
	{
		res1=ANGLE[motor_num].eer+T;//正转，自加一个周期的角度值（360）
		res2=ANGLE[motor_num].eer;
	}
	
		
	if(ABS(res1)<ABS(res2)) //不管正反转，肯定是转的角度小的那个是真的
	{
		ANGLE[motor_num].eer_eer = res1;
	}
	else
	{
		ANGLE[motor_num].eer_eer = res2;
	}
	
	ANGLE[motor_num].POS_ABS += ANGLE[motor_num].eer_eer;

	if (x==1)
	{
		ANGLE[motor_num].First_POS_ABS = ANGLE[motor_num].POS_ABS;
		x=0;
	}

	pos_old[motor_num]  = pos[motor_num];
}

其中，err表示两次采样间编码器的原始差值，可能包含多圈跳变。eer > 0，可能是 正向旋转接近周期终点（如从8000→100，真实差值为-7900，但计算为100-8000=-7900，应修正为 -7900 + 8191 = +291）。若 eer < 0，可能是 反向旋转跨越零点（如从100→8000，真实差值为+7900，但计算为8000-100=+7900，应修正为 7900 - 8191 = -291）。然后选择最小变化量，最后更新绝对角度。得到的POS_ABS为多圈绝对角度，控制电机转动距离。此算法通过周期改正和最小变化量的选择，解决了编码器环形溢出问题，保证计数准确。

注:煮播还是名刚大二的小小白，如果有不准确或者有问题的说法请包涵和指正。

---