TH小车A2机械臂-STM32智能小车红外巡黑线

第一章 小车组装与建模

1.1功能分析

小组选题为智能小车红外巡黑线TH小车A2机械臂（XR-TH)-STM32，小车板上搭载两个红外传感器，通过左右传感器感应黑线位置实现左转或右转，通过车载WIFI模块，可实现手机软件操作小车运动，机械臂运动以及摄像头转动，可以通过软件进行速度调节功能。

1.2 组装

小车包括：主体零件，STM32主板，WIFI模块，红外传感器，超声波模块，摄像头，机械臂。

STM32主板：通过程序烧录，使小车具备自动巡线，远程遥控功能

WIFI模块：使小车可以通过WIFI连接通过底部表示编号进行远程遥控

红外传感器：识别行走路线，传输给主板

摄像头：传输图像数据进行远程遥控

图1.2.1 装配实物图

1.3问题与创新

在组装过程中，我们遇到了一些挑战，例如确保红外传感器的精确度和稳定性。为了解决这些问题，进行了多次测试和调整，通过改进传感器的安装位置和角度，提高了小车的性能。

在创新方面，我们小组引入了机械臂和摄像头的集成，这不仅增加了小车的功能性，也提高了其实用价值。机械臂可以执行简单的抓取和搬运任务，而摄像头则可以提供实时视频反馈，使操作者能够更直观地了解小车周围的环境。此外，通过使用手机应用程序，可以轻松地控制小车和机械臂的动作，以及调整小车的速度。

1.4零件图与三维建模

小组零件图使用CAD建模，三维建模使用SolidWorks，确保了设计的精确性和零件的兼容性。通过这些高精度的模型，我们能够详细分析每个零件的尺寸和功能，以及它们在整体结构中的作用。此外，三维建模还允许我们在实际制造之前进行虚拟装配，这有助于发现潜在的设计问题并提前解决，从而节省时间和成本。在设计过程中，我们还考虑了零件的可制造性，确保设计不仅在理论上可行，而且在实际生产中也是高效的。

1.4.1 SolidWorks软件介绍

SolidWorks是首个基于Windows的三维CAD系统，自1995年起屡获国际大奖，包括连续四年获得CADENCE的最佳编辑奖。该软件以其易用性、稳定性和创新性著称，显著缩短了设计周期并提高了产品上市速度。它集成了Windows OLE技术、直观设计、parasolid内核和第三方软件兼容性，成为全球最受欢迎的CAD软件之一。SolidWorks功能丰富，易于学习和使用，提供了多种设计方案，减少了设计错误，提升了产品质量。其直观的操作和强大的设计功能使得产品设计过程全可编辑，零件、装配和工程图之间完全相关。在设计小车时，SolidWorks帮助工程师通过测量尺寸、绘制零件、设计金属底板和组装零件，以及使用扫描、阵列、旋转和拔模等功能来缩短制图时间，并通过对称镜像提高效率。

1.4.2零件图

图1.4.1 前盖板

图1.4.2 后板

图1.4.3 电池板

图1.4.4 前板

图1.4.5 AutoCAD爆炸图

图1.4.6 装配图三视图

1.4.3 零件表

序号	名称	数量
1	Wi-Fi天线	1
2	Wi-Fi模块	1
3	4自由度机械臂	1
4	车轮	10
5	驱动齿轮	2
6	履带	2
7	前板	1
8	电池板	1
9	后板	1
10	3.0螺钉	10
11	电机	2
12	主体	1
13	1.2螺母	26
14	1.2螺钉	26
15	前盖板	1
16	摄像头	1
17	超声波传感器	2
18	红外传感器	2
19	1.5螺钉	20

1.4.4 SolidWorks零件图

图1.4.7 前盖板

图1.4.8 前板

图2.4. 9 后板

图1.4.10小车主体

1.4.5 SoildWorks装配图

在SolidWorks软件中进行小车的装配，需按照模块化和逐步装配的原则，以确保装配过程的高效与准确。  

首先，以主体（12）作为装配的核心参考，利用SolidWorks中的配合功能，将前板（7）和后板（9）分别通过平面配合和孔轴配合与主体固定，确保零件间的对齐和稳固。前盖板（15）则通过顶部的螺孔与主体结合，需注意板件的方向和孔位的一致性，以避免干涉问题。  

接着，进行履带驱动系统的装配。在主体两侧分别安装车轮（4），使用SolidWorks的轴心配合确保车轮与支架的同轴性。将驱动齿轮（5）与电机（11）的轴端进行配合，调整齿轮的位置以确保与履带（6）的啮合良好。随后，将履带安装到车轮和驱动齿轮的外侧，利用路径生成工具确认其运行轨迹是否顺畅，并调整松紧程度。  

在完成驱动系统后，安装供电模块。电池板（8）通过下方的固定孔与主体底部结合，需要确保板件贴合紧密，以避免运行中松动。  

随后，安装感知模块。在前板顶部预留的接口处固定摄像头（16），通过角度调节功能调整其方向，确保前方视野清晰。超声波传感器（17）安装于摄像头下方，通过距离约束保持其位置。红外传感器（18）则位于前盖板底部，利用高度配合功能设置其与地面的距离，以保证路径检测的准确性。  

最后，完成Wi-Fi模块和机械臂的装配。将Wi-Fi模块（2）和天线（1）固定在主体下部，通过空间配合功能调整位置，确保信号传输的稳定性。机械臂（3）安装在前顶板上，使用多自由度旋转约束功能验证各关节的灵活性，并检查运动范围是否受到其他组件的干涉。  

装配完成后，通过SolidWorks的干涉检查工具逐一检测零件间的配合情况，确保没有干涉问题存在。最后，利用运动仿真功能测试履带的运行和机械臂的运动，以验证装配的准确性和小车的功能性。此装配过程充分利用了SolidWorks的约束和仿真功能，使得装配结果既符合设计要求，又便于后续的优化与调整。

图1.4.11 Solidworks装配图

•  第二章 硬件设计与搭接

2.1 软件平台介绍

Proteus软件是英国Lab Center Electronics公司出版的EDA工具软件。它将电路仿真软件、PCB设计软件和虚拟模型仿真软件三合一，为用户提供了从概念到产品的完整设计平台。以下是关于Proteus平台的介绍：

2.1.1 主要功能与特点：

原理图布图：支持电路原理图的绘制，方便用户进行电路设计和布局。

PCB自动或人工布线：提供PCB设计功能，支持自动布线和手动布线，帮助用户完成PCB的布局和设计。

SPICE电路仿真：集成了SPICE仿真引擎，可以对电路进行精确的仿真分析，包括模拟电路、数字电路和混合信号电路。

单片机与外围电路协同仿真：支持多种单片机模型的仿真，包括8051、HC11、PIC10/12/16/18/24/30/DsPIC33、AVR、ARM、8086和MSP430等，以及Cortex和DSP系列处理器。用户可以在原理图上直接编程，并实时查看仿真结果。

2.1.2 应用领域：

电子电路设计：适用于各种电子电路的设计，包括模拟电路、数字电路和混合信号电路。

嵌入式系统开发：支持单片机及外围器件的仿真，广泛应用于嵌入式系统开发。

PCB设计与布局：提供PCB设计功能，支持多层板设计，帮助用户完成PCB的布局和设计。

综上所述，Proteus是一款功能全面、易于使用的EDA工具软件，广泛适用于电子设计、教学和研究等领域。

专业设计是机电专业教育中的一个重要环节，它不仅是对学生所学知识的一次综合运用，也是对学生解决实际问题能力的一次全面检验。

通过专业设计，学生可以在真实或模拟的工作环境中，运用所学的理论知识和技能，解决具体的工程问题。这一过程有助于培养学生的工程实践能力和创新思维，同时也是学生从理论学习向实际操作过渡的重要阶段。专业设计的质量直接关系到学生能否熟练该专业的知识，以及未来在职业发展中的竞争力。

2.2 硬件选择设计思路

2.2.1 核心控制器：STM32F103R6

本项目采用STM32F103R6微控制器作为核心控制器，负责小车的总体控制与协调功能。其主要任务包括处理小车的寻迹功能、避障功能以及远程控制模式之间的切换。此外，核心控制器还控制串口通信模块，用以模拟蓝牙功能，支持远程数据传输和指令接收。

图 2.2.1  STM32F103R6

    

2.2.2 显示系统

小车配备了液晶显示屏，用于实时显示当前运行模式（寻迹模式或远程控制模式）以及红外传感器的状态。显示系统还设置了定位器，用于调节液晶屏的清晰度，确保运行信息的直观呈现。

图 2.2.2 LM016L显示器

2.2.3 模式切换

通过按键实现小车在以下三种状态间切换：

寻迹模式：利用红外传感器实现路径跟随。

远程控制模式：通过串口通信实现模拟蓝牙功能，用户远程操控小车。

图 2.2.3模式切换按钮

2.2.4 红外系统

小车的红外系统由左右两侧的红外传感器组成，主要功能是检测地面的黑线以实现路径跟随功能。通过优化传感器安装位置与灵敏度设置，确保小车能够高效识别路径并做出精确调整。

图2.2.4 双侧后驱动轮和红外传感器

2.2.5 电机驱动

小车采用后轮驱动的设计，左右两侧的驱动轮由电机带动，并由L298电机驱动模块控制。模块的主要功能包括：控制后轮电机的正转与反转。实现小车的灵活转向与精确运动控制。

图2.2.5 L298电机驱动模块

2.2.6 串口通信

串口通信模块用于实现数据传输与指令接收，主要功能是模拟蓝牙通信。在远程控制模式下，小车通过串口接收用户指令并作出实时响应，同时反馈小车的运行状态，增强用户体验。

图2.2.6 蓝牙模块

2.2.7 寻迹功能

寻迹功能依托左右两侧的红外传感器来实现。红外传感器能够实时感应路径变化，并通过开关连接模拟不同路径信号，从而测试和验证小车的寻迹能力。在路径偏离时，小车通过核心控制器快速调整运动方向，确保精确跟随预定轨迹。

2.2.8系统功能简述

本系统具备多种实用功能，能够有效满足小车的多场景应用需求。首先，通过红外传感器实现路径跟踪功能。红外传感器能够实时感应地面黑线，并根据路径变化自动调整行进方向，确保小车能够精准跟随既定轨迹，从而实现路径跟踪的高效性与可靠性。

其次，系统支持灵活的模式切换功能。用户可以通过简单的按键操作，在寻迹模式、远程控制模式等多种运行模式之间快速切换。这种设计不仅提升了系统的便捷性，还使小车能够适应不同的应用场景需求，扩展了其功能范围。

此外，小车配备了液晶显示屏，能够实时反馈当前运行模式和红外传感器的工作状态。用户可以直观地通过屏幕了解小车的运行信息，从而方便对系统进行监控和调整，提高操作体验。

最后，系统采用高效的电机驱动控制方案，使用L298电机驱动模块实现后轮的正转、反转以及灵活转向。该设计不仅支持小车的精准运动控制，还能满足多种复杂运动方式的需求，为小车的灵活性提供了可靠保障。

图2.2.7 模拟仿真连线

2.3 设计电路及绘制电路仿真

根据基本功能设计思路，设计了硬件电路。并用Proteus绘制电路原理图，并进行了仿真模拟。

2.3.1电路功能概述

电路主要功能是通过红外传感器检测路径上的黑白区域，从而判断小车的行驶方向，实现自动巡线。同时，还可以通过模式选择开关切换至手动模式，利用蓝牙模块远程控制小车。此外，液晶屏实时显示小车的工作状态。

由于Proteus软件不能完整体现红外传感器的功能，这里用两个开关反应红外传感器的检测状态，开关合并时，红外传感器状态为未检测到黑线。开关未闭合时，红外传感器状态为检测到黑线。

2.3.2 引脚连接

PA0 和 PA7：接收红外传感器的输入信号，判断路径状态（黑线或白线）。

PB5、PB7、PB9、PB11：控制 L298 电机驱动芯片，调节左右电机的方向和转速。

PA10 和 PA9：与蓝牙模块通信，用于接收远程指令。

PB15：接模式选择开关 K1，切换自动/手动控制模式。

图2.3.1 Proteus硬件接线图

2.3.3 巡线过程分析

巡线状态下，当两侧红外传感器都未检测到黑线时，图中两红外开关皆闭合，两电机执行正向运动，小车前进指令，如图2.3.2所示。

巡线状态下，当左侧红外传感器都检测到黑线时，图中左侧红外开关打开，右侧红外开关闭合，左电机停止，右电机正向运动，小车左转指令，如图2.3.3所示。

巡线状态下，当右侧红外传感器都检测到黑线时，图中右侧红外开关打开，左侧红外开关闭合，右电机停止，左电机正向运动，小车左转指令，如图2.3.4所示。

巡线状态下，当两侧红外传感器都检测到黑线时，图中两红外开关皆打开，两电机停止，小车运动停止，运动结束，如图2.3.5所示。

图2.3.2巡线两侧未检测到黑线仿真

图2.3.3巡线左侧检测到黑线仿真

图2.3.4巡线右侧检测到黑线仿真

图2.3.5巡线两侧检测都到黑线仿真

  第三章 软件编程与调试

3.1 开发环境

3.1.1 程序编写软件（Keil5）

Keil 5，全称 Keil µVision 5，是一款专为嵌入式系统开发设计的集成开发环境（IDE），广泛应用于基于ARM Cortex-M内核的微控制器开发。

选择Keil 5进行STM32程序编写的原因主要有以下几点：

1. 强大的功能集成：Keil 5集成了代码编辑、编译、调试和仿真功能，可以在一个环境中完成整个开发流程，大幅提升开发效率。  

2. 优秀的调试能力：支持源码级调试，能够通过设置断点、逐步执行代码等方式快速定位和解决问题，适用于嵌入式系统的复杂调试需求。  

3. 广泛的支持性：Keil 5内置对ARM Cortex-M内核的全面支持，并提供丰富的开发资源和示例代码，特别适合STM32系列单片机的开发。  

4. 高兼容性：支持多种硬件仿真器（如ST-Link），便于程序烧录与实时调试，进一步简化开发流程。

3.1.2 STM32的开发板与各功能版

本次设计的巡线小车总体结构如图3.1.１所示。

核心控制板为STM32单片机及其外围电路,‌STM32F103RCT6是一种32位嵌入式微控制器，采用ARM Cortex-M3内核，主频为72MHz，具有高性能和丰富的外设接口。‌ 其程序存储器容量为256KB，RAM容量为48KB，支持多种通信接口，如CAN、I2C、SPI、USART、USB等。‌

主要有4个模块组成：电源电路模块、电机电路模块、红外传感器电路模块、WIFI电路模块。电源模块采用AP117-3.3和AP117-5.0得到3.3V和5.0V的电源，为单片机和电机等供电；电机驱动芯片采用L298，电机为12V直流电机；左右红外传感器连接于PC13、PC14串口；WIFI电路为采用AR9331-AL1A芯片控制的复杂电路。

图3.1.1开发板结构图

图3.1.2功能版实物图

图3.1.3 STM32开发板实物图

图3.1.4Wi-Fi模块实物图

3.2 红外巡线功能

3.2.1红外传感器原理：

红外传感器前端包含一个红外发射头和一个红外接收头,通过尾部旋钮来调整红外发射强度（顺时针增强，逆时针减弱）。红外线照射到物体表面后发生反射，红外接收头在接受到反射回来的红外光信号后，会改变自身电阻，当反射线强度大到一定程度，电阻低于设定值，会触发比较器状态变化，输出低电平，在我们看来就是检测到障碍物了。

图3.2.1红外传感器原理图

障碍物距离越近，反射强度越强；

障碍物距离越远，反射强度越弱；

3.2.2巡线简单算法：

在车头安装俩个垂直向下的红外避障模块，作为探测黑线的传感器，当传感器返回的电平为 0 时，表明是正常地面，当传感器返回的电平是 1 时，表明是黑线。从而根据俩个红外探头的状态，来判断车头的状态是处于黑线的何种位置，再对车身进行方向校准，每一次程序循环都会进行一次方向校准，直至小车一致保持与黑线平行的状态进而传感器不会被触发。

图3.2.2巡线规律图

注：红色代表传感器亮灯状态,灰色代表传感器灭灯状态

3.3 红外巡线功能代码实现

3.3.1控制流程图

图3.3.1红外巡线控制流程图

3.3.2基础代码

/* 
* 函数名：MOTOR_GPIO_Config * 描述 ：电机引脚初始化 
* 输入 ：无 
* 输出 ：无 
* 调用 ：功能函数中调用 
*/  
void MOTOR_GPIO_Config(void)  
{  
GPIO_InitTypeDef GPIO_InitStructure;  
RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOC, ENABLE);  
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8  
|GPIO_Pin_9;  
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;  
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  
GPIO_Init(GPIOC, &GPIO_InitStructure);  
GPIO_ResetBits(GPIOC, GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8  
|GPIO_Pin_9);  
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);  
/*****************************电机 ENA PA0 ENB 
PA1*************************/  
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;  
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; // 复用推  
挽输出  
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  
GPIO_Init(GPIOA, &GPIO_InitStructure);  
}  
/* 
* 函数名：Fllow_GPIO_Config * 描述 ：红外巡线引脚初始化 
* 输入 ：无 
* 输出 ：无 
* 调用 ：功能函数中调用 
*/  
void Fllow_GPIO_Config(void)  
{  
GPIO_InitTypeDef GPIO_InitStructure;  
RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOC, ENABLE);  
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13|GPIO_Pin_14;  
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;  
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  
GPIO_Init(GPIOC, &GPIO_InitStructure);  
GPIO_SetBits(GPIOC,GPIO_Pin_13|GPIO_Pin_14);  
}  
/* 
* 函数名：TrackLine * 描述 ：红外巡线模式 
* 输入 ：无 
* 输出 ：无 
* 调用 ：function 中调用 
*/  
void TrackLine(void)//巡线  
{  
if((TR_L==0)&&(TR_R==0))//两边同时都没有探测到黑线  
{  
MOTOR_GO_FORWARD;  
return;  
}  
if((TR_L == 0)&&(TR_R== 1))//右侧遇到障碍  
{  
MOTOR_GO_RIGHT;  
return;  
}  
if((TR_L == 1)&&(TR_R == 0))//左侧遇到障碍  
{  
MOTOR_GO_LEFT;  
return;  
}  
if((TR_L == 1)&&(TR_R == 1))//左右都检测到黑线，就如视频中的那样遇到一  
道横的胶带  
{  
MOTOR_GO_STOP;  
return;  
}  
}  
/* 
* 函数名：main 
* 描述 ：程序入口函数 
* 输入 ：无 
* 输出 ：无 
* 调用 ：无 
*/  
void main(void)  
{  
/* 设置系统时钟为 72M */  
SystemInit();  
MOTOR_GPIO_Config();//电机引脚初始化  
Fllow_GPIO_Config();//巡线传感器引脚初始化  
ENA_H;ENB_H//EN 引脚初始化  
while (1)  
{  
TrackLine();  
}  
}  

3.4 创新功能--手动控制

3.4.1运动状态分析

前进：左、右电机均正转，PWM 信号的占空比控制履带速度。

后退：左、右电机均反转，PWM 控制履带倒退速度。

左转：左侧电机反转，右侧电机正转。

右转：左侧电机正转，右侧电机反转。

停止时设置：关闭电机输出

3.4.2控制流程图

图3.4.1手动控制控制流程图

3.4.3基础代码

if(buffer[0]==0x00)//手动控制  
   {switch(buffer[1])  
         {  
           case 0x00:MOTOR_GO_STOP;Set_Left_Speed(speed_left); Set_Right_Speed(speed_right);return;           //停止  
           case 0x01:MOTOR_GO_FORWARD;Set_Left_Speed(speed_left); Set_Right_Speed(speed_right);return;        //前进  
           case 0x02:MOTOR_GO_BACK;Set_Left_Speed(speed_left); Set_Right_Speed(speed_right);return;           //后退  
           case 0x03:MOTOR_GO_LEFT;Set_Left_Speed(speed_left); Set_Right_Speed(speed_right);return;           //左转  
           case 0x04:MOTOR_GO_RIGHT;Set_Left_Speed(speed_left); Set_Right_Speed(speed_right);return;          //右转  
           case 0x05:MOTOR_GO_FORWARD;Set_Left_Speed(speed_left); Set_Right_Speed(20);return;          //左前  
           case 0x06:MOTOR_GO_FORWARD;Set_Left_Speed(20); Set_Right_Speed(speed_right);return;          //右前  
           case 0x07:MOTOR_GO_BACK;Set_Left_Speed(speed_left); Set_Right_Speed(20);return;          //左后  
           case 0x08:MOTOR_GO_BACK;Set_Left_Speed(20); Set_Right_Speed(speed_right);return;          //右后  
           default:return;                          //停止  
         }  
   }  

3.5 改进方向--自适应巡线算法（暂未成功）

自适应巡线算法是指通过智能算法（如PID动态调整、模糊逻辑控制、深度学习等），使小车能够根据不同路况（直线、弯道、岔路等）动态调整巡线行为，从而提升巡线效率和精度。下面是其硬件需求、关键算法和代码实现的详细内容。

3.5.1 PID控制算法公式

PID控制器的核心公式为：

• e(t)：当前误差（目标值与实际值的差）
• Kp：比例系数，决定系统对误差的直接反应速度。
• Ki：积分系数，消除稳态误差，但过大可能导致系统振荡。
• Kd：微分系数，抑制误差变化过快，增强系统稳定性。

在实际的STM32小车控制中，离散化公式为：

其中：

• ∑e(i)：误差的累积（积分部分）。
• e(t)−e(t−1)：误差的变化率（微分部分）。

3.5.2 控制流程图

控制流程图概述：

1. 读取传感器： 读取红外传感器的数据，确定误差。
2. 误差计算： 根据传感器数据计算当前误差（偏差）。
3. 动态调整PID参数： 根据当前路径动态调整PID控制参数。
4. PID计算： 计算PID控制量（包括比例、积分、微分部分）。
5. 电机控制： 根据计算出的控制量调整电机速度。
6. 更新误差： 保存当前误差，为下次计算做准备。
7. 循环执行： 重复执行循环，实时调整小车路径。

图3.5.1 自适应巡线算法控制流程图

3.5.3 代码实现

1.定义PID控制变量，用于存储误差、PID参数等。

// PID控制参数  
float Kp = 0.5, Ki = 0.1, Kd = 0.2;  
float error = 0, prev_error = 0, integral = 0;  
// 小车目标速度（基准速度）  
int base_speed = 50;  
// 传感器数据读取  
int read_sensor_left()   
{  
    return GPIO_ReadInputDataBit(SENSOR_PORT, SENSOR_LEFT);  
}  
int read_sensor_right()   
{  
    return GPIO_ReadInputDataBit(SENSOR_PORT, SENSOR_RIGHT);  
}  

2.PID控制逻辑实现PID公式，计算电机的速度调整值。

void pid_control() {  
    // 读取传感器状态  
    int left = read_sensor_left();  
    int right = read_sensor_right();  
    // 根据传感器状态计算误差  
    if (left && !right) {  
        error = -1.0; // 偏向左边  
    } else if (!left && right) {  
        error = 1.0; // 偏向右边  
    } else {  
        error = 0.0; // 保持在中间  
    }  动态调整PID参数
    //   
    float curvature = calculate_curvature(); // 计算当前路径的曲率  
    dynamic_pid_adjustment(curvature); // 根据曲率调整Kp, Ki, Kd  
    // 计算PID各部分  
    integral += error;  // 积分  
    if (integral > 10.0) integral = 10.0;  // 积分限幅  
    if (integral < -10.0) integral = -10.0;  
    float derivative = error - prev_error;  // 微分  
    float correction = Kp * error + Ki * integral + Kd * derivative;  
    // 调整电机速度  
    int left_speed = base_speed - correction;  
    int right_speed = base_speed + correction;  
    // 设置电机速度  
    set_motor_speed(left_speed, right_speed);  
    // 更新上一次误差  
    prev_error = error;  
}  

3.动态调整参数，根据曲率或速度实时调整PID参数。

void dynamic_pid_adjustment(float curvature) {  
    if (curvature > 0.5) {  
        Kp = 0.6; Ki = 0.2; Kd = 0.3; // 高曲率：增强控制力度  
    } else {  
        Kp = 0.4; Ki = 0.1; Kd = 0.2; // 低曲率：降低灵敏度  
    }  
} 

4.设置电机速度通过PWM信号调整电机速度。

void set_motor_speed(int left_speed, int right_speed) {  
    // 限制速度范围  
    if (left_speed < 0) left_speed = 0;  
    if (left_speed > 100) left_speed = 100;  
    if (right_speed < 0) right_speed = 0;  
    if (right_speed > 100) right_speed = 100;  
    // 设置电机输出（示例：通过PWM控制）  
    TIM_SetCompare1(TIM2, left_speed);  // 假设TIM2_CH1用于左电机  
    TIM_SetCompare2(TIM2, right_speed); // 假设TIM2_CH2用于右电机  
}  

实物产品及部分源代码来自：小R科技

如有侵权请联系作者删除