基于stm32f10的车机闭环控制

原理图

电机驱动接口

一共分为输入和输出端口

输入一共有两组，分为ab两组，pwm控制转速，ain2和ain1的高低电平控制转向，stby控制驱动是否工作
输出也有两组，编码器a相和b相

PWMA->PB10
PWMB->PB11
AIN1->B4
AIN2->B3
BIN1->B5
BIN2->B6

电机接口

编码器a相和b相输入到单片机进行计算，计算出转速和转向，连接有外部中断功能的io口
BO1,BO2为电机的驱动信号输入，还有两个为稳压模块供电，电机5v和gnd接反，编码器就不会工作

AO_A->PA0
AO_B->PA1
BO_A->PA6
BO_B->PA7

开环控制

pwm波控制车速

Cube初始化配置

这里是我自己设置的，如果pwm设置成别的pid的数据需要自己自行调试

选择模式和初始化

预分频器（PSC） 和 自动重载寄存器（ARR）

Prescaler（预分频器）设置为定时器时钟频率

$$实际计数频率=时钟源频率/(Prescaler+1)$$

Counter Period（计数周期，ARR - Auto Reload Register）

$$PWM周期=(ARR+1)/计数频率$$

$$占空比=Pulse/(ARR+1)\ast 100$$

初始占空比

设置的步骤

步骤1：确定定时器时钟频率 1. ⾸先确定定时器的时钟源频率（通常是APB1或APB2时钟频率） 例如，如果APB1时钟频率是72MHz，那么定时器时钟频率通常也是72MHz（除⾮有 额外的分频设置）。

步骤2：设定PWM频率 1. 根据需要的PWM频率，确定预分频器（Prescaler）和⾃动重装寄存器（ARR）的值 例如，如果您需要⽣成1kHz的PWM信号，且定时器时钟频率为72MHz：

选择合适的Prescaler和ARR值。例如，您可以选择：

Prescaler = 7199（将时钟频率分频为10kHz）

ARR = 999（使PWM频率为1kHz）

步骤3：设定PWM占空⽐ 1. 确定所需的占空⽐（例如50%），然后计算对应的CCR值 假设ARR = 999，那么对应的50%占空⽐：

步骤4：在CubeMX中配置

1. 打开CubeMX并选择定时器：选择⽤于PWM的定时器（如TIMx）并选择PWM输出引脚。

2. 配置定时器参数

   Prescaler：设置为72。

   Counter Period (ARR)：设置为999。

   Pulse (CCR)：设置为500（⽤于⽣成50%的占空⽐）。

3. ⽣成代码并初始化：在⽣成的代码中，定时器的初始化代码会⾃动使⽤这些配置

定时器时钟频率 f_timer 为 1 MHz（每秒 1000000 次）。

设置 ARR 为 999，意味着定时器周期为 1000 个时钟周期，即 PWM 信号的周期为 1 毫秒（1 kHz 的频率）。

假设 CCR 设置为 500，那么高电平持续的时间为 500 个时钟周期。

初始化

  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);//开启定时器2 通道3 PWM模式
  
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);//开启定时器2 通道3 PWM模式

宏来修改占空比

__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 40);//修改定时器2 通道3为40占空比

gpio口控制方向

Cube初始化配置

改标签

代码

宏定义

#define Amplitude    95      /* PWM满幅是100 */
#define Dead_Voltage 2070      /* 死区电压 */
#define PWMA TIM2->CCR3         /* PB10 */
#define PWMB TIM2->CCR4         /* PB11 */

控制电机进行正转、反转、停止

/**
 *    @brief 控制电机进行正转、反转、停止
 *    @param None
 *    @retval None
 */
void LeftMotor_Go() //左电机正转 AIN输出相反电平  BIN也输出相反电平
{
	HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_SET);
	HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
}
void LeftMotor_Back()  //左电机反转
{
	HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_SET);
}
void LeftMotor_Stop()  //左电机停止 AIN和BIN输出相同电平
{
	HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
}
void RightMotor_Go() //右电机正转 AIN输出相反电平  BIN也输出相反电平
{
	HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_SET);
	HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);

}
void RightMotor_Back()  //右电机反转
{
	HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_SET);
}
void RightMotor_Stop()  //右电机停止 AIN和BIN输出相同电平
{
	HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
}

滤波

float LimitPWM(float data, float max)
{
    // 检查最大值是否为有效值
    if (max <= 0) 
	{
        return 0; // 如果max不合理，返回0作为错误处理
    }

    // 限制data值在[-max, max]之间
    if (data < -max) 
	{
        return -max; // 如果data小于-max，返回-max
    }
	else if (data > max) 
	{
        return max; // 如果data大于max，返回max
    }

    return data; // 如果data在范围内，返回原始data
}

分别控制左右电机速度

// 控制左电机
void ControlLeftMotor(int leftMotorPWM)
{
    // 控制左电机的方向
    if (leftMotorPWM > 0)
    {  
        // 如果左电机PWM是正值，逆时针转
        LeftMotor_Back();  
    }
    else
    {  
        // 如果左电机PWM是负值，顺时针转动
        LeftMotor_Go();  
    }

    leftMotorPWM = abs(leftMotorPWM);  // 取绝对值，忽略死区电压
    PWMA = LimitPWM(leftMotorPWM, Amplitude);  // 设置左电机PWM
}

// 控制右电机
void ControlRightMotor(int rightMotorPWM)
{
    // 控制右电机的方向
    if (rightMotorPWM > 0)
    {  
        // 如果右电机PWM是正值，逆时针转动
        RightMotor_Go();  
    } 
    else
    {
        // 如果右电机PWM是负值，顺时针转动
        RightMotor_Back();
    }

    rightMotorPWM = abs(rightMotorPWM);  // 取绝对值，忽略死区电压
    PWMB = LimitPWM(rightMotorPWM, Amplitude);  // 设置右电机PWM
}

总的控制函数

用来控制左右电机的前进与后退，方便后面进行原地转向

/**
 *    @brief 控制电机进行速度控制
 *    @param 
*            对于单个轮子来说输入的pwm为正则向前走，为负则向后走
			 leftMotorPWM：左电机的 PWM 值，用于控制左电机的速度，
			 rightMotorPWM：右电机的 PWM 值，用于控制右电机的速度。
 *    @retval None
 */
void MotorControl(int leftMotorPWM, int rightMotorPWM)
{
    ControlLeftMotor(leftMotorPWM);  // 控制左电机
    ControlRightMotor(rightMotorPWM);  // 控制右电机
}

读取脉冲数

通常情况下编码器旋转一周会输出固定的脉冲数，即编码器的分辨率，通过测量固定时间T内编码器输出的脉冲数即可求得电机的转速。

假设编码器的分辨率为 ( P )（单位：脉冲/转），即编码器在电机转动一周时会输出 ( P ) 个脉冲。在 ( T ) 时间内测得的脉冲数为 ( m ) 个。要计算电机的转速（单位：转/分钟），我们可以使用以下步骤：

1. 首先，计算电机在 ( T ) 秒内转动的圈数。由于每转动一圈编码器产生 ( P ) 个脉冲，( m ) 个脉冲对应的圈数 ( n ) 为：

   $$n=\frac{m}{P}$$

2. 然后，将 ( n ) 圈转换为每分钟的转数。已知 ( n ) 圈是在 ( T ) 秒内完成的，所以每秒的转数为
   $$(\frac{n}{T})$$
   每分钟的转数为：
   $$RPM=\frac{n}{T}\times 60=\frac{m}{P}\times \frac{60}{T}$$

因此，电机的转速 ( RPM )（单位：转/分钟）可以用公式表示为：
$$RPM=\frac{60\times m}{P\times T\times 4}\times R$$
这里：

• ( m ) 是在时间 ( T ) 内测得的脉冲数
• ( P ) 是编码器的分辨率
• ( R ) radio为减速比
• ( T ) 是测量时间，单位为秒（注意转换为分钟需要乘以 60）

通过这个公式，你可以计算出电机的转速，只需要将 ( m )，( P )，( R )和 ( T ) 的值代入即可。

外部中断读取脉冲数

cube配置（外部中断模式）

更节省资源

改标签，设置外部中断模式为上升沿和下降沿都触发

开启全局中断

代码

int pulse_numA;
int pulse_numB;
// 外部中断回调函数
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    // 判断是否为GPIO_PIN_0中断，即A相编码器脉冲输入
    if(GPIO_Pin == GPIO_PIN_0) 
    {
        // A相信号下降沿
        if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0) == 0) 
        {
            // 如果B相信号也为低，表示反向旋转
            if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_1) == 0)
                pulse_numB--;
            else  // 否则为正向旋转
                pulse_numB++;
        }
        // A相信号上升沿
        else 
        {
            // 如果B相信号为低，则为正向旋转
            if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_1) == 0)
                pulse_numB++;
            else  // 否则为反向旋转
                pulse_numB--;
        }
    }

    // 判断是否为GPIO_PIN_1中断，即B相编码器脉冲输入
    else if(GPIO_Pin == GPIO_PIN_1) 
    {
        // B相信号下降沿
        if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_1) == 0) 
        {
            // 如果A相信号为低，则为正向旋转
            if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0) == 0)
                pulse_numB++;
            else  // 否则为反向旋转
                pulse_numB--;
        }
        // B相信号上升沿
        else 
        {
            // 如果A相信号为低，则为反向旋转
            if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0) == 0)
                pulse_numB--;
            else  // 否则为正向旋转
                pulse_numB++;
        }
    }

    // 判断是否为GPIO_PIN_6中断，处理另一组编码器输入（如果存在）
    if(GPIO_Pin == GPIO_PIN_6) 
    {
        // A相信号下降沿
        if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_6) == 0) 
        {
            // 如果B相信号为低，则为反向旋转
            if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_7) == 0)
                pulse_numA--;
            else  // 否则为正向旋转
                pulse_numA++;
        }
        // A相信号上升沿
        else 
        {
            // 如果B相信号为低，则为正向旋转
            if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_7) == 0)
                pulse_numA++;
            else  // 否则为反向旋转
                pulse_numA--;
        }
    }

    // 判断是否为GPIO_PIN_7中断，处理第二组B相编码器输入
    else if(GPIO_Pin == GPIO_PIN_7) 
    {
        // B相信号下降沿
        if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_7) == 0) 
        {
            // 如果A相信号为低，则为正向旋转
            if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_6) == 0)
                pulse_numA++;
            else  // 否则为反向旋转
                pulse_numA--;
        }
        // B相信号上升沿
        else 
        {
            // 如果A相信号为低，则为反向旋转
            if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_6) == 0)
                pulse_numA--;
            else  // 否则为正向旋转
                pulse_numA++;
        }
    }
}

定时器中断间隔读取脉冲数

定时器中断读取脉冲

用一个没有配置的定时器滤波，打开定时器全局中断

• 定时时间=（Prescaler +1）* （Counter Period +1）/ 晶振频率（72Mhz）10ms读取一次

代码

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if(htim == &htim1)//htim1 500HZ  2ms 中断一次
	{
		TimerCount++;
		if(TimerCount %5 == 0)//每10ms执行一次
		{
			encountA=pulse_numA;
			encountB=-pulse_numB;
			rpma=(float)encountA*100/4321*60;
			rpmb=(float)encountB*100/4321*60;
			pulse_numA=0;
			pulse_numB=0;//清零
		}
		
	}
}

速度计算为1秒读取的脉冲数/减速比/编码器线速/4

PID控制

原理

在我的应用中，PWM值限制在 0 到 100 之间

了解 PID 控制原理

• Kp (比例增益)：比例增益主要影响系统对误差的反应。当误差较大时，Kp 会产生一个较大的控制信号。适当增大 Kp 可以提高系统响应速度，但过大可能导致系统过冲和振荡。
• Ki (积分增益)：积分增益通过累积历史误差来修正系统长期存在的小误差。如果系统存在持续的偏差（例如，目标值和实际值的差异），Ki 会逐渐增大控制信号，直到误差消失。过大的 Ki 可能导致系统振荡和不稳定。
• Kd (微分增益)：微分增益根据误差变化速率做出反应，帮助系统减小过冲并提高系统的稳定性。Kd 对快速变化的误差敏感，可以抑制系统的振荡。

调节 PID 参数的基本原则

调节PID参数是一项迭代的过程，通常需要进行反复实验和微调。

调节 Kp (比例增益)

• 目标：让系统响应目标值的变化，不要过快或过慢。
• 方法：从一个小的 Kp 值开始，逐步增大，直到系统对误差的响应足够快，但不会导致输出过大或过冲。
• 注意：如果 Kp 太大，系统可能会发生振荡或过冲，导致输出值超出 0 到 100 的范围。

调节 Ki (积分增益)

• 目标：消除长期的小误差，使系统更精确地达到目标值。
• 方法：从 Ki = 0 开始，逐渐增大。Ki 的作用是消除由比例控制器无法解决的稳定误差（例如，常见的偏差）。增加 Ki 可以提高系统的精度，但过大可能导致振荡。
• 注意：如果 Ki 设置过大，可能导致误差累积过快，造成控制输出过大，甚至产生不稳定的振荡。

调节 Kd (微分增益)

• 目标：减少系统的过冲和振荡。
• 方法：Kd 控制误差变化的速率，它可以减缓过度的响应，帮助减少系统振荡。首先将 Kd 设置为零，然后逐步增大，直到系统反应平稳且没有过度震荡。
• 注意：如果 Kd 设置过大，系统响应可能会变得过于迟缓，或者产生不必要的抑制。

使用 PID 调参技巧

调参通常是一个渐进的过程，下面是几种常见的调参技巧：

Ziegler-Nichols 调参法（经验法）

这是最常用的调参方法之一，基于通过实验找到的经验公式。以下是基本步骤：

  1. 将 `Ki` 和 `Kd` 设置为 0。
  2. 增大 `Kp`，直到系统开始稳定振荡（输出值开始周期性波动）。
  3. 记录下此时的 `Kp` 值，这个值被称为临界增益 `Kc`。
  4. 记录下此时的振荡周期 `P`。
  5. 使用以下公式来计算 `Ki` 和 `Kd`：
     - `Kp = 0.6 * Kc`
     - `Ki = 2 * Kp / P`
     - `Kd = Kp * P / 8`

逐步调整法

逐步调整每个参数，从 Kp 开始，调整到合适值后再调整 Ki，最后调整 Kd。这种方法适合有经验的调参人员，可以灵活调整。

实际应用中的调参

在你的情况下，PWM 输出范围是 0 到 100，调参时你需要特别注意以下几点：

• 限制输出范围：确保控制输出（pid->actual_val）在 0 到 100 之间。例如，如果 PID 计算得到的值超出了这个范围，可以通过如下代码进行限制：

  if (pid->actual_val > 100) pid->actual_val = 100;
  if (pid->actual_val < 0) pid->actual_val = 0;
  

• 避免过冲和震荡：设置 Kp 时要注意不要让系统产生过大的响应，这样可能会导致输出超出 0 到 100 的范围，或者产生较大的振荡。

• 逐步增加 Ki：积分项有助于消除长期误差，但如果积分项太大，可能会导致控制器输出过度修正，从而导致输出溢出（大于 100）。因此，要小心增大 Ki 的值。

• 微调 Kd：适当的 Kd 值可以帮助减少系统振荡，尤其是当误差变化较快时，Kd 有助于平滑输出。过高的 Kd 值可能会使控制器反应过于迟缓，因此要小心调整。

调试时观察和调整

• 在调试时，可以观察 PID 控制器的响应：
  • 如果系统响应过快并发生过冲，减小 Kp 或增加 Kd。
  • 如果系统反应迟缓且无法跟踪目标，增加 Kp。
  • 如果长期存在偏差，增加 Ki。

最终的 PID 参数需要通过实验来确定，不同的系统和不同的负载可能需要不同的参数设置。调节过程中，可能需要反复进行试验，微调各个参数，直到系统达到稳定、快速响应的目标。

转速，转数和脉冲数的转化函数

转速对应编码器脉冲数

增量式控制

/**************************************************************************
功    能: 计算转速对应编码器脉冲数
输    入: rpm：转速；ppr：码盘数；ratio：减速比；cnt_time：计数时间(ms)
返 回 值: 电机脉冲数 
**************************************************************************/
long Rpm_Encoder_Cnt(float rpm,uint16_t ppr,uint16_t ratio,uint16_t cnt_time)
{
    return (rpm*ratio*ppr*4)/(60*1000/cnt_time);            /* 4倍频 */       
}

转数对应编码器脉冲数

用于位置环控制

/**************************************************************************
功    能: 计算转数对应编码器脉冲数
输    入: num：转数；ppr：码盘数；ratio：减速比
返 回 值: 电机脉冲数 
**************************************************************************/
long Num_Encoder_Cnt(float num,uint16_t ppr,float ratio)
{
    return (num*ratio*ppr*4);                               /* 4倍频 */
}

长度对应编码器脉冲数

通常用来控制小车差速进行自转

/**************************************************************************
功    能: 计算所走长度对应编码器脉冲数
输    入: Length：所走长度；ppr：码盘数；ratio：减速比
返 回 值: 电机脉冲数 
**************************************************************************/
long Length_Encoder_Cnt(float Length,uint16_t ppr,float ratio)
{
	float num=Length/20.4;//厘米为单位，20.4为车轮周长
    return (num*ratio*ppr*4);                               /* 4倍频 */
}
//4分之1圆周为11.10775

定义基础变量

//声明一个结构体类型
typedef struct 
{
	float target_val;//目标值
	float Kp,Ki,Kd;//比例，积分，微分系数
	
} tPid;

初始化

void PID_init(void)
{
	//速度环
	pidMotor1Speed.target_val=0;
	pidMotor1Speed.Kp=5;
	pidMotor1Speed.Ki=0.11;
	pidMotor1Speed.Kd=0;
	pidMotor2Speed.target_val=0;
	pidMotor2Speed.Kp=5;
	pidMotor2Speed.Ki=0.11;
	pidMotor2Speed.Kd=0;
	pidMotor1PositionSpeed.target_val=0;
	pidMotor1PositionSpeed.Kp=5;
	pidMotor1PositionSpeed.Ki=0.11;
	pidMotor1PositionSpeed.Kd=0;
	pidMotor2PositionSpeed.target_val=0;
	pidMotor2PositionSpeed.Kp=5;
	pidMotor2PositionSpeed.Ki=0.11;
	pidMotor2PositionSpeed.Kd=0;
	//位置环
	pidMotor1Position.target_val=0;
	pidMotor1Position.Kp=0.18;
	pidMotor1Position.Ki=0.002;
	pidMotor1Position.Kd=0;
	pidMotor2Position.target_val=0;
	pidMotor2Position.Kp=0.18;
	pidMotor2Position.Ki=0.002;
	pidMotor2Position.Kd=0;
	//巡线环
	pidOpenmv_Tracking.target_val=0;
	pidOpenmv_Tracking.Kp=3;
	pidOpenmv_Tracking.Ki=0;
	pidOpenmv_Tracking.Kd=0.12;
}

增量式

通过这个函数改变目标值，增量式的目标值的输入是转速

void PID_control(float rpm1,float rpm2)
{
	//pid控制电机速度
	//pidMotor1Speed.target_val=rpma;
	pidMotor1Speed.target_val=rpm1*4321/6000;
	pidMotor2Speed.target_val=rpm2*4321/6000;
	//pid控制电机速度
	//Rpm_Encoder_Cnt(rpmb,11,98,10);
}

float Incremental_PID(tPid* pid,int actual_val)//传入目标值，实际值
{
		static float Bias,Pwm,Last_bias,next_bias;//bias=Err
		// static float Integral_bias;
		Bias=pid->target_val-actual_val;//计算偏差
		// Integral_bias+=Bias;
		// Integral_bias=LimitPWM(Integral_bias,5000);
		Pwm+=(pid->Kp*(Bias-Last_bias)+pid->Ki*Bias+pid->Kd*(Bias+next_bias-2*Last_bias));//增量式PI控制器
		next_bias = Last_bias;
		Last_bias=Bias;
		if(Pwm>99)Pwm=99;
		if(Pwm<-99)Pwm=-99;
		return Pwm;
}

在定时器中断回调函数进行速度修正调整

  if (abs(encountA)<=5&&pidMotor1Speed.target_val == 0) 
    PWMA = 0;
    else
    {
            motorA = Incremental_PID(&pidMotor1Speed,encountA); /* 增量PID控制器 */
            ControlLeftMotor(motorA);
    }

    if (abs(encountB)<=5&&pidMotor2Speed.target_val == 0) 
    PWMB = 0;
    else
    {
            motorB = Incremental_PID(&pidMotor2Speed,encountB); /* 增量PID控制器 */   
            ControlRightMotor(motorB);
    }

位置式

通过这个函数改变目标值，增量式的目标值的输入是转数

void PID_control_position(int rnuma,int rnumb)
{
	//pid控制电机位置
	pidMotor1Position.target_val=(float)Num_Encoder_Cnt(rnuma,11,98);
	pidMotor2Position.target_val=(float)Num_Encoder_Cnt(rnumb,11,98);
}

通过这个函数控制车子自转

void PID_turn_control(uint8_t direction)
{

	//pid控制转向
	if(direction==0)//左转90度
	{
			pidMotor1Position.target_val=Length_Encoder_Cnt(-11.10775,11,98);;
			pidMotor2Position.target_val=Length_Encoder_Cnt(11.10775,11,98);
	}
	else if(direction==1)//右转90度
	{
			pidMotor1Position.target_val=Length_Encoder_Cnt(11.10775,11,98);
			pidMotor2Position.target_val=Length_Encoder_Cnt(-11.10775,11,98);
	}
	else if(direction==2)//不控制转向
	{
			pidMotor1Position.target_val=0;
			pidMotor2Position.target_val=0;
	}

}

/*********************************************************************
函数功能：位置式PID控制器
入口参数：编码器测量位置信息，目标位置
返回  值：电机PWM
根据位置式离散PID公式 
pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  
∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2,,k;
pwm代表输出
********************************************************************/
int Position_PID (tPid*position,int actuel_position)
{ 	
	 Bias=position->target_val-actuel_position;//计算偏差
	 Integral_bias+=Bias;
	 Integral_bias=LimitPWM(Integral_bias,fabs(Target_Velocity));
	 Pwm=position->Kp*Bias+position->Ki*Integral_bias+position->Kd*(Bias-Last_Bias);//位置式PID控制器
	 Last_Bias=Bias;
	if(Pwm>10000)Pwm=10000;
	if(Pwm<-10000)Pwm=-10000;
	return Pwm;
}

串级pid控制

串级控制函数

位置环输出给速度环，调整速度环的目标速度

void PID_control_positionspeed(float rpm1,float rpm2)
{
	//pid控制电机速度
	//pidMotor1Speed.target_val=rpma;
	pidMotor1PositionSpeed.target_val=rpm1*4321/6000;
	pidMotor2PositionSpeed.target_val=rpm2*4321/6000;
	//pid控制电机速度
	//Rpm_Encoder_Cnt(rpmb,11,98,10);
}

在定时器中断回调函数进行速度修正调整

int motor_out=50;//串级pid控制限制的最大速度，巡线环控制的基础速度
static float motorA_position=0;
static float motorB_position=0;
if(pidMotor1Position.target_val!=0||pidMotor2Position.target_val!=0)//如果有目标值，则进行位置环控制
{
    encountA_sum+=encountA;//进行脉冲数累加
    encountB_sum+=encountB;
    motorA_position=Position_PID (&pidMotor1Position,encountA_sum);//位置环输出期望的速度
    motorA_position=LimitPWM(motorA_position,motor_out);//限制最大pwm值，限制输入给速度环的最大速度
    motorB_position=Position_PID (&pidMotor2Position,encountB_sum);//位置环输出期望的速度
    motorB_position=LimitPWM(motorB_position,motor_out);//限制最大pwm值，限制输入给速度环的最大速度
    PID_control_positionspeed(motorA_position,motorB_position);//位置环的输出值输入给速度环
    if(fabs((float)encountA_sum-pidMotor1Position.target_val)<10)             /* 滤除部分干扰 */
    {
        motorA_position=0;
        ControlLeftMotor(motorA_position);
        // encountA_sum=0;                                      /* 停止输出 */
    }
    else
    {
       motorA_position = Incremental_PID(&pidMotor1PositionSpeed,encountA);      /* 增量式速度控制 */
        ControlLeftMotor(motorA_position);                               /* 赋值 */
    }
    if(fabs((float)encountB_sum-pidMotor2Position.target_val)<10)             /* 滤除部分干扰 */
    {
        motorB_position=0;
        ControlRightMotor(motorB_position);
        // encountB_sum=0;                                       /* 停止输出 */
    }
    else
    {
        motorB_position = Incremental_PID(&pidMotor2PositionSpeed,encountB);      /* 增量式速度控制 */
        ControlRightMotor(motorB_position);                             /* 赋值 */
    }
    if(motorA_position==0&&motorB_position==0)
    {
        turn_flag=0;
        track_flag=0;
    }
}

巡线环控制

使用位置式pid

/*巡线状态检测及PID运算*/

float PID_Openmv_Tracking(int8_t ThisState,tPid *pid)
{
float error=ThisState;    //此次的误差
static float integral,last_error;    //积分累加项
float output;      //PID输出
integral += error;    //积分累加
    if (integral > 100) integral = 100;  // 防止积分项溢出
    if (integral < -100) integral = -100;
output = pid->Kp * error + pid->Ki * integral + pid->Kd * (error - last_error);
last_error = error;
return output;   //最后PID运算得到的是一个电机的差速调整值
}

通过位置给g_cThisState赋值

 if (leftArea2== 0 && leftArea1 == 0 && rightArea1 == 0&&rightArea2 == 0) 
		{
			// 中间检测到线，左右没有线，继续前进
			g_cThisState = 0;
		} 
		else if (leftArea2== 0 && leftArea1 == 1 && rightArea1 == 0&&rightArea2 == 0) 
		{
			// 中间和左侧检测到线，右侧没有线，可能偏离路线，继续前进或寻线
			g_cThisState = 1; // 根据需要可调整为其他状态，如停止
		} 
		else if (leftArea2== 1 && leftArea1 == 1 && rightArea1 == 0&&rightArea2 == 0) 
		{
			// 中间和右侧检测到线，左侧没有线，右转
			g_cThisState = 2;
		} 
		else if (leftArea2== 1 && leftArea1 == 0 && rightArea1 == 0&&rightArea2 == 0) 
		{
			// 左侧检测到线，右侧没有线，左转
			g_cThisState = 3;
		} 
		else if (leftArea2== 0 && leftArea1 == 0 && rightArea1 == 1&&rightArea2 == 0)
		{
			// 右侧检测到线，左侧没有线，右转
			g_cThisState = -1;
		}
		else if (leftArea2== 0 && leftArea1 == 0 && rightArea1 == 1&&rightArea2 == 1)
		{
			// 右侧检测到线，左侧没有线，右转
			g_cThisState = -2;
		}
		else if (leftArea2== 0 && leftArea1 == 0 && rightArea1 == 0&&rightArea2 == 1)
		{
			// 右侧检测到线，左侧没有线，右转
			g_cThisState = -3;
		}

在电机函数修正双轮差速

int motor_out=50;//串级pid控制限制的最大速度，巡线环控制的基础速度
void motor_proc(void)
{
    static float PWMA_out=0;
    static float PWMB_out=0;
    float fHW_PID_Out=PID_Openmv_Tracking(g_cThisState,&pidOpenmv_Tracking);
    PWMA_out=LimitPWM((motor_out-fHW_PID_Out),70);
    PWMB_out=LimitPWM((motor_out+fHW_PID_Out),70);
    PID_control(PWMA_out,PWMB_out);//速度环控制
}