基于树莓派的电机倒立摆控制系统开发

洲洲不是州州

已于 2023-03-04 18:33:04 修改

阅读量1.8k

点赞数 2

分类专栏：嵌入式完整作品文章标签： linux arm开发嵌入式硬件

于 2022-10-24 21:23:55 首次发布

本文链接：https://blog.csdn.net/qq_41834692/article/details/127500346

版权

倒立摆树莓派 PID控制霍尔编码器 Simulink仿真

关键词由CSDN通过智能技术生成

嵌入式完整作品专栏收录该内容

22 篇文章

订阅专栏

本文详细介绍了基于树莓派的倒立摆系统设计，包括系统建模、硬件接口设计、软件控制策略及PID参数调试。通过Simulink仿真验证模型，并利用编码器、霍尔传感器和L298驱动器实现电机闭环控制。文章重点阐述了角度和位置双闭环PID控制，以及电机驱动和传感器接口的Python编程实现，展示了系统的稳定控制效果。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

目录（注：完整论文和代码可主页联系作者）

摘要

随着现在控制理论在实际生活中的广泛使用，，倒立摆作为一种验证控制理论正确性的一种重要工具，使得对于倒立摆控制的研究越来越有必要。本文在分析了倒立摆的理论模型，基于树莓派linux平台这一强大微型电脑下，控制编码器直流电机驱动和调速以及PID控制，再一次验证了控制科学与工程这一领域相关理论的正确性，以及相关参数对于倒立摆稳定性的影响大小。设计了树莓派与电机及其驱动系统）之间的接口电路，编码器直流电机硬件驱动电路，电源电路，树莓派与电位传感器之间的接口电路，硬件部分由树莓派控制芯片，编码器电机，电位传感器组成，以树莓派为驱动器的控制芯片，软件部分是在树莓派上利用python语言进行驱动电机代码，编码器和电位传感器数据处理的代码。采用L298驱动器进行对电机的驱动，通过Simulink仿真得到控制倒立摆PID的相关参数，采用位置环和速度环双闭环控制倒立摆，编写控制软件，实现倒立摆的控制，包括起摆，倒立稳定控制；编写人机界面，实现对倒立摆控制系统的控制操作，以及对倒立摆数据的实时显示与存储，从而实现对倒立摆的稳定控制。另外设计了编码器接口电路用来连接电机上的霍尔编码器和树莓派，对树莓派通过设置频率和占空比来输出PWM波，驱动编码器电机旋转，通过调节PWM的占空比进而调节电机的电压来实现对速度的调节。通过霍尔编码器来实现对电机的位置和速度的实时反馈，经过驱动器上的编码器接口与树莓派GPIO口相连，从而形成一个闭环控制系统。运用增量式PID算法求出每次控制量的增量值来改变每次控制量输出的大小，从而让电机的实际状态达到所设定的目标状态，实现对倾角和位置的闭环控制，并把电机的实时状态传输到上位机进行实时显示。

关键词

倒立摆、树莓派、PID控制、霍尔编码器、Simulink仿真、人机交互界面

第1章绪论

1.1背景和意义

1.1.1 背景

在控制科学与工程这门学科发展过程中,为了验证一种控制学科上的相关理论，以及是否能在实际生活中是否有效使用，往往需要在现实中具体来搭建一个模型来验证该理论，并且该理论中的各种参数能够通过该模型调节，观察验证这些参数对控制对象的调节作用。本次毕业设计的倒立摆系统就是像以上的实际验证模型，控制理论的科学性就可以通过该倒立摆模型来验证。随着现代控制理论和科学的不断发展，倒立摆系统就变为现在越来越多的用作验证控制学科相关理论的有效性的一种方法。

倒立摆系统就是一个典型的不稳定的、较为复杂的、非线性的的模型，是现如今进行多种验证实验的非常理想的一个控制模型，本次毕业设计做的倒立摆的模型控制过程就是使一根金属摆杆能够平稳的保持在垂直位置，当摆杆与水平面保持垂直位置后，摆杆能够在一定的平衡位置以较小的振荡和较小的角度和速度变化，自动调节自身位置保持平衡，并且在受到外界的微小扰动能够自发的维持平衡位置[1]。

倒立摆的研究具有很大的实际意义，其中部分重要的工程背景包括以下几点：
（1）为了提高侦察卫星上摄像机拍摄照片的质量，其摄像机也需要能够像倒立摆那样保持稳定，使其自动地保持摄像机云台的稳定，消除震动。

（2）如今的多级火箭的控制，其飞行姿态的控制也需要用多级倒立摆系统的控制理论[2]。

（3）通信卫星在轨道上正确位置上运行时，也要保持正确的朝向，使卫星天线的指向一直指向地球，它的太阳能电池板一直朝向太阳方向。

（4）现代的比较前沿的机器人的行走控制方法大部分是以双倒立摆系统模型为理论基础，自从机器人了后，机器人中的其中一项技术――人形机器人的行走姿态控制到现在都没有比较令人完全满意的控制方法。

（5）在现代的火箭导弹等飞行器的设计过程中，其正确的飞行状态也需要倒立摆控制理论方面的相关控制原理，飞行器的控制同样需要像倒立摆控制一样不断进行实时控制。

1.1.2 意义

倒立摆系统的研究，能够实际的检验控制理论中的新的控制方法的可行性，验证其是否能够有效的处理实际的非线性和不稳定问题。因此，要完整完成本课题，取得理想效果就要充分熟悉并灵活运用控制工程的原理、灵活运用树莓派的优势，并设计基于树莓派设计树莓派与电机（及其驱动系统）之间的接口电路，并编写控制软件，实现倒立摆的控制，包括起摆，倒立稳定控制。因此。本课题具有重大意义。

1.2 国内外研究现状

现在世界上在倒立摆方面上研究理论的主要着重在研究倒立摆的稳定控制和研究倒立摆系统的自起摆控制。两者相比，倒立摆的稳定控制研究更具有一定的实际意义，成果也更明显一些。早在1960年，当时倒立摆控制在实际工程的应用作用，就使得众多研究人员开始了对倒立摆系统的研究工作。到了1960年后期，倒立摆被认为是一个典型不稳定、非线性的例证被众多研究人员提出[3]。从这开始，对于倒立摆系统的研究变成了控制界关注的焦点。

近些年来，虽然出现各种种类的倒立摆模型，但是实际上能够独立研发制造较为完善准确的倒立摆模型的厂家并不多。目前国内各高校基本上都采用香港固高公司和加拿大Quanser公司生产的系统[5]。

因为倒立摆系统在双足机器人的行走控制上，在火箭飞行控制飞行姿态的控制上和以及多种伺服云台的稳定性上有着多种应用。所以，对倒立摆控制理论研究具有重大的实际意义。倒立摆系统是一个用来验证控制理论的正确性和实际应用中的可行性的很好的工具。

第二章倒立摆设计方案

2.1 倒立摆系统建模

倒立摆系统是控制理论研究中理想的被控对象，为了便于直观的分析出控制倒立摆运行的各个参数，下面利用控制科学的相关理论对倒立摆系统进行理论上的模型建立，同时也便于接下来倒立摆实物的搭建。

下面为单级倒立摆系统模型，将根据此模型利用牛顿定律进行受力分析，计算倒立摆系统的传递函数。

图 2-1 单级倒立摆系统

如图2-1所示，一个小车上固定着一个摆杆，在水平面上受到其他外力的作用，构成了一个标准的倒立摆系统，由于在这里现实中摩檫力极小，在此次建模中忽略不计，该系统受到自身重力，驱动力两个外力共同作用，同时也有小车与摆杆内部的相互作用力，这里的驱动力实际上是由连接在小车上的传送带装置提供，控制倒立摆的稳定就是依靠驱动力使各个力之间达到一种动态平衡，使得小车做和倒立摆相似的运动，使摆杆保持稳定垂直状态[6]。

系统的各个参量含义如下：

参量	含义	测量大小
M	小车质量	0.2275Kg
m	摆杆质量	0.0924Kg
l	摆杆长度的一半	0.185m
J	摆杆的转动惯量	$0.04212Kg\cdot m^2$
u(t)	加在小车上的力	待定
x	小车的位置	待定
$\Theta$	摆杆与垂直方向的夹角	待定

表 2-1 参量列表

根据牛顿运动定律和刚体运动定律，可得：

长杆绕其固定点的转动方程为：

$J\Theta\ddot{\cdot }=N_{}y l sin\Theta -N_{x} l sin\Theta$ (2-1)

小车和摆杆的相互作用力的方向的分量为：

这里公式实在是懒得编辑了，省略 (2-2)

根据控制理论中的微分运算法则变换（2-2）式为：

略 (2-3)

小车和摆杆的相互作用力的方向的分量为：

略 (2-4)

根据控制理论中的微分运算法则变换（2-4）式为：

略 (2-5)

小车在水平方向的运动方程为：

略 (2-6)

考虑到摆杆调节平衡位置时，摆杆的倾角仅在范围之间调节，所可以近似的认为：

略 (2-7)

再对式(2-1)、(2-3)、(2-5)做进一步简化后得：

略 (2-8)

略 (2-9)

略 (2-10)

结合式(2-6)、(2-8)、(2-9)、(2-10)可得小车最后的运动方程为：

略 (2-11)

略 (2-12)

整理(2-11)、(2-12)式，得：

代入表2-1中各参数数值到(2-13)、(2-14)式得： (2-15) (2-16)

根据控制科学原理，进行拉普拉斯变换，可得传递函数为： (2-17) (2-18)

2.2 Simulink仿真

由以上结论，利用Simulink现有得模型，将式(2-13)、(2-14)代入Matlab进行仿真，建立出倒立摆系统模型，仿真结果可通过示波器，图像，数据的方式显示出来，直观的理解倒立摆相关数据参数对系统稳定性的影响。

图 2-2 Simulink系统框图

得到的响应的波形如下：

图 2-3 Simulink仿真波形

第三章硬件设计方案

3.1整体设计方案

电机驱动器系统整体采用树莓派作为主控芯片，采用按键作为输入控制，再连接驱动芯片L298控制驱动电路，以此来驱动电机。电机上面连接有霍尔编码器，编码器再连接到驱动器上的编码器接口后与单片机相连，从而实现对电机的闭环控制，树莓派再通过串口对电机的状态进行打印显示。

图 3-1 系统整体框图

3.2 供电接口电路

供电电路采用15V航模电源供电，采用三个降压模块，分别降至12V给直流电机供电，降至5V给编码器供电，降至3V给电位传感器供电，另外树莓派直接采用充电宝5V-2.4A的USB供电。

图 3-2 供电接口电路设计

3.3 树莓派与传感器接口电路

树莓派的GPIO口编号采用 BCM规则，该规则是一种更底层的工作方式，其与Broadcom的片上系统中信道编号相对应。其缺点是当换用一个新的板子时，需要重新查找信道号和物理引脚编号之间的对应规则。树莓派的GPIO.22和GPIO.23接到霍尔编码器的A、B相实时采集电机的转动数据，通过GPIO.27和GPIO.28口控制直流电机的正反转，通过GPIO.26给电机相应的占空比控制电机的转速。另外通过GPIO.25口传输数据到上位机，以便实时观测倒立摆的运行参数，电位传感器通过GPIO.24向树莓派传输倒立摆摆杆与平衡位置的角度偏差，使程序能够通过位置和速度双闭环调整摆杆的位置。

图 3-3 树莓派接口电路图

第四章软件设计方案

4.1 主函数

在主函数的逻辑中，先对树莓派中的GPIO口进行初始化，将GPIO口设置BCM编号方式，并设置成各GPIO口为输入输出模式，初始化GPIO口为低电平。再进行PWM的初始化，设置频率，调制占空比，复位所有系统中使用到的外设，初始化系统时钟、按键以及串口通信等的配置。对控制电机用到的定时器进行初始化，并配置脉冲输出，使电机状态初始化为零。延时一段时间后，对PID控制器的各个参数进行初始化。随后进入循环状态，对按键的状态进行扫描判断，如果判断启动按键的电平状态为按下的状态，接着后面就进行PID参数的初始化和电机速度的初始化，继而调用启动电机的函数，让电机以一定的PWM输出值运行[8]。这个过程通过编码器函数在中断中实时的采集电机速度值，并将该值传给倾角PID和位置PID函数，进行双闭环控制实时的调节电机的占空比，从而使电机的状态达到目标状态，进而实现倒立摆稳定运行。

图 4-1 主函数流程图

4.2 定时器中断函数

在树莓派中，python中的中断函数是通过开启一个新的线程定时执行一段固定的函数，函数为 threading.Timer(中断时间, 函数名)，设定时间为50ms，即每过50ms执行进入PID函数，进行一次PID计算，然后将PID计算的占空比赋值给电机，进而控制电机的速度。

4.2.1 位置闭环定时器中断函数

在位置闭环控制中，其实就是利用电机旋转时，其上的编码器产生脉冲，然后由定时器计数，累加测量电机的位置信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。当计数达到位置环的计时周期之后，就会对编码器捕获到的信号值进行计算，得到当前时刻的位置值。当前时刻的位置状态用总的脉冲数来衡量，总的脉冲数的计算值需要用编码器定时器的计数最大值乘以溢出次数再加上一计数周期内的捕获值而得到，接着就会把PID控制器计算函数返回的增量值加到当前PWM输出值上面，作为下一次位置环的PWM输出值。同时，会对速度进行限幅，防止刚开始的时候速度太大。接着对方向进行判断之后，输出计算完成的PWM，实时传给电机控制电机的占空比。

图 4-2 位置闭环中断回调函数

4.2.2 角度闭环定时器中断函数

这是最核心的控制，其他的控制相对于倾角控制而言都是干扰。

在倾角闭环控制里，实际上就是根据单位时间获取的脉冲数测量电机的转过的角度信息，并与电位器平衡位置的目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。当达到倾角环的计时周期之后，树莓派会先计算当前电机转过的角度。当前角度值的计算是将前后采集到的两次编码器作差计算，这样计算得到的当前角度值的是每样本周期采集到的脉冲数。将编码器的值转化成角度值，下面还需要对当前角度计算值的单位进行转化，转化为和设定的目标角度值（即角度传感器的平衡位置）同样的单位，即和设定的目标角度值一样单位的当前角度值。接着，计算PWM输出值，并判断方向后，输出计算完成的PWM，实时传给电机控制电机的占空比。

图 4-3 角度闭环中断回调函数

4.2.3 位置角度双闭环定时器函数

位置角度双闭环的程序中，位置环和速度环都有各自的计算周期，达到各自的计时周期之后，就会各自进行相应的计算，当位置环和角度环分别进行计算之后，然后会将两者计算的PWM做差，由于倾角环是最核心的控制，因此在这里，最终的PWM计算值为倾角环PWM值减去位置环的PWM值，然后把还需要将该值进行PWM值限幅，避免占空比100%带来的不稳定因素，才会输出最终的PWM输出值，赋值给电机。

图 4-4 位置角度双闭环回调函数流程图

4.3 编码器输入捕捉程序

采用的编码器为13线的霍尔编码器，电机的减速比为1：20，故同步轮转一圈，电机可以输出260个脉冲，编码器集成了上拉电阻和比较整形功能，可以直接输出方波。电机转动时，霍尔传感器就会根据磁钢转动产生的磁场作用而输出脉冲信号，增大电机转轴上所均匀固定的磁钢的块数，那么，电机旋转一周所产生的脉冲个数就会随之而增加，单片机可以对编码器产生的信号进行捕捉[9]，进而通过计算得到电机的位置和速度信息。将编码器对于的GPIO口设置为输入模式，实时连续的检测脉冲，采用的是边缘检测方式。在边缘检测方式中，add_event_detect()函数运行后，会为回调函数另外开启一个线程，与主程序并发运行，因此不容易错过当 CPU 忙于处理其它事物时输入状态的改变。但是同一进程内也不能有太过耗费CPU时间的部分，否则仍会导致脉冲的丢失。

图 4-5 编码器输入捕获程序

4.4 PID算法

PID控制器计算函数的输入是被控对象的当前值，对于位置环来说就是当前位置，对于角度环来说就是当前角度。先定义偏差和增量值，接着计算偏差，偏差等于被控对象的目标值减去当前实际值[10]，并设定允许的偏差范围。接着根据公式计算增量值，增量值等于P乘以当前误差减去I乘以上次的误差加上D乘以上上次的误差，并做一个存储误差的操作，把上次的误差值赋值给上上次，当前的误差值赋值给上次。最后返回计算得到的输出量的增值大小[11]。

图 4-6 增量式PID算法流程图

第五章调试

5.1 PID参数整定

整体PID的调节方法大致为：

1、P,I,D置 0，逐渐增大 P，直至响应时间达到预期效果并出现震荡

2、增大微分项 D，直至波动减到预期

3、若存在过冲，减小 P

4、增大积分项 I

PID参数的整定方法大致上可以分为两种，可以概括为理论计算整定法和工程整定法两大类。理论计算整定法主要是根据系统的数学模型，经过精确的理论分析计算得到各个参数，无论采取哪种方法，如果要真正要投入到实际工程中使用的话，还得需要结合实际工程调整和修改[12]。工程整定法主要是调试人员根据自身所积累的工程经验，设定相关的参数，然后再根据不断地实验进行调整，以达到理想的控制效果。

工程整定法中最主要的是试凑法，简而言之，试凑法就是实验人员根据以往的相关经验先设定一个初始的参数值，然后让系统试运行一下，将数据传输到示波器上，观察调节的波形，根据调节系统的P、I、D三个参数对曲线的影响效果，按照调节顺序，一般先调节比例部分，再加入积分部分，最后有需要再引入微分作用的顺序，对调节器的参数进行反复调整，多次观察波形形状，不断微调，直到调出稳定的波形，此时的PID的值即为符合条件的参数。

操作步骤为：

（1）把积分项和微分项的系数都设为零，只使用比例项，根据经验设定一个比例系数，让系统运行起来，根据曲线调整比例系数的大小，如果超调量太大，就减小比例系数，不断调整，得到理想的4:1过渡曲线。

（2）加入积分项，把上面调节出来的比例系数增大1.2倍，把积分系数由小到大进行调整，如果曲线振荡的比较厉害，就适当减小积分系数。

（3）一般比例作用加积分作用就能满足系统的要求，得到比较理想的调节效果，如果最终效果还不算让人满意的话，系统确实需要，就再加入微分作用。按照经验由小到大设置微分系数，如果曲线振荡比较严重的话，就减小微分系数，如果曲线超调量严重而且很难快速衰减下来，就增大微分系数。最后再结合图线的形状，整体对三个参数进行调整，慢慢调出满意的效果[13]。

5.2 角度闭环PID参数整定

将电机和电位传感器连接，设定一个电位器的平衡位置，将计算出的电机角度变化不断与平衡位置的角度值比较，采用试凑法，先引入比例环节，慢慢地对比例系数进行微调，找出衰减震荡的趋势。因为电机角度变化越小，越难精确控制，所以把电位传感器的角度偏移值加大，再来看运行的效果。可以看到将目标角度从原来的增大到之后，曲线已经逐渐趋于稳定，下一步就是对曲线的超调量和后面的振荡进行微调。对比例系数进行微调，可以发现，曲线的超调量减小，而且后面的振荡也比之前趋于平缓[14]。

最后进入积分作用，并对比例系数和积分系数进行不断地调整，以3115为平衡位置时的角度值，曲线经过一定的超调之后，基本经过一个振荡就可以达到目标速度，之后平稳的运行下去，取得了比较满意的效果。

图 5-7 引入比例环节试运行

图 5-8 调节P至出现衰减震荡

图 5-9 增大目标角度看运行效果

图 5-10 进一步增大目标角度并微调P

图 5-11 角度闭环最终效果图

5.3 位置闭环PID参数整定

调节位置闭环时，参考前面调节角度闭环的经验，先引入比例环节，观察运行的波形。只在比例环节作用下，可以看到系统在达到目标位置之后，会一直在目标位置附近振荡。

下一步引入积分环节，可以看到系统达到目标位置之后，不再振荡，可以停在目标位置。后面主要进行微调，减少系统达到目标位置之前的振荡次数[15]。

最后，加入微分项，并整体进行微调，使系统经过一定的超调之后，就能尽快的达到目标位置，来实现比较理想的控制效果。

图 5-12 位置闭环仅引入比例作用

图 5-13 位置闭环PI调节

图 5-14 位置闭环最终效果图

5.4 位置角度双闭环调试

在调节位置倾角双闭环中，参考前面调节调节的经验，让系统保持平衡的位置的姿态运行到目标位置之后停止，即角度环的波形始终保持同一水平线上下波动，而位置环在经过一段时间后曲线稳定在目标位置。

系统以电机与电位器平衡位置之差为-200~200范围之内表示平衡位置（超出这个范围，无论怎么调节，倒立摆都无法保持平衡），下图为设定倒立摆保持平衡姿态向左移动一个单位长度之后停止的运行效果展示图，可以看到系统基本以目标角度（即平衡位置）运行到目标位置之后停止，达到比较满意的效果。

图 5-15 位置-角度曲线图

第六章总结

本文设计了基于树莓派倒立摆的方案模型，利用matlab中的Simulink进行了倒立摆的仿真，验证了方案的实际可行性，并且学习了树莓派的python编程方法，掌握了部分linux系统，在控制倒立摆稳定的过程中，并且以PID控制器的算法思想为理论基础，设计了基于树莓派的电机位置角度双闭环控制的软件程序，同时利用工程整定法的试凑法对各个PID的参数进行多次实验，最终得到满意的控制效果。

第七章程序代码

部分程序代码：

#霍尔脉冲读取函数

 GPIO_A=22 #编码器A相

 GPIO_B=23 #编码器B相

 GPIO.setup(GPIO_A, GPIO.IN,pull_up_down=GPIO.PUD_UP)   #通过22号引脚读取A相脉冲数据

 GPIO.setup(GPIO_B, GPIO.IN,pull_up_down=GPIO.PUD_UP)   #通过23号引脚读取B相脉冲数据

 counter=0      #A相脉冲初值

 counter1=0     #B相脉冲初值

 def my_callback(channel):          #边缘检测回调函数

     global counter                 #设置为全局变量

     if GPIO.event_detected(GPIO_A):        #检测到一个脉冲则脉冲数加1

         counter=counter+1

 def my_callback1(channel1):            #这里的channel和channel1无须赋确定值

     global counter1

     if GPIO.event_detected(GPIO_B):

         counter1=counter1+1

 GPIO.add_event_detect(GPIO_A,GPIO.RISING,callback=my_callback) #在引脚上添加上升临界值检测再回调

 GPIO.add_event_detect(GPIO_B,GPIO.RISING,callback=my_callback1)




PID函数：

import pyb

class PID:

    def __init__(self, pwm_range, kp_A, ki_A, kd_A, kp_B, ki_B, kd_B):

        pwm_A = 0

        pwm_B = 0

        err = 0

        err_A = 0

        err_B = 0

        last_err_A = 0

        last_err_B = 0

        self.pwm_range = pwm_range

        self.kp_A = kp_A

        self.ki_A = ki_A

        self.kd_A = kd_A

        self.kp_B = kp_B

        self.ki_B = ki_B

        self.kd_B = kd_B

        self.pwm_A = 0

        self.pwm_B = 0

        self.err = err

        self.err_A = err_A

        self.err_B = err_B

        self.last_err_A = last_err_A

        self.last_err_B = last_err_B

    

    # 增量PID原理代码

    def incremental_pid(self, now, target):

        '''

        函数功能：增量PI控制器

        入口参数：当前的编码器值，目标速度对应的编码器值

        返回值  ：电机PWM

        根据增量式离散PID公式：

        pwm += Kp[e(k) - e(k-1)] + Ki*e(k) + Kd[e(k) - 2e(k-1) + e(k-2)]

        e(k)代表本次偏差；

        e(k-1)代表上一次的偏差；    以此类推

        pwm代表增量输出。

        '''

        err = target - now

        pwm = pwm + self.kp*(err - last_err) + self.ki*err + self.kd*(err - last_err)

        if (pwm >= self.pwm_range): # 限幅，防止pwm值超出100

            pwm = self.pwm_range

        if (pwm <= -self.pwm_range):

            pwm = -self.pwm_range

        last_err = err

        return pwm



    def pid_A(self, now, target):

        self.err_A = target - now

        self.pwm_A = self.pwm_A + self.kp_A*(self.err_A - self.last_err_A) + self.ki_A*self.err_A + self.kd_A*(self.err_A - self.last_err_A)

        if (self.pwm_A >= self.pwm_range):

            self.pwm_A = self.pwm_range

        if (self.pwm_A <= -self.pwm_range):

            self.pwm_A = -self.pwm_range

        self.last_err_A = self.err_A

        return self.pwm_A



    def pid_B(self, now, target):

        self.err_B = target - now

        self.pwm_B = self.pwm_B + self.kp_B*(self.err_B - self.last_err_B) + self.ki_B*self.err_B + self.kd_B*(self.err_B - self.last_err_B)

        if (self.pwm_B >= self.pwm_range):

            self.pwm_B = self.pwm_range

        if (self.pwm_B <= -self.pwm_range):

            self.pwm_B = -self.pwm_range

        self.last_err_B = self.err_B

        return self.pwm_B



电机驱动程序：

class Motor:

    def __init__(self,direction,speed):

        self.IN1 = 27 # 电机A的正极IO口

        self.IN2 = 28

        self.ENA = 16   # 电机的PWM输入IO口

        self.hz = 500   #定义频率变量（等一下用）

        self.pwm = 0

        self.speed = speed    #定义占空比变量（等下用）运行时默认的占空比为0

        self.direction = direction

               

    def setup(self):

        print('Begin setup IN1 IN2 ENA')

        GPIO.setwarnings(False)  #忽略警告

        GPIO.setmode(GPIO.BCM)    # 使用BCM编号方式

        GPIO.setup(self.ENA, GPIO.OUT)  # 将连接ENA的GPIO引脚设置为输出模式

        GPIO.setup(self.IN1, GPIO.OUT)   # 将连接IN1的GPIO引脚设置为输出模式

        GPIO.setup(self.IN2, GPIO.OUT)   # 将连接IN2的GPIO引脚设置为输出模式

        self.pwm = GPIO.PWM(self.ENA, self.hz)   # 设置向ENA输入PWM脉冲信号，频率为hz并创建PWM对象

        self.pwm.start(self.speed)         # 以speed的初始占空比开始向ENA输入PWM脉冲信号

        print('Setup IN1 IN2 ENA over')



    def destroy(self):

        GPIO.output(self.IN1, GPIO.LOW)

        GPIO.output(self.IN2, GPIO.LOW)

        GPIO.output(self.ENA, GPIO.LOW)

        GPIO.cleanup()

        

    def motor_run(self,direction,speed):

        '''

        函数功能：控制方向，控制速度

        '''

        #self.setup()

        if direction == 0:     #如果键盘输入的数值为0，则电机正转

            GPIO.output(self.IN1, GPIO.LOW)

            GPIO.output(self.IN2, GPIO.HIGH)

            self.pwm.ChangeDutyCycle(speed)   # 改变PWM占空比

        if direction == 1:      #如果键盘输入的数值为1，则电机反转

            GPIO.output(self.IN1, GPIO.HIGH)

            GPIO.output(self.IN2, GPIO.LOW)

            self.pwm.ChangeDutyCycle(speed)        

        if direction == 2:      #如果键盘输入的数值为2，则电机停止转动

            GPIO.output(self.IN1, GPIO.LOW)

            GPIO.output(self.IN2, GPIO.LOW)

            GPIO.output(self.ENA, GPIO.LOW)