[问答]

请问STM32使用CAN总线怎么实现大疆M3508电机的速度闭环控制？

2178 电机

问答对人有帮助，内容完整，我也想知道答案 0 本文详细介绍电机的控制原理和使用CAN总线实现电机速度闭环的代码实操，最后实现电机的速度控制任务。 0
2021-9-24 06:10:59　　评论淘帖0 邀请回答您可以邀请以下用户，快速回答问题 × bertvwang 该类别下有 10 个回答。邀请回答纯纯纯牛奶该类别下有 10 个回答。邀请回答王伟01 该类别下有 9 个回答。邀请回答幽默该类别下有 9 个回答。邀请回答 Ehunt 该类别下有 8 个回答。邀请回答 tutu304725938 该类别下有 8 个回答。邀请回答 dahairenlyy 该类别下有 8 个回答。邀请回答凌晨3点睡该类别下有 8 个回答。邀请回答 fhbding 该类别下有 8 个回答。邀请回答 hzp_bbs1 该类别下有 8 个回答。邀请回答笔画张该类别下有 8 个回答。邀请回答偶是糕富帅该类别下有 8 个回答。邀请回答温暖镜头该类别下有 7 个回答。邀请回答 kmno4 该类别下有 7 个回答。邀请回答 djelje 该类别下有 7 个回答。邀请回答 pingnai 该类别下有 7 个回答。邀请回答雅博电子科技该类别下有 7 个回答。邀请回答 liutiefu 该类别下有 7 个回答。邀请回答低调de炫耀爱该类别下有 7 个回答。邀请回答 jenny042 该类别下有 7 个回答。邀请回答举报李永每相关推荐 • 请问一下大疆M3508电机ID怎么设置？ 1000 • 如何实现直流减速电机的速度闭环控制呢 2206 • 变频器闭环控制速度控制精度一般是多少？ 8324 • 能够利用加速度计测量飞行速度，从而实现速度闭环控制吗？ 3350 • 变频电机速度闭环反馈的模式 2069 • 怎样利用电机旋转角度和转速双闭环去搭建一个双闭环控制系统？ 1434 • 如何去实现BLDC电机速度的闭环控制呢 1403 • 基于AVR单片机的步进电机的闭环控制有谁做过吗 4448 • 直流电机闭环控制 7950 • 用labview实现伺服电机的闭环控制，用PID（泛布尔算法）控制，有大神会么？？？？ 5461 1个回答

答案对人有帮助，有参考价值 0 一. 绪论本文接上一篇STM32实现四驱小车（四）姿态控制任务——偏航角串级PID控制算法，在本文中介绍电机的控制原理和使用CAN总线实现电机速度闭环的代码实操，最后实现电机的速度控制任务。二. 电机速度环PID原理电机的种类特别多，小型机器人（无人机、无人车）上普遍使用直流无刷电机，航模使用的一般是高速低扭矩，车模一般通过减速器低速实现高扭矩。一般电机与电机驱动器配套使用，用的话只需要知道如何供电和如何给控制信号就行了。本文使用大疆的M3508电机，使用CAN总线进行控制，长相如图所示。 CAN总线是汽车行业的应用标准，稍大型的电机很多是CAN总线控制的。CAN总线是一种现场总线，具有速度快、传输距离远、连接节点多的优点，具有错误检测、错误通知和错误恢复功能，使得其特别适合工业过程监控设备的互联。CAN总线的知识点很复杂，现在升级版的CAN-Open更是乖乖不得了，我啃了好几天才大致弄懂一些。大家主要要会用就行了。回到我们的电机，众所周知伺服电机有位置伺服、速度伺服、力伺服，大家一定也听过电机的三闭环控制，也就是位置环——速度环——电流环，如图所示，根据控制目标的不同，从外环到内环分别对应电机的位置伺服、速度伺服与力伺服，也可以说三种模式：位置模式、速度模式、转矩模式。上一篇我们讲过姿态环的角度、角速度串级PID，所以看到这里的位置环——速度环——电流环大家一定不陌生了，而且似乎发现了什么有趣的东西，是不是外环的控制变量和内环的控制变量依次成导数关系？（电流决定的就是力，也就是角加速度）。电机一般要配合驱动控制器使用，有的驱动器很牛皮，里面做好了三种模式，控制器给位置信号、速度信号就行了。有的次一点，只做了速度伺服，有的更次，啥也没做。我们今天用的大疆M33508电机就啥也没做，，，驱动器只是上报电机的速度、电流、转角、温度等信息，需要控制器去读这些信息，然后做闭环计算，控制器将计算结果传输给驱动器，再去控制电机电流。这里我们主要是做速度伺服。上图变成这样。三. STM32使用CAN总线实现大疆M3508电机的速度闭环控制这里我们需要对电机做一个速度闭环控制，由于大疆电机提供了例程，所以我直接移植了它的代码，当然有些地方做了修改。创建一个motor_pid.h和motor_pid.c文件（其实完全可以用上一章写的pid.c和pid.h文件，这里因为是官方提供的例程，所以直接用了）。 motor_pid.h文件： #ifndef __MOTOR_PID_H #define __MOTOR_PID_H /* Includes ------------------------------------------------------------------/ #include "stm32f7xx_hal.h" enum { LLAST = 0, LAST = 1, NOW = 2, POSITION_PID, DELTA_PID, }; typedef struct __pid_t { float p; float i; float d; float set[3]; //目标值,包含NOW， LAST， LLAST上上次 float get[3]; //测量值 float err[3]; //误差 float pout; //p输出 float iout; //i输出 float dout; //d输出 float pos_out; //本次位置式输出 float last_pos_out; //上次输出 float delta_u; //本次增量值 float delta_out; //本次增量式输出 = last_delta_out + delta_u float last_delta_out; float max_err; float deadband; //err < deadband return uint32_t pid_mode; uint32_t MaxOutput; //输出限幅 uint32_t IntegralLimit; //积分限幅 void (f_param_init)(struct __pid_t pid, //PID参数初始化 uint32_t pid_mode, uint32_t maxOutput, uint32_t integralLimit, float p, float i, float d); void (f_pid_reset)(struct __pid_t pid, float p, float i, float d); //pid三个参数修改 } pid_t; void PID_struct_init( pid_t pid, uint32_t mode, uint32_t maxout, uint32_t intergral_limit, float kp, float ki, float kd); float pid_calc(pid_t pid, float fdb, float ref); #endif motor_pid.c文件： / Includes ------------------------------------------------------------------/ #include "motor_pid.h" #include "mytype.h" #include //#include "cmsis_os.h" #define ABS(x) ((x>0)? (x): (-x)) void abs_limit(float a, float ABS_MAX){ if(a > ABS_MAX) a = ABS_MAX; if(a < -ABS_MAX) a = -ABS_MAX; } /参数初始化--------------------------------------------------------------/ static void pid_param_init( pid_t pid, uint32_t mode, uint32_t maxout, uint32_t intergral_limit, float kp, float ki, float kd) { pid->IntegralLimit = intergral_limit; pid->MaxOutput = maxout; pid->pid_mode = mode; pid->p = kp; pid->i = ki; pid->d = kd; } /中途更改参数设定(调试)------------------------------------------------------------/ static void pid_reset(pid_t pid, float kp, float ki, float kd) { pid->p = kp; pid->i = ki; pid->d = kd; } /** @bref. calculate delta PID and position PID @param[in] set： target @param[in] real measure / float pid_calc(pid_t* pid, float get, float set){ pid->get[NOW] = get; pid->set[NOW] = set; pid->err[NOW] = set - get; //set - measure if (pid->max_err != 0 && ABS(pid->err[NOW]) > pid->max_err ) return 0; if (pid->deadband != 0 && ABS(pid->err[NOW]) < pid->deadband) return 0; if(pid->pid_mode == POSITION_PID) //位置式p { pid->pout = pid->p * pid->err[NOW]; pid->iout += pid->i * pid->err[NOW]; pid->dout = pid->d * (pid->err[NOW] - pid->err[LAST] ); abs_limit(&(pid->iout), pid->IntegralLimit); pid->pos_out = pid->pout + pid->iout + pid->dout; abs_limit(&(pid->pos_out), pid->MaxOutput); pid->last_pos_out = pid->pos_out; //update last time } else if(pid->pid_mode == DELTA_PID)//增量式P { pid->pout = pid->p * (pid->err[NOW] - pid->err[LAST]); pid->iout = pid->i * pid->err[NOW]; pid->dout = pid->d * (pid->err[NOW] - 2pid->err[LAST] + pid->err[LLAST]); abs_limit(&(pid->iout), pid->IntegralLimit); pid->delta_u = pid->pout + pid->iout + pid->dout; pid->delta_out = pid->last_delta_out + pid->delta_u; abs_limit(&(pid->delta_out), pid->MaxOutput); pid->last_delta_out = pid->delta_out; //update last time } pid->err[LLAST] = pid->err[LAST]; pid->err[LAST] = pid->err[NOW]; pid->get[LLAST] = pid->get[LAST]; pid->get[LAST] = pid->get[NOW]; pid->set[LLAST] = pid->set[LAST]; pid->set[LAST] = pid->set[NOW]; return pid->pid_mode==POSITION_PID ? pid->pos_out : pid->delta_out; // } /* @bref. special calculate position PID @attention @use @gyro data!! @param[in] set： target @param[in] real measure / float pid_sp_calc(pid_t* pid, float get, float set, float gyro){ pid->get[NOW] = get; pid->set[NOW] = set; pid->err[NOW] = set - get; //set - measure if(pid->pid_mode == POSITION_PID) //位置式p { pid->pout = pid->p * pid->err[NOW]; if(fabs(pid->i) >= 0.001f) pid->iout += pid->i * pid->err[NOW]; else pid->iout = 0; pid->dout = -pid->d * gyro/100.0f; abs_limit(&(pid->iout), pid->IntegralLimit); pid->pos_out = pid->pout + pid->iout + pid->dout; abs_limit(&(pid->pos_out), pid->MaxOutput); pid->last_pos_out = pid->pos_out; //update last time } else if(pid->pid_mode == DELTA_PID)//增量式P { pid->pout = pid->p * (pid->err[NOW] - pid->err[LAST]); pid->iout = pid->i * pid->err[NOW]; pid->dout = pid->d * (pid->err[NOW] - 2pid->err[LAST] + pid->err[LLAST]); abs_limit(&(pid->iout), pid->IntegralLimit); pid->delta_u = pid->pout + pid->iout + pid->dout; pid->delta_out = pid->last_delta_out + pid->delta_u; abs_limit(&(pid->delta_out), pid->MaxOutput); pid->last_delta_out = pid->delta_out; //update last time } pid->err[LLAST] = pid->err[LAST]; pid->err[LAST] = pid->err[NOW]; pid->get[LLAST] = pid->get[LAST]; pid->get[LAST] = pid->get[NOW]; pid->set[LLAST] = pid->set[LAST]; pid->set[LAST] = pid->set[NOW]; return pid->pid_mode==POSITION_PID ? pid->pos_out : pid->delta_out; // } /pid总体初始化-----------------------------------------------------------------/ void PID_struct_init( pid_t pid, uint32_t mode, uint32_t maxout, uint32_t intergral_limit, float kp, float ki, float kd) { /init function pointer/ pid->f_param_init = pid_param_init; pid->f_pid_reset = pid_reset; // pid->f_cal_pid = pid_calc; // pid->f_cal_sp_pid = pid_sp_calc; //addition /init pid param / pid->f_param_init(pid, mode, maxout, intergral_limit, kp, ki, kd); } 再创建一个motor.h和motor.c文件，用于写电机PID控制的应用代码。 motor.h内容为： #ifndef __MOTOR #define __MOTOR #include "stm32f7xx_hal.h" #include "mytype.h" #include "can.h" /电机的参数结构体/ 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; #define motor_num 6 //电机数量 /* Extern ------------------------------------------------------------------/ extern moto_measure_t moto_chassis[]; u8 get_moto_measure(moto_measure_t ptr, CAN_HandleTypeDef* hcan); u8 set_moto_current(CAN_HandleTypeDef* hcan, s16 SID, s16 iq1, s16 iq2, s16 iq3, s16 iq4); #endif motor.c文件： #include "can.h" #include "motor.h" //#include "cmsis_os.h" moto_measure_t moto_chassis[motor_num] = {0}; //4 chassis moto moto_measure_t moto_info; void get_total_angle(moto_measure_t p); void get_moto_offset(moto_measure_t ptr, CAN_HandleTypeDef* hcan); /******************************************************************************************* * @Func void get_moto_measure(moto_measure_t ptr, CAN_HandleTypeDef hcan) * @Brief 接收通过CAN发过来的信息 * @Param * @Retval None * @Date 2015/11/24 ******************************************************************************************/ u8 get_moto_measure(moto_measure_t ptr, CAN_HandleTypeDef* hcan) { if(HAL_CAN_Receive(hcan,CAN_FIFO0,0)!=HAL_OK) return 0;//接收数据 ptr->last_angle = ptr->angle; ptr->angle = (uint16_t)(hcan->pRxMsg->Data[0]<<8 \| hcan->pRxMsg->Data[1]) ; ptr->real_current = (int16_t)(hcan->pRxMsg->Data[2]<<8 \| hcan->pRxMsg->Data[3]); ptr->speed_rpm = ptr->real_current; //这里是因为两种电调对应位不一样的信息 ptr->given_current = (int16_t)(hcan->pRxMsg->Data[4]<<8 \| hcan->pRxMsg->Data[5])/-5; ptr->hall = hcan->pRxMsg->Data[6]; if(ptr->angle - ptr->last_angle > 4096) ptr->round_cnt --; else if (ptr->angle - ptr->last_angle < -4096) ptr->round_cnt ++; ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle; return hcan->pRxMsg->DLC; } /this function should be called after system+can init / void get_moto_offset(moto_measure_t ptr, CAN_HandleTypeDef hcan) { ptr->angle = (uint16_t)(hcan->pRxMsg->Data[0]<<8 \| hcan->pRxMsg->Data[1]) ; ptr->offset_angle = ptr->angle; } #define ABS(x) ( (x>0) ? (x) : (-x) ) /** @bref 电机上电角度=0，之后用这个函数更新3510电机的相对开机后（为0）的相对角度。 / void get_total_angle(moto_measure_t p){ int res1, res2, delta; if(p->angle < p->last_angle){ //可能的情况 res1 = p->angle + 8192 - p->last_angle; //正转，delta=+ res2 = p->angle - p->last_angle; //反转 delta=- }else{ //angle > last res1 = p->angle - 8192 - p->last_angle ;//反转 delta - res2 = p->angle - p->last_angle; //正转 delta + } //不管正反转，肯定是转的角度小的那个是真的 if(ABS(res1) delta = res1; else delta = res2; p->total_angle += delta; p->last_angle = p->angle; } u8 set_moto_current(CAN_HandleTypeDef hcan, s16 SID, s16 iq1, s16 iq2, s16 iq3, s16 iq4) { hcan->pTxMsg->StdId = SID; hcan->pTxMsg->IDE = CAN_ID_STD; hcan->pTxMsg->RTR = CAN_RTR_DATA; hcan->pTxMsg->DLC = 0x08; hcan->pTxMsg->Data[0] = iq1 >> 8; hcan->pTxMsg->Data[1] = iq1; hcan->pTxMsg->Data[2] = iq2 >> 8; hcan->pTxMsg->Data[3] = iq2; hcan->pTxMsg->Data[4] = iq3 >> 8; hcan->pTxMsg->Data[5] = iq3; hcan->pTxMsg->Data[6] = iq4 >> 8; hcan->pTxMsg->Data[7] = iq4; //HAL_CAN_Transmit(hcan, 1000); if(HAL_CAN_Transmit(&CAN1_Handler,1000)!=HAL_OK) return 1; //发送 return 0; } motor.c里面主要实现两个函数，一个是get_moto_measure(moto_measure_t ptr, CAN_HandleTypeDef hcan)，用于获得各个电机的当前信息，一个是set_moto_current(CAN_HandleTypeDef* hcan, s16 SID, s16 iq1, s16 iq2, s16 iq3, s16 iq4)，用来经过PID计算后，设置电机电流（速度PID输出的是电流）。四. UCOS-III电机控制任务的实现写完了驱动函数和初级应用函数，下面可以写核心代码了，在上一章STM32实现四驱小车（四）姿态控制任务——偏航角串级PID控制算法的基础上，补充完整motor_task. //motor任务 u8 motor_task(void *p_arg) { OS_ERR err; CPU_SR_ALLOC(); u8 i; u16 retry; u8 flag_motor[motor_num]; //电机信息接受成功标志 pid_t pid_speed[motor_num]; //电机速度PID环 float set_speed_temp; //加减速时的临时设定速度 int16_t delta; //设定速度与实际速度只差值 int16_t max_speed_change = 400; //电机单次最大变化速度，加减速用 //PID初始化 for (i = 0; i < 4; i++) { PID_struct_init(&pid_speed, POSITION_PID, 16384, 16384, 1.5f, 0.1f, 0.0f); //4 motos angular rate closeloop. } while (1) { //PID采样，获取电机数据 retry = 0; flag_motor[0] = flag_motor[1] = flag_motor[2] = flag_motor[3] = 0; while (retry < 20) { get_moto_measure(&moto_info, &CAN1_Handler); if (CAN1_Handler.pRxMsg->StdId == 0x201) { moto_chassis[0] = moto_info; flag_motor[0] = 1; } if (CAN1_Handler.pRxMsg->StdId == 0x202) { moto_chassis[1] = moto_info; flag_motor[1] = 1; } if (CAN1_Handler.pRxMsg->StdId == 0x203) { moto_chassis[2] = moto_info; flag_motor[2] = 1; } if (CAN1_Handler.pRxMsg->StdId == 0x204) { moto_chassis[3] = moto_info; flag_motor[3] = 1; } if (flag_motor[0] && flag_motor[2]&& flag_motor[1]&& flag_motor[3]) break; else retry++; } //PID计算输出 //OS_CRITICAL_ENTER(); for (i = 0; i < 5; i++) { //PID计算 //加减速 delta = (int16_t)set_speed - moto_chassis.speed_rpm; if (delta > max_speed_change) set_speed_temp = (float)(moto_chassis.speed_rpm + max_speed_change); else if (delta < -max_speed_change) set_speed_temp = (float)(moto_chassis.speed_rpm - max_speed_change); else set_speed_temp = set_speed; //pid_calc(&pid_speed, (float)moto_chassis.speed_rpm, set_speed); pid_calc(&pid_speed, (float)moto_chassis.speed_rpm, set_speed_temp); } // printf("PID out: %frn", pid_speed[1].pos_out); // printf("real speed: %d rn", moto_chassis[1].speed_rpm); // printf("set speed: %f rn", set_speed[1]); //PID 输出 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)); //OS_CRITICAL_EXIT(); delay_ms(5); } } 电机任务首先进行PID的初始化，PID_struct_init(&pid_speed, POSITION_PID, 16384, 16384, 1.5f, 0.1f, 0.0f)。初始化四个电机为位置式PID、PID输出限幅16384，PID参数为Kp=1.5， Ki=0.1, Kd=0。之后进入while（1）循环，查询电机当前速度（get_moto_measure），然后依次对四个电机进行PID计算(pid_calc)，然后一次性将四个电机的PID计算结果输送到CAN总线(set_moto_current)。这里我还做了加减速处理，避免电机速度调整太快超过PID的输出限幅。到此完整的链路就实现了。建议大家再把前面的四篇文章读一下，对于整个控制逻辑和数据传输链路就会更清楚了。整体来说四个任务是一环套一环的，但是又是独立的。这样的代码架构在程序设计与代码调试上会非常快，能够快速定位Bug，所以建议还在裸奔的小伙伴们去学一学操作系统，会大大提升程序设计水平。

2021-9-24 11:48:35 评论举报张倩