stm32pid寻迹代码

以下是一个基于STM32的PID寻迹代码示例：

#include "stm32f10x.h"
#include "motor.h"
#include "delay.h"

#define Kp 0.8f       // 比例系数
#define Ki 0.05f      // 积分系数
#define Kd 0.2f       // 微分系数

#define SPEED 800    // 左右电机的初始速度

#define LFP GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_0)  // 左前红外传感器
#define LBP GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_1)  // 左后红外传感器
#define RFP GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_2)  // 右前红外传感器
#define RBP GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_3)  // 右后红外传感器

float P_error,I_error,D_error;
float last_error=0;
float speed_left,speed_right;

void PID(float error)
{
    P_error=error;
    I_error+=error;
    D_error=error-last_error;
    last_error=error;

    speed_left=SPEED+Kp*P_error+Ki*I_error+Kd*D_error;
    speed_right=SPEED-Kp*P_error-Ki*I_error-Kd*D_error;

    if(speed_left>1000)speed_left=1000;  // 限制速度
    if(speed_right>1000)speed_right=1000;

    if(speed_left<0)speed_left=0;
    if(speed_right<0)speed_right=0;

    Motor_Left(speed_left);
    Motor_Right(speed_right);
}

void Tracing(void)
{
    if(LFP==0&&LBP==0&&RFP==0&&RBP==0)  // 四路红外均检测到黑线，小车停止
    {
        Motor_Stop();
        return;
    }

    if(LFP==0&&LBP==0&&RFP==1&&RBP==0)  // 直线
    {
        PID(0);
    }

    if(LFP==0&&LBP==1)  // 向左偏
    {
        PID(-1);
    }

    if(RFP==0&&RBP==1)  // 向右偏
    {
        PID(1);
    }

    if(LFP==1&&LBP==1&&RFP==0&&RBP==0)  // 左侧检测到黑线，小车左转
    {
        Motor_Left(0);
        Motor_Right(SPEED);
    }

    if(LFP==0&&LBP==0&&RFP==1&&RBP==1)  // 右侧检测到黑线，小车右转
    {
        Motor_Left(SPEED);
        Motor_Right(0);
    }
}

int main(void)
{
    Motor_Init();
    delay_init();
    while(1)
    {
        Tracing();
        delay_ms(5);  // 延时等待传感器稳定
    }
}

以上代码中，通过四路红外传感器检测黑线位置，根据偏差通过PID算法控制左右电机的转速，实现小车的寻迹行驶。其中，`Motor_Left`和`Motor_Right`为控制电机转速的函数。