基于PID的减速电机控制（stm32f103c8t6）_嵌入式系统与MCU-面包板社区

楼主

基于PID的减速电机控制（stm32f103c8t6）

KA_IX

5372 主题
5429 帖子
11410 积分

身份：LV7 中级工程师

E币：4876

发消息

 771

 0

发表于 2023-5-12 11:14:56

显示全部楼层

本例程采用了HAL库进行项目开发（主要使用软件CubexMX和keil5），文章末尾会有代码开源，欢迎各位对文章进行指正和探讨。

基于PID的减速电机控制

一、硬件模块与原理图
1、硬件组成

硬件组成：stm32f103c8t6最小系统板；0.96寸LED12864（I2C通讯模式）；智能小车12v移动电源；25GA370直流减速电机（带霍尔编码器）；JDY-31蓝牙模块；L298N电机驱动模块；杜邦线若干；1个面包板；

图片如下：

8b09fa472c9d4ed6a65ec7b9e24979ba~noop.image?_iz=58558&from=article.jpg

2、模块分析
1、L298N电机驱动模块
038c119470f848a18c0d1a8f171930f3~noop.image?_iz=58558&from=article.jpg

64e613a497864bb383b0f1053b9d2e00~noop.image?_iz=58558&from=article.jpg

1.模块可驱动两路直流电机，输出A和B各接一直流电机即可；

2.若使用12V供电，将12V供电端口及GND接上电源正负即可，同时5V供电端可以作为最小系统板的输入电源；

3.若不需要使用PWM调速，只需要控制电机正反转，则逻辑A与B跳线帽插上即可，相当于始终使能；

4.若需要使用PWM调速，需将跳线帽拔起，将使能端接上单片机IO口。（定时器IO口，PWM输出模式）；

5.逻辑输入四个端口IN1、IN2、IN3、IN4接单片机四个IO口，每两个端口控制的一路电机。

温馨提示： 特别不建议新手或者资金有限的情况下，使用电机驱动模块直连成品开发板，很容易烧坏。

原因：（1）由于电机的特性，电机在堵转或者高负载下，电流会增大，可能会影响到单片机。（2）新手玩单片机可能出现短路等情况，很容易板子冒烟；

L298N的转动逻辑图：

de01e638f3f2477ca9d1a0b0b903c69b~noop.image?_iz=58558&from=article.jpg

2、0.96寸OLED（I2C通讯）
8bb02c1ecd974dd681f4823b61c14ca7~noop.image?_iz=58558&from=article.jpg

665ea870ff1f404bbfcd53c0a1a02411~noop.image?_iz=58558&from=article.jpg

（1）目前市面主要分为OLED与LCD这2种屏幕；

（2）OLED自发光特性，LCD都要背光，而OLED不需要，因为它是自发光。这样同样的显示，OLED效果要来得好一些；

（3）多种接口方式：6800，8080两种并行接口方式，4线的穿行SPI接口，IIC接口方式（2线）；

（4）不要接过高电压，3.3V就可以正常工作了；

（5）OLED不足之处是做大之后成本较高。

本实验采用了0.96寸OLED的屏幕（通讯方式IIC），4个接线柱（SCL,SDA,GND,VCC）； IIC通讯实现方式： IIC(Inter－Integrated Circuit)总线是一种由 PHILIPS 公司开发的两线式串行总线，用于连接微控制器及其外围设备。它是由数据线 SDA 和时钟 SCL 构成的串行总线，可发送和接收数据。高速 IIC 总线一般可达 400kbps 以上。

模拟IIC通讯：

I2C 是支持多从机的，也就是一个 I2C 控制器下可以挂多个 I2C 从设备，这些不同的 I2C从设备有不同的器件地址，这样 I2C 主控制器就可以通过 I2C 设备的器件地址访问指定的 I2C设备了。SDA 和SCL 这两根线必须要接一个上拉电阻，一般是 4.7K。其余的 I2C 从器件都挂接到 SDA 和 SCL 这两根线上，这样就可以通过 SDA 和 SCL 这两根线来访问多个 I2C设备。

I2C 协议：（1）起始位；（2）停止位；（3）数据传输；（4）应答信号；（5）I2C 写时序；（6）I2C 读时序

I2C 写时序：

5eb5f838ce2c4a028d1cb96ad6fb49e6~noop.image?_iz=58558&from=article.jpg

1)、开始信号。

2)、发送 I2C 设备地址，每个 I2C 器件都有一个设备地址，通过发送具体的设备地址来决

定访问哪个 I2C 器件。这是一个 8 位的数据，其中高 7 位是设备地址，最后 1 位是读写位，为

1 的话表示这是一个读操作，为 0 的话表示这是一个写操作。

3)、 I2C 器件地址后面跟着一个读写位，为 0 表示写操作，为 1 表示读操作。

4)、从机发送的 ACK 应答信号。

5)、重新发送开始信号。

6)、发送要写写入数据的寄存器地址。

7)、从机发送的 ACK 应答信号。

8)、发送要写入寄存器的数据。

9)、从机发送的 ACK 应答信号。

10)、停止信号。

I2C 读时序：

64e0f1eba47f4b36bc9c350d37fd29db~noop.image?_iz=58558&from=article.jpg

I2C 单字节读时序比写时序要复杂一点，读时序分为 4 大步，第一步是发送设备地址，第二步是发送要读取的寄存器地址，第三步重新发送设备地址，最后一步就是 I2C 从器件输出要读取的寄存器值，我们具体来看一下这几步。

1)、主机发送起始信号。

2)、主机发送要读取的 I2C 从设备地址。

3)、读写控制位，因为是向 I2C 从设备发送数据，因此是写信号。

4)、从机发送的 ACK 应答信号。

5)、重新发送 START 信号。

6)、主机发送要读取的寄存器地址。

7)、从机发送的 ACK 应答信号。

8)、重新发送 START 信号。

9)、重新发送要读取的 I2C 从设备地址。

10)、读写控制位，这里是读信号，表示接下来是从 I2C 从设备里面读取数据。

11)、从机发送的 ACK 应答信号。

12)、从 I2C 器件里面读取到的数据。

13)、主机发出 NO ACK 信号，表示读取完成，不需要从机再发送 ACK 信号了。

14)、主机发出 STOP 信号，停止 I2C 通信。

3、JDY-31蓝牙模块
a46f5e9ac45b4d299b58f2bb1d64b5ab~noop.image?_iz=58558&from=article.jpg

af8104f3e3ce4190a2058ef9c175711b~noop.image?_iz=58558&from=article.jpg

市场上蓝牙模块有很多，常见的JDY-xx，HC-xx等系列。其实看似高级的蓝牙功能背后就是简单的串口通讯；

USART 的全称是 Universal Synchronous/Asynchronous Receiver/Transmitter，也就是同步/异步串行收发器。相比 UART 多了一个同步的功能，在硬件上体现出来的就是多了一条时钟线。一般 USART 是可以作为 UART 使用的，也就是不使用其同步的功能。

串口通讯协议：

数据包：串口通讯的数据包由发送设备通过自身的TXD接口传输到接收设备得RXD接口，在协议层中规定了数据包的内容，具体包括起始位、主体数据（8位或9位）、校验位以及停止位，通讯的双方必须将数据包的格式约定一致才能正常收发数据。

具体如图所示：

f0da51fa3dad4e0792a2985961a92c58~noop.image?_iz=58558&from=article.jpg

波特率：由于异步通信中没有时钟信号，所以接收双方要约定好波特率，即每秒传输的码元个数，以便对信号进行解码，常见的波特率有4800、9600、115200等。STM32中波特率的设置通过串口初始化结构体来实现。

注意：MCU设置的波特率大小要与蓝牙APP设置的大小一致！

4、6线减速电机（带编码器）模块：

市面上电机有很多，常用的有步进电机，直流减速电机，伺服电机等等；编码器：用来测量电机转速的仪器元件，常见的有：霍尔编码器，光电编码器等电机的驱动原理很简单，给电压差即可使得电机转动，调速则利用PWM调节发。

bbb000cee6674bd0a05371041ae9018a~noop.image?_iz=58558&from=article.jpg

编码器原理： 编码器是一种将角位移或者角速度转换成一串电数字脉冲的旋转式传感器。编码器工作原理：霍尔编码器是有霍尔马盘和霍尔元件组成。霍尔马盘是在一定直径的圆板上等分的布置有不同的磁极。霍尔马盘与电动机同轴，电动机旋转时，霍尔元件检测输出若干脉冲信号，为判断转向，一般输出两组存在一定相位差的方波信号。

e8d1d25b81df46a79365fb86d7134691~noop.image?_iz=58558&from=article.jpg

59bae6773cae40aeb4d9162a29a97bf8~noop.image?_iz=58558&from=article.jpg

注意：通过判断A与B相哪一位在前，即可判断出正转还是反转

二、CubexMX设置

使用的MCU为stm32f103c8t6：

7f1e2b681848460cbf9f3d53091d82f0~noop.image?_iz=58558&from=article.jpg

RCC：

afb5090a5261401ebe99bc92433c7d87~noop.image?_iz=58558&from=article.jpg

SYS：

18c663e5436f4b82a877620818990a1a~noop.image?_iz=58558&from=article.jpg

注意：Debug这里一定要设置成Serial Wire否则可能出现芯片自锁

GPIO设置：

e8b4eb7ebbf44b8099630b662eb5c3bb~noop.image?_iz=58558&from=article.jpg

定时TIM2用来测速与测量正转反转（计数器模式）

878873b25c0e4a72b4e9dbec99f65fa2~noop.image?_iz=58558&from=article.jpg

定时3：PWM调节

c6ab7a3af4a547839df1236169a04107~noop.image?_iz=58558&from=article.jpg

I2C：

6fc7e7645d124e7dad4e571dab466814~noop.image?_iz=58558&from=article.jpg

USART1：

b3bef450ce0847c3abaa516454a3de58~noop.image?_iz=58558&from=article.jpg

之后按照自己习惯生成初始化文件

三、代码

自动生成的：

0ea9581decf840068642c205eea6a1d5~noop.image?_iz=58558&from=article.jpg

需要自己编写的：

e860a257a2dc4503a3830a8f0b91619a~noop.image?_iz=58558&from=article.jpg

I2C代码：

#include "oled.h"
#include "asc.h"
#include "main.h"
void WriteCmd(unsigned char I2C_Command)//???
{
HAL_I2C_Mem_Write(&hi2c2,OLED0561_ADD,COM,I2C_MEMADD_SIZE_8BIT,&I2C_Command,1,100);
}
void WriteDat(unsigned char I2C_Data)//???
{
HAL_I2C_Mem_Write(&hi2c2,OLED0561_ADD,DAT,I2C_MEMADD_SIZE_8BIT,&I2C_Data,1,100);
}
void OLED_Init(void)
{
HAL_Delay(100); //????????
WriteCmd(0xAE); //display off
WriteCmd(0x20); //Set Memory Addressing Mode
WriteCmd(0x10); //00,Horizontal Addressing Mode;01,Vertical Addressing Mode;10,Page Addressing Mode (RESET);11,Invalid
WriteCmd(0xb0); //Set Page Start Address for Page Addressing Mode,0-7
WriteCmd(0xc8); //Set COM Output Scan Direction
WriteCmd(0x00); //---set low column address
WriteCmd(0x10); //---set high column address
WriteCmd(0x40); //--set start line address
WriteCmd(0x81); //--set contrast control register
WriteCmd(0xff); //???? 0x00~0xff
WriteCmd(0xa1); //--set segment re-map 0 to 127
WriteCmd(0xa6); //--set normal display
WriteCmd(0xa8); //--set multiplex ratio(1 to 64)
WriteCmd(0x3F); //
WriteCmd(0xa4); //0xa4,Output follows RAM content;0xa5,Output ignores RAM content
WriteCmd(0xd3); //-set display offset
WriteCmd(0x00); //-not offset
WriteCmd(0xd5); //--set display clock divide ratio/oscillator frequency
WriteCmd(0xf0); //--set divide ratio
WriteCmd(0xd9); //--set pre-charge period
WriteCmd(0x22); //
WriteCmd(0xda); //--set com pins hardware configuration
WriteCmd(0x12);
WriteCmd(0xdb); //--set vcomh
WriteCmd(0x20); //0x20,0.77xVcc
WriteCmd(0x8d); //--set DC-DC enable
WriteCmd(0x14); //
WriteCmd(0xaf); //--turn on oled panel
}
void OLED_SetPos(unsigned char x, unsigned char y) //???????
{
WriteCmd(0xb0+y);
WriteCmd(((x&0xf0)>>4)|0x10);
WriteCmd((x&0x0f)|0x01);
}
void OLED_Fill(unsigned char fill_Data)//????
{
unsigned char m,n;
for(m=0;m<8;m++)
{
WriteCmd(0xb0+m); //page0-page1
WriteCmd(0x00); //low column start address
WriteCmd(0x10); //high column start address
for(n=0;n<128;n++)
{
WriteDat(fill_Data);
}
}
}
void OLED_CLS(void)//??
{
OLED_Fill(0x00);
}
void OLED_ON(void)
{
WriteCmd(0X8D); //?????
WriteCmd(0X14); //?????
WriteCmd(0XAF); //OLED??
}
void OLED_OFF(void)
{
WriteCmd(0X8D); //?????
WriteCmd(0X10); //?????
WriteCmd(0XAE); //OLED??
}
// Parameters : x,y -- ?????(x:0~127, y:0~7); ch[] -- ???????; TextSize -- ????(1:6*8 ; 2:8*16)
// Description : ??codetab.h??ASCII??,?6*8?8*16???
void OLED_ShowStr(unsigned char x, unsigned char y, unsigned char ch[], unsigned char TextSize)
{
unsigned char c = 0,i = 0,j = 0;
switch(TextSize)
{
case 1:
{
while(ch[j] != '\0')
{
c = ch[j] - 32;
if(x > 126)
{
x = 0;
y++;
}
OLED_SetPos(x,y);
for(i=0;i<6;i++)
WriteDat(F6x8[c][i]);
x += 6;
j++;
}
}break;
case 2:
{
while(ch[j] != '\0')
{
c = ch[j] - 32;
if(x > 120)
{
x = 0;
y++;
}
OLED_SetPos(x,y);
for(i=0;i<8;i++)
WriteDat(F8X16[c*16+i]);
OLED_SetPos(x,y+1);
for(i=0;i<8;i++)
WriteDat(F8X16[c*16+i+8]);
x += 8;
j++;
}
}break;
}
}
// Parameters : x,y -- ?????(x:0~127, y:0~7); N:???.h????
// Description : ??ASCII_8x16.h????,16*16??
void OLED_ShowCN(unsigned char x, unsigned char y, unsigned char N)
{
unsigned char wm=0;
unsigned int adder=32*N;
OLED_SetPos(x , y);
for(wm = 0;wm < 16;wm++)
{
WriteDat(F16x16[adder]);
adder += 1;
}
OLED_SetPos(x,y + 1);
for(wm = 0;wm < 16;wm++)
{
WriteDat(F16x16[adder]);
adder += 1;
}
}
// ????????????????,????????“??——???——????”??????ascll.h?????(????)
//???????:x:?????
// y:???(??0-7)
// begin:????????????????ascll.c???????
// num:????????
// ????“??”,??????????????????0,1,???0,??????,??:x:0,y:2,begin:0,num:2
void OLED_ShowCN_STR(u8 x , u8 y , u8 begin , u8 num)
{
u8 i;
for(i=0;i<num;i++){OLED_ShowCN(i*16+x,y,i+begin);} //OLED????
}
// Parameters : x0,y0 -- ?????(x0:0~127, y0:0~7); x1,y1 -- ?????(???)???(x1:1~128,y1:1~8)
// Description : ??BMP??
void OLED_DrawBMP(unsigned char x0,unsigned char y0,unsigned char x1,unsigned char y1,unsigned char BMP[])
{
unsigned int j=0;
unsigned char x,y;
if(y1%8==0)
y = y1/8;
else
y = y1/8 + 1;
for(y=y0;y<y1;y++)
{
OLED_SetPos(x0,y);
for(x=x0;x<x1;x++)
{
WriteDat(BMP[j++]);
}
}
}
void OLED_ShowChar(u8 x,u8 y,u8 chr,u8 Char_Size)
{
unsigned char c=0,i=0;
c=chr-' ';//???????
if(x>128-1){x=0;y=y+2;}
if(Char_Size ==16)
{
OLED_SetPos(x,y);
for(i=0;i<8;i++)
WriteDat(F8X16[c*16+i]);
OLED_SetPos(x,y+1);
for(i=0;i<8;i++)
WriteDat(F8X16[c*16+i+8]);
}
else {
OLED_SetPos(x,y);
for(i=0;i<6;i++)
WriteDat(F6x8[c][i]);
}
}
u32 oled_pow(u8 m,u8 n)
{
u32 result=1;
while(n--)result*=m;
return result;
}
//??2???
//x,y :????
//len :?????
//size:????
//mode:?? 0,????;1,????
//num:??(0~4294967295);
void OLED_ShowNum(u8 x,u8 y,u32 num,u8 len,u8 size2)
{
u8 t,temp;
u8 enshow=0;
for(t=0;t<len;t++)
{
temp=(num/oled_pow(10,len-t-1))%10;
if(enshow==0&&t<(len-1))
{
if(temp==0)
{
OLED_ShowChar(x+(size2/2)*t,y,' ',size2);
continue;
}else enshow=1;
}
OLED_ShowChar(x+(size2/2)*t,y,temp+'0',size2);
}
}

UART代码：

#include "uart.h"
uint8_t USART1_RX_BUF[USART1_REC_LEN];//????,??USART_REC_LEN???.
uint16_t USART1_RX_STA=0;//??????//bit15:??????,bit14~0:??????????
uint8_t USART1_NewData;//?????????1????????
extern int flag;
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)//????????
{
if(huart ==&huart1)
{
if((USART1_RX_STA&0x8000)==0)//?????
{
if(USART1_NewData==0x5A)//????0x5A
{
USART1_RX_STA|=0x8000; //?????,?USART2_RX_STA??bit15(15?)?1
}
else
{
USART1_RX_BUF[USART1_RX_STA&0X7FFF]=USART1_NewData;
if(USART1_RX_BUF[1] == 0x01)
{
flag = 2;
}
USART1_RX_STA++; //???????1
if(USART1_RX_STA>(USART1_REC_LEN-1))USART1_RX_STA=0;//??????,??????
}
}
HAL_UART_Receive_IT(&huart1,(uint8_t *)&USART1_NewData,1);
}
}

常规的编写如上，但是本人的MCU存在问题，单片机并未接收到预设的数据。

所以，本人项目中采用了下方代码：

#include "uart.h"
uint8_t USART1_RX_BUF[USART1_REC_LEN];//????,??USART_REC_LEN???.
uint16_t USART1_RX_STA=0;//??????//bit15:??????,bit14~0:??????????
uint8_t USART1_NewData;//?????????1????????
extern int flag;
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)//????????
{
if(huart ==&huart1)
{
USART1_RX_BUF[USART1_RX_STA&0X7FFF]=USART1_NewData;
USART1_RX_STA++; //???????1
if(USART1_RX_STA>(USART1_REC_LEN-1))USART1_RX_STA=0;//??????,??????
if(USART1_RX_BUF[USART1_RX_STA-4] == 0xA0)
{
flag = 1;
}
if(USART1_RX_BUF[USART1_RX_STA-4] == 0x90)
{
flag = 2;
}
if(USART1_RX_BUF[USART1_RX_STA-4] == 0xD0)
{
flag = 3;
}
if(USART1_RX_BUF[USART1_RX_STA-4] == 0x88)
{
flag = 4;
}
if(USART1_RX_BUF[USART1_RX_STA-4] == 0x48)
{
flag = 5;
}
HAL_UART_Receive_IT(&huart1,(uint8_t *)&USART1_NewData,1);
}
}

如果大家自己使用的花，可以根据自己的蓝牙APP写这段程序，有问题欢迎留言

Motor代码：

#include "motor.h"
void MOTOR_GO()
{
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1,3000);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,GPIO_PIN_SET);
}
void MOTOR_BACK()
{
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_SET);
}
void MOTOR_STOP()
{
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_RESET);
}
void MOTOR_UP()
{
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1,1);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,GPIO_PIN_SET);
}
void MOTOR_DOWN()
{
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1,400);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,GPIO_PIN_SET);
}

PID：

PID算法：

PID分为位置型和增量型

增量型即通过 u(k)-u(k-1) 从而得出式子：

公式的第一部分是比例式是为了让值按一定比例达到目标值；

第二部分是积分值，正值，在计算的过程中往往会受到环境等一些其他因素的影响，导致值不能到达目标值；

第三部分是微分值，通常是负值，后一次偏差值往往小于前一次偏差值，目的是为了防止值增加过大，通常起一个阻碍的作用；

84a8ac7636a44d7180089f9faadddebd~noop.image?_iz=58558&from=article.jpg

PID代码：

#include "pid.h"
#include "tim.h"
#include "main.h"
#include "math.h"
#include "i2c.h"
#include "oled.h"
unsigned int MotorSpeed; //È«¾Ö±äÁ¿£¬µç»úµ±Ç°×ªËÙ
int SpeedTarget = 750; //Ä¿±ê×ªËÙ
int MotorOutput; //µç»úÊä³ö
//1.ÀûÓÃTIM2¼ÆËãµç»ú×ªËÙ
void GetMotorSpeed(void)
{
// int CaptureNumber = (short)__HAL_TIM_GET_COUNTER(&htim2); //HAL¿âº¯Êý¼ÆËãÂö³å´ÎÊý
//
// //µç»ú×ªËÙ×ª»»Speed=1sÄÚµÄÂö³åÊý/44(Ò»È¦11¸öÐÅºÅ£¬4±¶Æµ·¨)/34¼õËÙ±È
// int MotorSpeed=CaptureNumber*20/44/34*2*3.14*3;
// OLED_ShowNum(40,0,MotorSpeed,4,16);
//
// __HAL_TIM_GET_COUNTER(&htim2) = 0; //¼ÆÊýÆ÷ÇåÁã
int CaptureNumber = (short)__HAL_TIM_GET_COUNTER(&htim2); //???????
__HAL_TIM_GET_COUNTER(&htim2) = 0;
// int Speed=CaptureNumber*5/44/34*2*3.14*3;
int Direction = __HAL_TIM_IS_TIM_COUNTING_DOWN(&htim2);
if(Direction == 1)
{
CaptureNumber -= 65535;
}
MotorSpeed=CaptureNumber;
OLED_ShowNum(40,0,MotorSpeed,4,16);
HAL_Delay(100);
OLED_CLS();
// __HAL_TIM_GET_COUNTER(&htim2) = 0;
}
int Error_Last,Error_Prev; //ÉÏ´ÎÎó²î£¬ÉÏÉÏ´ÎÎó²î
int Pwm_add,Pwm; //PWMÔöÁ¿,PWMÕ¼¿Õ±È
int Kp = 5, Ki = 3, Kd = 1;//PIDËã·¨ÏµÊý£¬¸¡µãÀàÐÍ£¬Ð¾Æ¬¼ÆËãÄÜÁ¦Ò»°ãÊ±½¨ÒéÕûÐÍ£¬»òÕß*1024
int SpeedInnerControl(int Speed,int Target) //ËÙ¶ÈÄÚ»·¿ØÖÆ
{
int Error = Target - Speed; //Îó²î = Ä¿±êËÙ¶È - Êµ¼ÊËÙ¶È
Pwm_add = Kp * (Error - Error_Last) + //±ÈÀý
Ki * Error + //»ý·Ö
Kd * (Error - 2.0f * Error_Last + Error_Prev); //Î¢·Ö
Pwm += Pwm_add; //Êä³öÁ¿=ÔÊ¼Á¿+ÔöÁ¿
Error_Prev = Error_Last; //±£´æÉÏÉÏ´ÎÎó²î
Error_Last = Error; //±£´æÉÏ´ÎÎó²î
if(Pwm > 4999) Pwm = 3000; //ÏÞÖÆÉÏÏÂÏÞ£¬·ÀÖ¹PWM³¬³öÁ¿³Ì
if(Pwm <-4999) Pwm =-3000;
return Pwm; //·µ»ØÊä³öÖµ
}
void SetMotorVoltageAndDirection(int Pwm)
{
if(Pwm < 0) //Èç¹ûPWMÐ¡ÓÚ0
{
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_SET);
Pwm = (-Pwm); //PWMÖ»ÄÜÈ¡ÕýÖµ£¬Èç¹ûÎª¸ºÊý£¬Ö±½ÓÈ¡·´
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, Pwm); //PWMµ÷ËÙ
} else
{
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,GPIO_PIN_SET);
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, Pwm); //PWMµ÷ËÙ
}
}
void ModePID()
{
GetMotorSpeed();
MotorOutput = SpeedInnerControl(MotorSpeed,SpeedTarget);
SetMotorVoltageAndDirection(MotorOutput);
}