大疆M3508电机位置与速度PID控制及自编上位机调参

一. 简介

上一篇:大疆M3508电机使用CAN通信进行速度PID闭环控制详解,对官方代码进行了移植,分别使用CAN查询接收与CAN中断接收两种方式实现了电机的速度PID控制,去掉了官方的操作系统,代码精简易读不少。这一篇咱们再接再厉,利用M3508电机实现电机位置和速度控制,同时配合本人编写的上位机软件,实现电机位置、速度的实时显示,通过可视化界面实现电机的PID调参与位置、速度设置

本文环境:

  • Keil MDK5.14
  • STM32CubeMX6.2.1
  • Qt5.14
  • 开发板/芯片:正点原子阿波罗 / F767IGT6

实现功能:

  • 大疆M3508位置PID控制
  • 大疆M3508速度PID控制
  • 上位机参数实时显示与调参

STM32工程下载链接:


Qt上位机(发行)下载链接:

二. 电机位置、速度、电流三闭环PID控制原理

关于电机的三种模式的控制,在上一篇中已经提到过了,原理图如下图所示。通过编码器等位置传感器的检测,可以获得电机转子的角度,角度变化量/时间变化量就是角速度,这样就能获得电机的实际转速和实际位置,然后与期望位置或期望电流比较,就能实现电机的位置控制或速度控制。

img

为了能够直观的看到实际的控制效果,我们常常希望能够看到控制器的响应曲线,就像学习自动控制原理时一样,对设计的PID控制器,观察闭环系统的阶跃响应图,通过观察超调量、调节时间、稳态精度等来调整PID参数。所以我们希望有一个上位机,实时显示电机的期望位置或速度、实际位置或速度,通过图像来观察控制效果并调整参数。

下面这个界面就是我花了一个晚上写出来的上位机(基于Qt)。首先直观的感受下它的用法吧!下面两张图分别是速度控制和位置控制模式下的响应曲线,PID参数我已经调整过。速度响应是有一点超调的,但可以接受,位置响应没有超调,因为希望能够平稳的到达指定转角。(需要说明一下,位置环的响应看上去有点慢,但其实没办法了,电机的速度达不到那么快,事实上它已经全速往期望转角跑了,从曲线就可以看出来几乎是个斜直线,也就是匀速运动

请添加图片描述
在这里插入图片描述

界面上的各个按钮的功能都如界面所示了,应该是一目了然的(自认为是这样的~ )。主界面就是两部分,左侧是串口数据设置和连接单片机,以及设置电机模式、位置、速度数据,还有操作图像(设置显示时间的功能我没加,感觉没必要了~)。右侧是数据视图,有三个标签页,第一个Tab是电机数据,可以设置电机PID,还可以单独设置每个电机的模式和数值。第二个标签页是速度曲线,图像中四条曲线,分别是两个电机的期望速度和实际速度。第三个标签页是位置曲线,四条曲线分别是两个电机的期望位置和实际位置。因为写这篇文章我只连接了一个电机,所以只有一个电机的数据。

三. STM32实现位置、速度控制

当然啦,上位机只是辅助,它不是必须的,主要是学习如何通过单片机实现电机的三种模式控制,如果只关心电机控制的朋友们可以不看上位机的部分,包括后面单片机中的数传部分的内容都可以不看。下面开始实战,一步一步升级打怪。(只讲主要代码,完整工程请到我提供的链接下载)

(一)电机数据接收

电机数据接收采用中断方式,CAN中断函数如下:

void HAL_CAN_RxCpltCallback(CAN_HandleTypeDef* hcan)
{
    int i = 0;
	if (hcan->Instance == CAN1)
	{
		//CAN_Receive_IT()函数会关闭FIFO0消息挂号中断,因此我们需要重新打开
		__HAL_CAN_ENABLE_IT(&CAN1_Handler, CAN_IT_FMP0); //重新开启FIF00消息挂号中断
		i = hcan->pRxMsg->StdId - 0x201;

		switch(hcan->pRxMsg->StdId){
			case 0x201: 
			case 0x202: 
			case 0x203: 
			case 0x204: 
				if (moto_chassis[i].msg_cnt < 50)
				    get_moto_offset(&moto_chassis[i], hcan);		// 开始的时候读出的是偏置值
				if (moto_chassis[i].msg_cnt == 50)
				{
					get_moto_measure(&moto_chassis[i], hcan);
					moto_chassis[i].total_angle -= moto_chassis[i].offset_angle;	// 实际的位置 = 读到的位置 - 偏置值	
				}
				else 
					get_moto_measure(&moto_chassis[i], hcan);
				moto_chassis[i].msg_cnt++;
				break;
			default:break;
		}
	}
}

中断中根据报文ID将数据更新到对应编号的电机中,收到一次数据就将msg_cnt加一,标记收到的次数。这里根据msg_cnt做了一个判断,主要就是读电机位置的偏置值,因为电机上电时候的数据不是0,为什么不是0就要问卖电机的了,但是相对转角是准确没有问题的,所以后面读到的转角减去这个偏置就是准确的值了。

moto_chassis[4]是四个电机的数组,定义在motor.c和motor.h中,原型为moto_measure_t moto_chassis[4];,moto_measure_t是电机数据结构体,如下,主要关注三个值,total_angle,speed_rpm,given_current,分别是转角位置、速度、电流。

typedef struct{
	int16_t	 	speed_rpm;
    int16_t  	real_current;
    int16_t  	given_current;
    uint8_t  	hall;
	uint16_t 	angle;				//abs angle range:[0,8191]
	uint16_t 	last_angle;	//abs angle range:[0,8191]
	uint16_t	offset_angle;
	int32_t		round_cnt;
	int32_t		total_angle;
	u8			buf_idx;
	u16			angle_buf[FILTER_BUF_LEN];
	u16			fited_angle;
	u32			msg_cnt;
}moto_measure_t;

(二)一些全局变量

本来这个工程很简单的,由于要设计与上位机通信,所以需要设置很多全局变量,包括电机的模式、电机的位置速度数据、PID参数等,这些全局变量全部定义在transmission.ctransmission.h中。以下是这两个文件中的全局变量,

// transmission.h文件
#ifndef __TRANSMISSION_H
#define __TRANSMISSION_H
#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "motor.h"

typedef struct{
	float kp;
	float ki;
	float kd;
	float kpv;
	float kiv;
	float kdv;
}motorPara_t;

extern u8 control_mode;
extern float set_position[4];	// 电机位置全局变量
extern float set_speed[4]; 	// 电机速度全局变量
extern float set_current[4]; 	// 电机电流全局变量
extern motorPara_t motorPara[4];
extern motorPara_t defalutPara;

extern u8 pid1_change_flag;	// pid参数更改改了
extern u8 pid2_change_flag;

#define POSITION_MODE 0
#define SPEED_MODE 1
#define CURRENT_MODE 2

#endif


// transmission.c文件
#include "transmission.h"
#include "string.h"

u8 control_mode = 1;		// 默认是速度模式
motorPara_t motorPara[4];
motorPara_t defalutPara = {0.04, 0.00001, 0, 1.5, 0.1, 0};

float set_position[4];	// 电机位置全局变量
float set_speed[4]; 	// 电机速度全局变量
float set_current[4]; 	// 电机电流全局变量

u8 pid1_change_flag = 0;	// pid参数更改改了
u8 pid2_change_flag = 0;

motorPara_t结构体是电机PID参数,每个电机有一个,双环PID共六个参数,于是定义一个数组motorPara[4]。PID初始化要用它,同时定义了一个默认参数的结构defalutPara,调好参数后就改到这个里面,主函数里面的方法是将motorPara[4]全部初始化为defalutPara的值。

然后就是control_mode, 三种模式,使用宏定义,分别是POSITION_MODE,SPEED_MODE,CURRENT_MODE

再就是电机位置、速度、电流的全局变量,set_position[4],set_speed[4],set_current[4]。好了,准备工作做好了,下面进入正题.

(三)电机PID初始化与位置、速度PID计算

main函数的代码如下,注释都标注在里面了

int main(void)
{
	u32 run_cnt = 0;
	u8 i;
	pid_t pid_speed[4];		   //电机速度PID环
	pid_t pid_position[4];	//电机电流PID环

	float set_speed_temp;			   //加减速时的临时设定速度
	int16_t delta;					   //设定速度与实际速度的差值
	int16_t max_speed_change = 500;   //电机单次最大变化速度,加减速用
									   // 500经测试差不多是最大加速区间,即从零打到最大速度不异常的最大值
	Write_Through();		 //Cahce强制透写
	Cache_Enable();			 //打开L1-Cache
	HAL_Init();																	 //初始化HAL库
	Stm32_Clock_Init(432, 25, 2, 9); //设置时钟,216Mhz
	delay_init(216);															 //初始化延时函数
	
	uart1_init(115200);															 //初始化USART
	uart2_init(115200);															 //初始化USART
	uart3_init(115200);															 //初始化USART

	LED_Init();																	 //初始化LED
	KEY_Init();																	 //初始化按键
	CAN1_Mode_Init(CAN_SJW_1TQ, CAN_BS2_6TQ, CAN_BS1_11TQ, 3, CAN_MODE_NORMAL); //CAN初始化,波特率1000Kbps
	
	//PID初始化, 电机参数都设为默认参数
	for (i = 0; i < 4; i++)
	{
		memcpy(&motorPara[i], &defalutPara, 24);
	}
	
	//PID初始化
	for (i = 0; i < 4; i++)
	{
		PID_struct_init(&pid_position[i], POSITION_PID, 8000, 2000, motorPara[i].kp, motorPara[i].ki, motorPara[i].kd);   //4 motos angular position closeloop.  最大输出8000对应电机转速8000rpm
		PID_struct_init(&pid_speed[i], POSITION_PID, 16384, 16384, motorPara[i].kpv, motorPara[i].kiv, motorPara[i].kdv); //4 motos angular rate closeloop.
	}
	
	// 初始化期望位置、速度、电流都为0
	set_position[0] = set_position[1] = set_position[2] = set_position[3] = 0; 	// -8000-8000,双向转
	set_speed[0] = set_speed[1] = set_speed[2] = set_speed[3] = 0; 				// -8000-8000,双向转
	set_current[0] = set_current[1] = set_current[2] = set_current[3] = 0;		// -8000-8000,双向转
	
	while (1)
	{
		// 力矩控制模式
		if (control_mode == CURRENT_MODE)
		{
			set_moto_current(&CAN1_Handler, 0x200, set_current[0], set_current[1], set_current[2], set_current[3]);
		}
		
		// 速度控制模式
		if (control_mode == SPEED_MODE)
		{
			for (i = 0; i < 2; i++)
			{
				//加减速
				delta = (int16_t)set_speed[i] - moto_chassis[i].speed_rpm;
				if (delta > max_speed_change)
					set_speed_temp = (float)(moto_chassis[i].speed_rpm + max_speed_change);
				else if (delta < -max_speed_change)
					set_speed_temp = (float)(moto_chassis[i].speed_rpm - max_speed_change);
				else
					set_speed_temp = set_speed[i];
				pid_calc(&pid_speed[i], (float)moto_chassis[i].speed_rpm, set_speed_temp);
			}
			
			set_moto_current(&CAN1_Handler, 0x200, (s16)(pid_speed[0].pos_out),
							 (s16)(pid_speed[1].pos_out),
							 (s16)(pid_speed[2].pos_out),
							 (s16)(pid_speed[3].pos_out));
		}
		
		if (control_mode == POSITION_MODE)
		{		
			for(i = 0; i<2; i++)
			{
				// 位置环
				pid_calc(&pid_position[i], (float)moto_chassis[i].total_angle, set_position[i]);
				// 速度环
				pid_calc(&pid_speed[i], (float)moto_chassis[i].speed_rpm, pid_position[i].pos_out);
			}
			
			set_moto_current(&CAN1_Handler, 0x200, (s16)(pid_speed[0].pos_out),
							 (s16)(pid_speed[1].pos_out),
							 (s16)(pid_speed[2].pos_out),
							 (s16)(pid_speed[3].pos_out));
			
		} 
		
		if (pid1_change_flag)
		{
			PID_struct_init(&pid_position[i], POSITION_PID, 8000, 2000, motorPara[0].kp, motorPara[0].ki, motorPara[0].kd);   //4 motos angular position closeloop.  最大输出8000对应电机转速8000rpm
			PID_struct_init(&pid_speed[i], POSITION_PID, 16384, 16384, motorPara[0].kpv, motorPara[0].kiv, motorPara[0].kdv); //4 motos angular rate closeloop.
			pid1_change_flag = 0;
		}
		if (pid2_change_flag)
		{
			PID_struct_init(&pid_position[i], POSITION_PID, 8000, 2000, motorPara[1].kp, motorPara[1].ki, motorPara[1].kd);   //4 motos angular position closeloop.  最大输出8000对应电机转速8000rpm
			PID_struct_init(&pid_speed[i], POSITION_PID, 16384, 16384, motorPara[1].kpv, motorPara[1].kiv, motorPara[1].kdv); //4 motos angular rate closeloop.
			pid1_change_flag = 0;
		}
		
		
//		// 上传数据,20ms上传一次
		if (run_cnt % 4 == 0)
		{
			uploadMoto12Info();
			uploadControl12Info();
			uploadPID1Info();
			uploadPID2Info();
			printf("ki %f angle %d\r\n", motorPara[0].kiv, moto_chassis[0].total_angle);
		}
		
		if (run_cnt % 100 == 0)
		{
			LED0_Toggle;
			LED1_Toggle;
		}
		
		run_cnt++;
		delay_ms(5); // 采样周期
	}
 									  	       
}

main函数讲解:

第一步,硬件初始化,包括时钟、串口、CAN通信、LED等,同时定义了电机位置和速度的PID结构体数组pid_speed[4],pid_position[4],每个电机都有一个位置PID和一个速度PID。

第二步,PID初始化,将默认参数写入四个电机的PID结构体中,然后初始化对应的位置PID和速度PID, PID_struct_init中的参数按顺序分别为:PID结构体对象,PID模式(位置PID和增量PID)、PID输出限幅、PID积分限幅、Kp,Ki,Kd。

第三步,初始化期望位置、速度、电流都为0

第四步,进入while循环,控制周期5ms。while循环里面有四步:

(1)电机控制, 根据````control_mode```进行三种模式的控制。

电流模式下直接设置电流,调用set_moto_current,第一个参数0x200是地址,后面四个参数是四个电机的电流。

速度模式下调用pid_calc进行速度PID计算,pid_calc参数为PID对象、实际值、期望值,速度PID计算出的是电流,然后set_moto_current将电流送出去。

位置模式下调用pid_calc进行位置PID计算,位置PID计算出来的结果是速度, 然后将这个速度作为期望速度,调用pid_calc进行速度PID计算,速度PID计算出的是电流,然后set_moto_current将电流送出去。

(2)参数检测,PID参数修改。

这个部分是使用的标志位的方式,如果收到了上位机的修改参数的命令,对应的标志位就会置位,主函数判断这个标志位从而将收到的参数更改到电机,方法是重新调用PID_struct_init进行初始化。

(3)上传数据,每隔20ms上传一次(上位机显示的实际时间大于20ms,因为程序运行需要时间)

(4)LED灯闪烁

至此功能全部写完了,要让电机动起来,就需要给模式、位置、速度、电流。 这些数据通过上位机设置。当然了,上位机只是辅助,你完全可以自己设定值,设置位置模式+位置值,或者设置速度模式+速度值,直接烧进程序,电机就能动了,或者写一个按键控制,然后通过肉眼看电机的控制效果。眼睛当然是不可靠的啦,所以下面看看怎么用上位机来控制。

四. 上位机设计与通信

(一)通信协议

要实现上位机与下位机的通信,首先就要定义好通信协议,这样发送和接收数据按照协议去打包和解包就行了。为了简便,我这里统一每帧数据固定25字节,其实可以用不定长的数据帧,定长的数据帧方便之处在于解析的时候要方便一些。通信协议定义如下(本文仅以两个电机为例,上位机也只显示两个电机的数据):

数据方向 byte0,byte1(帧头) byte2(功能字) byte3(数据字节数) byte4~byte23(数据) byte24(校验和)
上行 0xAA, 0xFF 0x01(电机状态帧) 16 电机1:位置(4bytes) 速度(2bytes) 电流(2bytes)
电机2:位置(4bytes) 速度(2bytes) 电流(2bytes)
sum
上行 0xAA, 0xFF 0x03(控制帧) 20 电机1:当前模式(2bytes) 期望位置(4bytes) 期望速度(2bytes) 期望电流(2bytes)
电机2:当前模式(2bytes) 期望位置(4bytes) 期望速度(2bytes) 期望电流(2bytes)
sum
上行 0xAA, 0xFF 0x05(PID帧) 20 电机1:位置环Kp(2bytes) 位置环Ki(2bytes) 位置环Kd(2bytes) 速度环Kp(2bytes) 速度环Ki(2bytes) 速度环Kd(2bytes) sum
上行 0xAA, 0xFF 0x06(PID帧) 20 电机2:位置环Kp(2bytes) 位置环Ki(2bytes) 位置环Kd(2bytes) 速度环Kp(2bytes) 速度环Ki(2bytes) 速度环Kd(2bytes) sum
下行 0xAA,0x55 0x01(PID帧) 12 电机1:位置环Kp(2bytes) 位置环Ki(2bytes) 位置环Kd(2bytes) 速度环Kp(2bytes) 速度环Ki(2bytes) 速度环Kd(2bytes) sum
下行 0xAA,0x55 0x02(PID帧) 12 电机2:位置环Kp(2bytes) 位置环Ki(2bytes) 位置环Kd(2bytes) 速度环Kp(2bytes) 速度环Ki(2bytes) 速度环Kd(2bytes) sum
下行 0xAA,0x55 0x05(控制帧) 10 设定模式(2bytes) 设定位置(4bytes) 设定速度(2bytes) 设定电流(2bytes) sum

介绍如下:

byte0~byte1,帧头,帧头是数据起始,上行帧头为0xAA, 0xFF,下行帧头为0xAA,0x55

byte2,功能字,数据帧的类型,当交互数据比较多时用来区分不同的数据,本例程数据量少,上行数据只有一帧数据,用0x01,表示电机数据,下行数据用0x01,0x02,0x03分别设置电机1的pid数据,电机2的pid数据,电机的控制模式以及控制量

byte3,数据字节数,标记本帧数据有效数据长度,当使用不定长数据帧时这一数据至关重要,解包时就是根据它来决定往后读多少个数据。本文是使用固定25字节的数据帧,解包时就没有用它

byte4~byte23,数据部分,最大传输20个字节有效数据,没有用到的部分补零。不同数据帧的数据内容已经在表上标出了,这里要注意的是使用多个字节传输一个数据量时是小端模式,也就是高字节在后,打包和解包时一定要注意

byte24,校验和。解包时校验用。

(二)上传数据和解析数据

然后我们按照通信协议打包数据上传就行了,上传数据的内容在transmission.ctransmission.h中。以下是两个文件中的上传函数部分的内容(没有显示前面已经贴出的全局变量了)

// transmission.h文件

u8 ComSendData(u8 fun,u8* data,u8 len);

u8 uploadMoto12Info(void);
u8 uploadControl12Info(void);
u8 uploadPID1Info(void);
u8 uploadPID2Info(void);

void analyseData(u8 *data);
// transmission.c文件
8 ComSendData(u8 fun, u8 *data, u8 len)
{
	u8 send_buf[25] = {0};
	u8 i;
	if (len > 20) return 1;		   //最多20字节数据
	send_buf[0] = 0XAA;	   //帧头
	send_buf[1] = 0XFF;	   //帧头 上行
	send_buf[2] = fun; //功能字
	send_buf[3] = len; //数据长度
	for (i = 0; i < len; i++)
		send_buf[4 + i] = data[i]; //复制数据
	for (i = 0; i < len + 4; i++)
		send_buf[24] += send_buf[i]; //计算校验和
	for (i = 0; i < 25; i++)  Uart3SendChar(send_buf[i]);
	return 0;
}

u8 uploadMoto12Info(void)
{
	s16 paramWrite[10] = {0};
	paramWrite[0] = (s32)(moto_chassis[0].total_angle);
	paramWrite[1] = (s32)(moto_chassis[0].total_angle) >> 16;
	paramWrite[2] = (s16)(moto_chassis[0].speed_rpm);
	paramWrite[3] = (s16)(moto_chassis[0].given_current);
	paramWrite[4] = (s32)(moto_chassis[1].total_angle) >> 16;
	paramWrite[5] = (s32)(moto_chassis[1].total_angle);
	paramWrite[6] = (s16)(moto_chassis[1].speed_rpm);
	paramWrite[7] = (s16)(moto_chassis[1].given_current);

	ComSendData(0x01, (u8 *)&paramWrite[0], 16);

	return 0;
}

u8 uploadControl12Info(void)
{
	s16 paramWrite[10] = {0};
	paramWrite[0] = (s16)control_mode;
	paramWrite[1] = (s32)(set_position[0]);
	paramWrite[2] = (s32)(set_position[0]) >> 16;
	paramWrite[3] = (s16)(set_speed[0]);
	paramWrite[4] = (s16)(set_current[0]);

	paramWrite[5] = (s16)control_mode;
	paramWrite[6] = (s32)(set_position[1]);
	paramWrite[7] = (s32)(set_position[1]) >> 16;
	paramWrite[8] = (s16)(set_speed[1]);
	paramWrite[9] = (s16)(set_current[1]);

	ComSendData(0x03, (u8 *)&paramWrite[0], 20);

	return 0;
}

u8 uploadPID1Info(void)
{
	s16 paramWrite[10] = {0};
	paramWrite[0] = (s16)(motorPara[0].kp *1000);
	paramWrite[1] = (s16)(motorPara[0].ki *1000);
	paramWrite[2] = (s16)(motorPara[0].kd *1000);
	paramWrite[3] = (s16)(motorPara[0].kpv *1000);
	paramWrite[4] = (s16)(motorPara[0].kiv *1000);
	paramWrite[5] = (s16)(motorPara[0].kdv *1000);

	ComSendData(0x05, (u8 *)&paramWrite[0], 12);

	return 0;
}

u8 uploadPID2Info(void)
{
	s16 paramWrite[10] = {0};
	paramWrite[0] = (s16)(motorPara[1].kp *1000);
	paramWrite[1] = (s16)(motorPara[1].ki *1000);
	paramWrite[2] = (s16)(motorPara[1].kd *1000);
	paramWrite[3] = (s16)(motorPara[1].kpv *1000);
	paramWrite[4] = (s16)(motorPara[1].kiv *1000);
	paramWrite[5] = (s16)(motorPara[1].kdv *1000);

	ComSendData(0x06, (u8 *)&paramWrite[0], 12);

	return 0;
}
// 解包,从一帧数据的功能字开始解析。 function + len + Data + sum
//								  1 		 1	  len    1
void analyseData(u8 *data)
{
	if (data[2] == 0x01) 	// PID帧
	{
		motorPara[0].kp = (float)((u16)data[5] << 8 | (u16)data[4]) / 1000.0f;
		motorPara[0].ki = (float)((u16)data[7] << 8 | (u16)data[6]) / 1000000.0f;
		motorPara[0].kd = (float)((u16)data[9] << 8 | (u16)data[8]) / 1000.0f;
		motorPara[0].kpv = (float)((u16)data[11] << 8 | (u16)data[10]) / 1000.0f;
		motorPara[0].kiv = (float)((u16)data[13] << 8 | (u16)data[12]) / 1000.0f;
		motorPara[0].kdv = (float)((u16)data[15] << 8 | (u16)data[14]) / 1000.0f;
		pid1_change_flag = 1;			
	}


	if (data[2] == 0x02)  
	{
		motorPara[1].kp = (float)((u16)data[5] << 8 | (u16)data[4]) / 1000.0f;
		motorPara[1].ki = (float)((u16)data[7] << 8 | (u16)data[6]) / 1000000.0f;
		motorPara[1].kd = (float)((u16)data[9] << 8 | (u16)data[8]) / 1000.0f;
		motorPara[1].kpv = (float)((u16)data[11] << 8 | (u16)data[10]) / 1000.0f;
		motorPara[1].kiv = (float)((u16)data[13] << 8 | (u16)data[12]) / 1000.0f;
		motorPara[1].kdv = (float)((u16)data[15] << 8 | (u16)data[14]) / 1000.0f;
		
	}

	if (data[2] == 0x03) // 控制帧
	{
		control_mode = (s16)((u16)data[5] << 8 | (u16)data[4]) ;
		set_position[0] = ((s32)((u32)data[9] << 24 | (u32)data[8] << 16 | (u32)data[7] << 8 | (u32)data[6]));
		set_speed[0] =  ((s16)((u16)data[11] << 8 | (u16)data[10]));
		set_current[0] = ((s16)((u16)data[13] << 8 | (u16)data[12]));
		
		set_position[1]=set_position[0]; 
		set_speed[1]=set_speed[0]; 
		set_current[1]=set_current[0];
	}
}

ComSendData是打包一帧数据后串口发送,传入参数是功能字ID、数据、数据长度,然后加入帧头和校验,25字节一帧数据。

upload打头的函数都是上传不同的内容的函数,每一个都有不同的functionID,打包数据就是按照前面写的通信协议来的,就不细讲了,PID参数上传时乘以1000,这样分辨率就是0.001,也就是精度到0.001。因为传输数据是按整形传的,不乘1000上传小数部分都会变成0。

analyseData函数用来解析上位机发送的数据。通过第三个自己判断functionID,从而更改不同的参数。还是按照前面的通信协议来的。

五. 结语

这个工程中STM32控制电机位置和速度的例程其实很好实现,为了科普(装逼),自己硬是弄了个上位机,当然也是检测一下自己近期QT学习的效果啦。只能说实现了功能,软件还谈不上完善。截止到写这篇文章时其实还有一个bug没解决,就是位置模式下写进位置数据单片机响应会出现偶尔短暂卡顿的情况,表现出来的状况是LED灯会长亮(灭)一会,上位机曲线更新就会停一会。之前写的用遥控器的摇杆控制时没有这个问题,后面解决了我会更新出来,有人知道原因或者解决了留言告诉我哈。

一开始其实写过一个使用乐迪遥控器控制单片机的例程,那是第一次实现位置控制。将接收机与单片机连接起来,使用SBUS通信,这样通过遥控器的切换按钮来切换位置模式、速度模式和电流模式。通过摇杆或者旋钮通道就能设置电机的位置、速度、电流值。感兴趣的可以参考一下哈,尤其是想要做遥控机器人的小伙伴们。

下载链接:STM32F7_HAL_CAN_M3508_位置与速度PID_SBUS遥控器控制.zip

版权声明:本文为CSDN博主「何为其然」的原创文章,遵循CC 4.0 BY-SA版权协议,转载请附上原文出处链接及本声明。
原文链接:https://blog.csdn.net/qq_30267617/article/details/120764889

生成海报
点赞 0

何为其然

我还没有学会写个人说明!

暂无评论

发表评论

相关推荐

PID和三环控制-以大疆M3508、M2006为例

三环控制和PID在电机的应用 前言: 最近用到了大疆的直流无刷(BLDC)减速电机M3508和M2006。做RoboMaster比赛的同学应该对它们很熟悉,这两款电机质量都不错&#xff0

基于STM32的指纹密码锁

设计简介: 本设计是基于单片机的指纹密码锁,主要实现以下功能: 矩阵按键输入密码,并通过按键显示*号可通过按键或手机开门密码可通过按键进行开门可通过蓝牙模块连接手机进行开门可通过指纹进