东阳营销型网站建设品牌,包工头接活app哪个好,网站建设营销的企业,清远市住房和城乡建设局门户网站从接触编码器到调出好康的PID波形认识电机及编码器只需动动手指#xff0c;就能让STM32得到电机转过的角度让电机转起来认识PID控制①比例调节器②积分调节③微分调节④比例积分微分调节数字PID调节器#xff08;1#xff09;数字PID位置型控制算法#xff08;2#xff09…
从接触编码器到调出好康的PID波形认识电机及编码器只需动动手指就能让STM32得到电机转过的角度让电机转起来认识PID控制①比例调节器②积分调节③微分调节④比例积分微分调节数字PID调节器1数字PID位置型控制算法2数字PID增量型控制算法PID控制器代码在程序中加入PID控制凑试法整定PID参数两个改进PID控制器的技巧积分分离加入死区最终的 带积分分离及死区 的改进型PID算法PID.cPID.h修正Encoder.cEncoder.h本文所用编程环境STM32 Cube IDE 1.5.0认识电机及编码器 这种灰头土脸的直流减速电机应该是各大科创比赛上最常见的了。 它们采用的是增量式编码器价格低廉。 不论是光电编码器还是霍尔编码器都会产生一对正交的脉冲信号来间接告诉我们电机状况。 只要按照一定规则对这对正交信号解读就能得到我们所需的信息。 幸运的是许多微控制器都带有硬件解码电路大大减弱了我们上手的难度。 两个正交编码脉冲输入信号的两个边沿均被正交编码脉冲电路计数因此由其产生的时钟频率是每个输入序列频率的4倍这个时钟将作为计数器的时钟源信号。
只需动动手指就能让STM32得到电机转过的角度
首先选择你所用的芯片本文所用的开发板是野火的指南者STM32F103VET6 输入工程名称后将高速时钟的时钟源设为外部晶振。 别忘了将debug选项与你的调试方法适配如果用的是普通的U盘状ST-Link V2选Serial Wire就行。 在时钟树中将系统时钟源锁相环倍频后的时钟信号并将HCLK设为一个合理的值按下回车软件会自动计算各部分所需的分频系数、预定标系数。 根据开发板引脚情况激活一个带有编码器模式的定时器设定其装载值、使能自动重装载并将编码器模式设为双通道模式以实现4倍分辨率的提升。本文所用的编码器线数为13ppr故当装载值设为13-1时可在溢出中断时得到整数的角度分化。 使能所用定时器的中断。
点击左上角的齿轮符号生成配置好的工程文件。 在主函数中加入几行代码以开始编码器工作模式。 /* Initialize all configured peripherals */MX_GPIO_Init();MX_TIM3_Init();/* USER CODE BEGIN 2 */HAL_TIM_Encoder_Start(htim3,TIM_CHANNEL_ALL); //编码器模式启动写ALL为开始该定时器TIMx的通道1和通道2编码器模式可自动计算。使用编码器模式不用输入捕获HAL_TIM_Encoder_Start_IT(htim3,TIM_CHANNEL_ALL); //开启中断__HAL_TIM_ENABLE_IT(htim3,TIM_IT_UPDATE); //使能更新中断htim3.Instance-CNT 0;/* USER CODE END 2 *//* Infinite loop *//* USER CODE BEGIN WHILE */while (1){/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}/* USER CODE END 3 */
}暂时只需将编码器电路的5V、GND、AB相接好。本文将A相接入PA6B相接入PA7。 把调试器接上后点击debug和继续图标开始调试。
在现场表达式窗口中监看htim3.Instance-CNT的计数值轻轻拨动码盘 可以发现码盘每正向转动约1/4圈计数值将从0至12循环变化。 接下来就是将计数值转化为主轴转过的角度了 加入下列程序 其中void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)是弱定义修饰的函数需要我们给出其实现。
/* USER CODE BEGIN 4 */int Angle 0;
int Target_Angle 180;
const int Step_Angle 360/4/30; //每两次计数器溢出中断时主轴转过的角度??
void Motor_Get_Angle(TIM_HandleTypeDef *htim) //放到HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)中
{if(htim htim3){if((htim-Instance-CR1 0x0010)4) //查询相应控制寄存器的第4位以判断转向Angle - Step_Angle;elseAngle Step_Angle;}
}/*** brief Period elapsed callback in non-blocking mode* param htim TIM handle* retval None*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{Motor_Get_Angle(htim);/* Prevent unused argument(s) compilation warning */UNUSED(htim);/* NOTE : This function should not be modified, when the callback is needed,the HAL_TIM_PeriodElapsedCallback could be implemented in the user file*/
}/* USER CODE END 4 */本文中所用的电机减速比为30电机机身上也写着30F故码盘每转动30圈主轴转动1圈。
又由于我们采用正交编码电路解码分辨率提高了4倍因此每两次同向溢出间的步进角度 Step_Angle 360/4/30 3° 通过查阅手册得知我们可以通过查询相应控制寄存器的第4位以判断转向。 再次进入debug将Angle加入监看转动主轴可以发现主轴的角度成功地解读出来了。 使能串口1 加入下列代码重定向以使用printf函数。
/* Private variables ---------------------------------------------------------*/
TIM_HandleTypeDef htim3;UART_HandleTypeDef huart1;/* USER CODE BEGIN PV */
#include stdio.h#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif
PUTCHAR_PROTOTYPE
{HAL_UART_Transmit(huart1, (uint8_t *)ch, 1, 0xFFFF);return ch;
}
/* USER CODE END PV *//* Private function prototypes -----------------------------------------------*/在main的while循环中加入下列代码就可以通过串口在上位机上显示角度。 /* Infinite loop *//* USER CODE BEGIN WHILE */while (1){extern int Angle;printf(%d\r\n, Angle);HAL_Delay(10);/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}/* USER CODE END 3 */本文所用的上位机是VOFA绘图很方便。 在控件页面中拖入一个波形图控件 在协议与连接模块中配置串口信息点击左上角的小圆点连接之后在波形图中将数据加入y轴。 转转主轴按下右下角的Auto键拖动下方的滚动条设置窗口就能得到编码器返回的波形
让电机转起来
单片机的IO口一般所能输出的是0~3.3/5V的数字离散信号。 因此如果我们人为地控制输出信号中的高电平和低电平比例就能产生平均电压可控的控制信号。 文本所用的驱动模块是经典的L298N当输入的两路控制信号不同时输出也不同。当输入为TTL高电平对应的输出便是高驱动能力的工作电压可以将L298N视为一个电平转换器将0~3.3/5V的TTL信号转变为0—12V的驱动信号。 若输入10的控制信号则输出的两路间的电压差为12V反之输入01的控制信号输出的两路间的电压差就为-12V从而实现控制电机的正反转。 根据开发板选择定时器产生PWM波形设置定时器时钟源为内部时钟使能要用的通道设置预分频系数和重装载值并使能自动重装载。 由于定时器时钟均为72MHz故当预分频系数设为72-1重装载值设为1000-1就可得到72M/72/10001000Hz的PWM信号分辨率为1000。 本文中将PC6、PC7接入L298N的输入端输出端接入电机线、电机线-将开发板与L298N共地。 在主循环前加入下列代码就能产生占空比为50%的控制信号令电机转动。 HAL_TIM_PWM_Start(htim8, TIM_CHANNEL_1);HAL_TIM_PWM_Start(htim8, TIM_CHANNEL_2);__HAL_TIM_SetCompare(htim8, TIM_CHANNEL_1, 500);__HAL_TIM_SetCompare(htim8, TIM_CHANNEL_2, 0);认识PID控制
PID控制是一种负反馈控制 PID调节器是一种线性调节器这种调节器是将设定值r和实际输出值y进行比较构成控制偏差er-y并将其比例、积分和微分通过线性组合构成控制量。如图 在实际应用中根据对象的特性和控制要求也可灵活地改变其结构取其中一部分环节构成控制规律例如PPIPD等。
①比例调节器
最简单的一种调节器 控制规律
其中Kp为比例系数U0是控制量的基准也就是当误差e(t)0时的控制作用比如阀门的起始开度、基准的信号等 特点有差调节只要偏差出现就能及时地产生与之成比例的调节作用具有调节及时的特点。 偏差e的大小受比例系数的影响。 比例作用迅速反应误差加大比例系数可以减小静差但不能消除稳态误差过大容易引起不稳定。 阶跃响应特性曲线
②积分调节
所谓积分作用是指调节器的输出与输入偏差的积分成比例的作用 控制规律 其中S0为积分速度。 特点①无差调节 ②稳定性变差积分引入了-90度相角。 积分作用消除静差积分作用太强容易引起超调甚至出现振荡。 积分作用响应曲线
③微分调节 微分作用减小超调克服振荡提高稳定性改善系统动态特性。 微分作用响应曲线
④比例积分微分调节 比例控制能迅速反应误差偏差一旦产生控制器立即产生控制作用从而减小误差但比例控制不能消除稳态误差KP的加大会引起系统的不稳定 积分控制主要用于消除静差提高系统的无差度。只要系统存在误差积分控制作用就不断地积累输出控制量以消除误差因而只要有足够的时间积分控制将能完全消除误差积分作用太强会使系统超调加大甚至使系统出现振荡 微分环节能反映偏差信号的变化趋势并能在偏差信号值变得太大之前在系统中引入一个有效的早期修正信号加快系统的动态响应速度减小调整时间同时可以减小超调量克服振荡使系统的稳定性提高从而改善系统的动态性能。
优点
技术成熟。 P、I、D三个参数的优化配置 兼顾了动态过程的现在、过去与将来的信息使动态过程快速、平稳和准确算法简单易被人们熟悉和掌握不需要建立对象数学模型控制效果好适应性好鲁棒性强
数字PID调节器
用数值逼近的方法实现PID控制规律。 数值逼近的方法当采样周期相当短时用求和代替积分、用后向差分代替微分使模拟PID离散化为差分方程。
1数字PID位置型控制算法 可得
位置型控制算法提供执行机构的位置u(k)比如阀门的开度。
2数字PID增量型控制算法
根据位置型控制算法写出u(k-1) u(k)- u(k-1)可得 增量型控制算法提供执行机构的增量△ u(k)比如步进电机的步数。 在控制系统中 ①如执行机构采用调节阀则控制量对应阀门的开度表征了执行机构的位置此时控制器应采用数字PID位置式控制算法 ②如执行机构采用步进电机每个采样周期控制器输出的控制量是相对于上次控制量的增加此时控制器应采用数字PID增量式控制算法 增量式控制算法的优点 (1)增量算法不需要做累加控制量增量的确定仅与最近几次误差采样值有关计算误差或计算精度问题对控制量的计算影响较小。而位置算法要用到过去的误差的累加值容易产生大的累加误差。 (2)增量式算法得出的是控制量的增量例如阀门控制中只输出阀门开度的变化部分误动作影响小必要时通过逻辑判断限制或禁止本次输出不会严重影响系统的工作。而位置算法的输出是控制量的全量输出误动作影响大。 (3)采用增量算法易于实现手动到自动的无冲击切换。 增量式PID控制算法与位置式PID控制算法相比有下列缺点 (1) 积分截断效应大有静态误差 (2) 溢出的影响大。
PID控制器代码
取自大疆Robomaster 开发板C例程。 源文件
/******************************(C) COPYRIGHT 2019 DJI***************************** file pid.c/h* brief pid实现函数包括初始化PID计算函数* note* history* Version Date Author Modification* V1.0.0 Dec-26-2018 RM 1. 完成*verbatimendverbatim****************************(C) COPYRIGHT 2019 DJI*****************************/#include ../PID/PID.h#include main.h#define LimitMax(input, max) \{ \if (input max) \{ \input max; \} \else if (input -max) \{ \input -max; \} \}/*** brief pid struct data init* param[out] pid: PID struct data point* param[in] mode: PID_POSITION: normal pid* PID_DELTA: delta pid* param[in] PID: 0: kp, 1: ki, 2:kd* param[in] max_out: pid max out* param[in] max_iout: pid max iout* retval none*/
/*** brief pid struct data init* param[out] pid: PID结构数据指针* param[in] mode: PID_POSITION:普通PID* PID_DELTA: 差分PID* param[in] PID: 0: kp, 1: ki, 2:kd* param[in] max_out: pid最大输出* param[in] max_iout: pid最大积分输出* retval none*/
void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout)
{if (pid NULL || PID NULL){return;}pid-mode mode;pid-Kp PID[0];pid-Ki PID[1];pid-Kd PID[2];pid-max_out max_out;pid-max_iout max_iout;pid-Dbuf[0] pid-Dbuf[1] pid-Dbuf[2] 0.0f;pid-error[0] pid-error[1] pid-error[2] pid-Pout pid-Iout pid-Dout pid-out 0.0f;
}/*** brief pid calculate* param[out] pid: PID struct data point* param[in] ref: feedback data* param[in] set: set point* retval pid out*/
/*** brief pid计算* param[out] pid: PID结构数据指针* param[in] ref: 反馈数据* param[in] set: 设定值* retval pid输出*/
fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set)
{if (pid NULL){return 0.0f;}pid-error[2] pid-error[1];pid-error[1] pid-error[0];pid-set set;pid-fdb ref;pid-error[0] set - ref;if (pid-mode PID_POSITION){pid-Pout pid-Kp * pid-error[0];pid-Iout pid-Ki * pid-error[0];pid-Dbuf[2] pid-Dbuf[1];pid-Dbuf[1] pid-Dbuf[0];pid-Dbuf[0] (pid-error[0] - pid-error[1]);pid-Dout pid-Kd * pid-Dbuf[0];LimitMax(pid-Iout, pid-max_iout);pid-out pid-Pout pid-Iout pid-Dout;LimitMax(pid-out, pid-max_out);}else if (pid-mode PID_DELTA){pid-Pout pid-Kp * (pid-error[0] - pid-error[1]);pid-Iout pid-Ki * pid-error[0];pid-Dbuf[2] pid-Dbuf[1];pid-Dbuf[1] pid-Dbuf[0];pid-Dbuf[0] (pid-error[0] - 2.0f * pid-error[1] pid-error[2]);pid-Dout pid-Kd * pid-Dbuf[0];pid-out pid-Pout pid-Iout pid-Dout;LimitMax(pid-out, pid-max_out);}return pid-out;
}/*** brief pid out clear* param[out] pid: PID struct data point* retval none*/
/*** brief pid 输出清除* param[out] pid: PID结构数据指针* retval none*/
void PID_clear(pid_type_def *pid)
{if (pid NULL){return;}pid-error[0] pid-error[1] pid-error[2] 0.0f;pid-Dbuf[0] pid-Dbuf[1] pid-Dbuf[2] 0.0f;pid-out pid-Pout pid-Iout pid-Dout 0.0f;pid-fdb pid-set 0.0f;
}
头文件
/******************************(C) COPYRIGHT 2016 DJI***************************** file pid.c/h* brief pid实现函数包括初始化PID计算函数* note* history* Version Date Author Modification* V1.0.0 Dec-26-2018 RM 1. 完成*verbatimendverbatim****************************(C) COPYRIGHT 2016 DJI*****************************/
#ifndef PID_H
#define PID_H#include main.htypedef float fp32;
typedef double fp64;enum PID_MODE
{PID_POSITION 0,PID_DELTA
};typedef struct
{uint8_t mode;//PID 三参数fp32 Kp;fp32 Ki;fp32 Kd;fp32 max_out; //最大输出fp32 max_iout; //最大积分输出fp32 set;fp32 fdb;fp32 out;fp32 Pout;fp32 Iout;fp32 Dout;fp32 Dbuf[3]; //微分项 0最新 1上一次 2上上次fp32 error[3]; //误差项 0最新 1上一次 2上上次} pid_type_def;
/*** brief pid struct data init* param[out] pid: PID struct data point* param[in] mode: PID_POSITION: normal pid* PID_DELTA: delta pid* param[in] PID: 0: kp, 1: ki, 2:kd* param[in] max_out: pid max out* param[in] max_iout: pid max iout* retval none*/
/*** brief pid struct data init* param[out] pid: PID结构数据指针* param[in] mode: PID_POSITION:普通PID* PID_DELTA: 差分PID* param[in] PID: 0: kp, 1: ki, 2:kd* param[in] max_out: pid最大输出* param[in] max_iout: pid最大积分输出* retval none*/
extern void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout);/*** brief pid calculate* param[out] pid: PID struct data point* param[in] ref: feedback data* param[in] set: set point* retval pid out*/
/*** brief pid计算* param[out] pid: PID结构数据指针* param[in] ref: 反馈数据* param[in] set: 设定值* retval pid输出*/
extern fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set);/*** brief pid out clear* param[out] pid: PID struct data point* retval none*/
/*** brief pid 输出清除* param[out] pid: PID结构数据指针* retval none*/
extern void PID_clear(pid_type_def *pid);#endif
PID_Init 函数的功能为设置 PID 控制器的模式各项系数最大输出值和积分上限最后将 PID 控制器的输入和输出均初始化为 0。 而计算 PID 使用的 PID_calc 函数则首先区分 PID 的模式是位置式还是增量式位置式则使 用公式 增量式则使用公式
在程序中加入PID控制
/* USER CODE BEGIN Includes */
#include ../PID/PID.h
#include math.h#define Motor_KP 0
#define Motor_KI 0
#define Motor_KD 0pid_type_def Motor_PID;
const static fp32 motor_pid[3] {Motor_KP, Motor_KI, Motor_KD};
/* USER CODE END Includes */HAL_TIM_PWM_Start(htim8, TIM_CHANNEL_1);HAL_TIM_PWM_Start(htim8, TIM_CHANNEL_2);PID_init(Motor_PID, PID_POSITION, motor_pid, 1000, 1000);/* USER CODE END 2 *//* Infinite loop *//* USER CODE BEGIN WHILE */while (1){extern int Angle;extern int Target_Angle;PID_calc(Motor_PID, Angle, Target_Angle);if(Motor_PID.out 0){__HAL_TIM_SetCompare(htim8, TIM_CHANNEL_1, Motor_PID.out); __HAL_TIM_SetCompare(htim8, TIM_CHANNEL_2, 0);}else{__HAL_TIM_SetCompare(htim8, TIM_CHANNEL_1, 0);__HAL_TIM_SetCompare(htim8, TIM_CHANNEL_2, -Motor_PID.out);}printf(%d,%d\r\n, Angle, Target_Angle);HAL_Delay(10);/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}进入debug模式监看Target_Angle及Motor_PID结构体 回到VOFA中将Target_Angle加入波形图
凑试法整定PID参数 根据Kp,i,d对控制过程的影响趋势参数整定采用先比例后积分再微分的整定步骤。 ①首先整定比例部分系数由小变大得到反应快超调小的响应曲线。如果系统已满足静差要求则直接使用比例即可。 比例项 Kp 过小时PID 控制器的反应速度较慢且存在静差。静差是指控制器的最终输出保 持为一个和期望值存在一定误差的值引发静差的原因时由于比例控制的输出和误差成线性 关系如果当误差值减小时比例控制器的输出值同样会减少导致比例控制器不可能达到 和期望值完全相同即误差值为零的情况因为如果误差值为零则比例控制器的输出也会为 零 直接在监看表达式中修改Kp的值 慢慢增大Kp并修改Target_Angle以观察响应曲线。
②比例调节系统静差不能满足要求时加入积分环节。所要达到的目的消除静差效果好。 ③经上两步后动态过程仍不能令人满意时可加d环节d由小到大变化。 先取d为零逐步增大d同时改变比例参数和积分时间直到系统得到好的动态性能和效果。 我们发现加入微分环节后超调量得到了一定程度的抑制控制但效果仍不理想这是积分环节造成的。
两个改进PID控制器的技巧
积分控制主要用于消除静差提高系统的无差度。积分作用太强会使系统超调加大甚至使系统出现振荡。
积分分离
改进原因在过程的启动、结束或大幅度增减设定值时短时间内系统输出有很大的偏差会造成PID运算的积分积累。由于系统的惯性和滞后在积分累积项的作用下往往会产生较大的超调和长时间的波动。特别对于温度、成份等变化缓慢的过程这一现象更为严重。 改进思路当被控量和给定值偏差大时取消积分控制以免超调量过大当被控量和给定值接近时积分控制投入消除静差。 改进方法 当 |e(k)| β时采用PD控制 当 |e(k)| β时采用PID控制。 对于积分分离应该根据具体对象及控制要求合理的选择阈值β 积分分离阈值β的确定 β过大达不到积分分离的目的 β过小则一旦被控量y(t)无法跳出各积分分离区只进行PD控制将会出现残差。 while (1){extern int Angle;extern int Target_Angle;if(fabs(Target_Angle-Angle)15){Motor_PID.Ki 0;Motor_PID.Iout 0;}elseMotor_PID.Ki Motor_KI;PID_calc(Motor_PID, Angle, Target_Angle);if(Motor_PID.out 0){__HAL_TIM_SetCompare(htim8, TIM_CHANNEL_1, Motor_PID.out);__HAL_TIM_SetCompare(htim8, TIM_CHANNEL_2, 0);}else{__HAL_TIM_SetCompare(htim8, TIM_CHANNEL_1, 0);__HAL_TIM_SetCompare(htim8, TIM_CHANNEL_2, -Motor_PID.out);}printf(%d,%d\r\n, Angle, Target_Angle);HAL_Delay(10);/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}加入积分分离后再调节就很容易得到控制效果不错的波形了
加入死区
改进原因在精度不高的场合为了避免控制动作过于频繁以消除由于频繁动作所引起的振荡和能量消耗有时采用带有死区的PID控制系统。 由于本实验中所测电机角度分辨率为3°为避免当误差较小时频繁动作加入死区。 /* Infinite loop *//* USER CODE BEGIN WHILE */while (1){extern int Angle;extern int Target_Angle;if(fabs(Target_Angle-Angle)15){Motor_PID.Ki 0;Motor_PID.Iout 0;PID_calc(Motor_PID, Angle, Target_Angle);}else if(fabs(Target_Angle-Angle)3){PID_calc(Motor_PID, Angle, Angle);}else{Motor_PID.Ki Motor_KI;PID_calc(Motor_PID, Angle, Target_Angle);}if(Motor_PID.out 0){__HAL_TIM_SetCompare(htim8, TIM_CHANNEL_1, Motor_PID.out);__HAL_TIM_SetCompare(htim8, TIM_CHANNEL_2, 0);}else{__HAL_TIM_SetCompare(htim8, TIM_CHANNEL_1, 0);__HAL_TIM_SetCompare(htim8, TIM_CHANNEL_2, -Motor_PID.out);}printf(%d,%d\r\n, Angle, Target_Angle);HAL_Delay(10);/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}/* USER CODE END 3 */别忘了把调好的参数写回去
#define Motor_KP 15
#define Motor_KI 0.2
#define Motor_KD 75最终的 带积分分离及死区 的改进型PID算法
PID.c
/******************************(C) COPYRIGHT 2019 DJI***************************** file pid.c/h* brief pid实现函数包括初始化PID计算函数* note* history* Version Date Author Modification* V1.0.0 Dec-26-2018 RM 1. 完成*verbatimendverbatim****************************(C) COPYRIGHT 2019 DJI*****************************/#include PID.h
#include math.h#define NULL 0#define LimitMax(input, max) \{ \if (input max) \{ \input max; \} \else if (input -max) \{ \input -max; \} \}/*** brief pid struct data init* param[out] pid: PID struct data point* param[in] mode: PID_POSITION: normal pid* PID_DELTA: delta pid* param[in] PID: 0: kp, 1: ki, 2:kd* param[in] max_out: pid max out* param[in] max_iout: pid max iout* retval none*/
/*** brief pid struct data init* param[out] pid: PID结构数据指针* param[in] mode: PID_POSITION:普通PID* PID_DELTA: 差分PID* param[in] PID: 0: kp, 1: ki, 2:kd* param[in] max_out: pid最大输出* param[in] max_iout: pid最大积分输出* retval none*/
void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout, fp32 Integral_Separation_Threshold, fp32 Dead_Zone)
{if (pid NULL || PID NULL){return;}pid-mode mode;pid-Kp PID[0];pid-Ki PID[1];pid-Kd PID[2];pid-max_out max_out;pid-max_iout max_iout;pid-Dbuf[0] pid-Dbuf[1] pid-Dbuf[2] 0.0f;pid-error[0] pid-error[1] pid-error[2] pid-Pout pid-Iout pid-Dout pid-out 0.0f;pid-Integral_Separation_Threshold Integral_Separation_Threshold;pid-Dead_Zone Dead_Zone;
}/*** brief pid calculate* param[out] pid: PID struct data point* param[in] ref: feedback data* param[in] set: set point* retval pid out*/
/*** brief pid计算* param[out] pid: PID结构数据指针* param[in] ref: 反馈数据* param[in] set: 设定值* retval pid输出*/
fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set)
{if (pid NULL){return 0.0f;}pid-error[2] pid-error[1];pid-error[1] pid-error[0];pid-set set;pid-fdb ref;pid-error[0] set - ref;if(fabs(pid-error[0]) pid-Dead_Zone)pid-error[0] 0;if (pid-mode PID_POSITION){pid-Pout pid-Kp * pid-error[0];if(fabs(pid-error[0]) pid-Integral_Separation_Threshold)pid-Iout pid-Ki * pid-error[0];elsepid-Iout 0;pid-Dbuf[2] pid-Dbuf[1];pid-Dbuf[1] pid-Dbuf[0];pid-Dbuf[0] (pid-error[0] - pid-error[1]);pid-Dout pid-Kd * pid-Dbuf[0];LimitMax(pid-Iout, pid-max_iout);pid-out pid-Pout pid-Iout pid-Dout;LimitMax(pid-out, pid-max_out);}else if (pid-mode PID_DELTA){pid-Pout pid-Kp * (pid-error[0] - pid-error[1]);if(fabs(pid-error[0]) pid-Integral_Separation_Threshold)pid-Iout pid-Ki * pid-error[0];elsepid-Iout 0;pid-Dbuf[2] pid-Dbuf[1];pid-Dbuf[1] pid-Dbuf[0];pid-Dbuf[0] (pid-error[0] - 2.0f * pid-error[1] pid-error[2]);pid-Dout pid-Kd * pid-Dbuf[0];pid-out pid-Pout pid-Iout pid-Dout;LimitMax(pid-out, pid-max_out);}return pid-out;
}/*** brief pid out clear* param[out] pid: PID struct data point* retval none*/
/*** brief pid 输出清除* param[out] pid: PID结构数据指针* retval none*/
void PID_clear(pid_type_def *pid)
{if (pid NULL){return;}pid-error[0] pid-error[1] pid-error[2] 0.0f;pid-Dbuf[0] pid-Dbuf[1] pid-Dbuf[2] 0.0f;pid-out pid-Pout pid-Iout pid-Dout 0.0f;pid-fdb pid-set 0.0f;
}
PID.h
/******************************(C) COPYRIGHT 2016 DJI***************************** file pid.c/h* brief pid实现函数包括初始化PID计算函数* note* history* Version Date Author Modification* V1.0.0 Dec-26-2018 RM 1. 完成*verbatimendverbatim****************************(C) COPYRIGHT 2016 DJI*****************************/
#ifndef PID_H
#define PID_H#include stdint.htypedef float fp32;
typedef double fp64;enum PID_MODE
{PID_POSITION 0,PID_DELTA
};typedef struct
{uint8_t mode;//PID 三参数fp32 Kp;fp32 Ki;fp32 Kd;fp32 max_out; //最大输出fp32 max_iout; //最大积分输出fp32 set;fp32 fdb;fp32 out;fp32 Pout;fp32 Iout;fp32 Dout;fp32 Dbuf[3]; //微分项 0最新 1上一次 2上上次fp32 error[3]; //误差项 0最新 1上一次 2上上次fp32 Integral_Separation_Threshold;fp32 Dead_Zone;} pid_type_def;
/*** brief pid struct data init* param[out] pid: PID struct data point* param[in] mode: PID_POSITION: normal pid* PID_DELTA: delta pid* param[in] PID: 0: kp, 1: ki, 2:kd* param[in] max_out: pid max out* param[in] max_iout: pid max iout* retval none*/
/*** brief pid struct data init* param[out] pid: PID结构数据指针* param[in] mode: PID_POSITION:普通PID* PID_DELTA: 差分PID* param[in] PID: 0: kp, 1: ki, 2:kd* param[in] max_out: pid最大输出* param[in] max_iout: pid最大积分输出* param[in] Integral_Separation_Threshold :积分分离阈值* param[in] Dead_Zone :死区* retval none*/
extern void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout, fp32 Integral_Separation_Threshold, fp32 Dead_Zone);/*** brief pid calculate* param[out] pid: PID struct data point* param[in] ref: feedback data* param[in] set: set point* retval pid out*/
/*** brief pid计算* param[out] pid: PID结构数据指针* param[in] ref: 反馈数据* param[in] set: 设定值* retval pid输出*/
extern fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set);/*** brief pid out clear* param[out] pid: PID struct data point* retval none*/
/*** brief pid 输出清除* param[out] pid: PID结构数据指针* retval none*/
extern void PID_clear(pid_type_def *pid);#endif
修正
四倍频时轮子转动一圈的脉冲数为4×减速比×编码器线数
其他则轮子转动一圈的脉冲数为2×减速比×编码器线数 可将其设为计数上限方便后续计算 500线减速比为27的电机
Encoder.c
/** Encoder.c** Created on: Jul 22, 2021* Author: Royic*/#include Encoder.h
#include math.hint32_t Wheel_Count[4] {0};
float Wheel_Speed[4] {0};void Encoder_Init(TIM_HandleTypeDef *htim)
{htim-Instance-CNT0;HAL_TIM_Encoder_Start(htim,TIM_CHANNEL_ALL);//编码器模式启动写ALL为开始该定时器TIMx的通道1和通道2编码器模式可自动计算。使用编码器模式不用输入捕获HAL_TIM_Encoder_Start_IT(htim,TIM_CHANNEL_ALL);//开启中断__HAL_TIM_ENABLE_IT(htim,TIM_IT_UPDATE); //使能更新中断一定要使能这一句话困扰了我两天
}float Motor_Get_Angle(uint8_t Motor_ID)
{switch(Motor_ID){case 0:return (1 - (float)Encoder_1_Htim.Instance-CNT/(ENCODER_PPR * Reduction_Ratio * ENCODER_TIMES)) * 360;case 1:return (1 - (float)Encoder_2_Htim.Instance-CNT/(ENCODER_PPR * Reduction_Ratio * ENCODER_TIMES)) * 360;case 2:return (float)Encoder_3_Htim.Instance-CNT/(ENCODER_PPR * Reduction_Ratio * ENCODER_TIMES) * 360;case 3:return (float)Encoder_4_Htim.Instance-CNT/(ENCODER_PPR * Reduction_Ratio * ENCODER_TIMES) * 360;}return 0;
}void Motor_Get_Speed() //放到1ms的中断中
{uint8_t i;static int32_t Wheel_Count_Temp[4] {0};static float Wheel_Angle_Temp1[4] {0};static float Wheel_Angle_Temp2[4] {0};for(i 0; i 4; i){Wheel_Angle_Temp2[i] Motor_Get_Angle(i);Wheel_Speed[i] 1000 * ((Wheel_Count[i] - Wheel_Count_Temp[i]) * 360 Wheel_Angle_Temp2[i] - Wheel_Angle_Temp1[i]);Wheel_Count_Temp[i] Wheel_Count[i];Wheel_Angle_Temp1[i] Wheel_Angle_Temp2[i];}
}void Motor_Get_Turns(TIM_HandleTypeDef *htim) //放到HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)中
{if(htim Encoder_1_Htim){if((htim-Instance-CR1 16) 4){Wheel_Count[0];}else{--Wheel_Count[0];}}else if(htim Encoder_2_Htim){if((htim-Instance-CR1 16) 4){Wheel_Count[1];}else{--Wheel_Count[1];}}else if(htim Encoder_3_Htim){if((htim-Instance-CR1 16) 4){--Wheel_Count[2];}else{Wheel_Count[2];}}else if(htim Encoder_4_Htim){if((htim-Instance-CR1 16) 4){--Wheel_Count[3];}else{Wheel_Count[3];}}
}Encoder.h
/** Encoder.h** Created on: Jul 22, 2021* Author: Royic*/#ifndef ENCODER_ENCODER_H_
#define ENCODER_ENCODER_H_#include main.h#define ENCODER_TIMES 4
#define ENCODER_PPR 500
#define Reduction_Ratio 27#define Encoder_1_Htim htim4
extern TIM_HandleTypeDef Encoder_1_Htim;#define Encoder_2_Htim htim5
extern TIM_HandleTypeDef Encoder_2_Htim;#define Encoder_3_Htim htim2
extern TIM_HandleTypeDef Encoder_3_Htim;#define Encoder_4_Htim htim3
extern TIM_HandleTypeDef Encoder_4_Htim;extern int32_t Wheel_Count[];
extern float Wheel_Speed[4];void Encoder_Init(TIM_HandleTypeDef *htim);
void Motor_Get_Turns(TIM_HandleTypeDef *htim);//放到HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)中
float Motor_Get_Angle(uint8_t Motor_ID);
void Motor_Get_Speed(); //放到1ms的中断中#endif /* ENCODER_ENCODER_H_ */
