devbox
运维做开发
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

小米电机CyberGear--STM32控制：绝对零点设置及位置模式正弦运动_小米电机运控模式

作者：运维做开发 | 2024-07-28 14:46:34

赞

踩

小米电机运控模式

本文参考了小米电机CyberGear STM32HAL 使用指南_小米电机瞬时电流串口指令-CSDN博客（没有关注提供代码中VOFA+通信内容），借助HAL库仅控制单个小米电机。可以实现小米电机绝对零点的设置，以及在位置模式下的正弦运动（运控模式经测试页是可行的），并向电脑反馈电机信息。

CyberGear官方提供的文档：小米企业网盘 -- 分享文件 (mioffice.cn)，提取码QDD1，包含说明书，固件包，电机三维图，上位机软件

注意：笔者在使用之前对CAN无了解，仅对串口传输有较充分的认识，调试CyberGear前搞清楚扩展帧和数据区后，根据官方说明书4.2章节以及参考博客，可以顺利对电机开发。

1.硬件

硬石的板子Stm32F407（含CAN接口），电源使用学生电源24V，电机接口使用网购的XT30PB（2+2）带线（不到20元），使用了板上的1个串口（通过仿真器与电脑通信）和CAN接口。

具体连接：通过仿真器TX\RX（板上USB接电脑同样可行，设置好对应串口即可）和板上一串口的RX\TX相连，电机引出线连到板上CAN口。

2.机械限位

SW2022绘制（低版本无法打开），3D打印制作，运动范围为60度。

结构中白色部分是一个120度的运动范围，对应可以做±60的运动。

注意：左图红色部分，电机自带的背面三颗螺钉无法拆卸

具体结构分享至网盘链接：https://pan.baidu.com/s/1mzTPC6rWjv7DBBEoDq2fhQ 
提取码：1234 

3.cubeMX设置

芯片选择其他初始设置在此不赘述，仅介绍关键内容

1）时钟

STM32F407芯片，时钟如下

2）CAN

电机默认波特率1Mbps，需要根据芯片时钟对CAN参数设置，笔者直接借助GPT，要求其根据上述时钟，分析产生1Mbps的波特率，设置如下

使能接收中断，具体是RX0还是RX1，要根据自己的板子确定，可参考他人博客。

3）板子与电脑串口通信（仅电脑接收板子传输的数据）

我使用仿真器的RX接口和32板的USART1与电脑连接；直接对应串口USB与电脑接也是可以

UART4异步通信，波特率115200,接收DMA；

4)TIM2

每隔10ms发送电机信息到电脑，不一定要定时器，也可以每次收到反馈之后，立刻把此次接收的数据传输给电脑

5）NVIC

4.控制程序

1）cybergear与电机交互部分，通信协议

xiaomi_driver.h，存储其定义的一些参数，cybergear.h直接引用即可

  1. #ifndef _XIAOMI_DRIVER_H_
  2. #define _XIAOMI_DRIVER_H_
  3. #include "main.h"
  4.  
  5. typedef enum {
  6.     RunMode_idx = 0x7005,
  7.     IqRef_idx = 0x7006,
  8.     SpdRef_idx = 0x700A,
  9.     LimitTorque_idx = 0x700B,
  10.     CurKp_idx = 0x7010,
  11.     CurKi_idx = 0x7011,
  12.     CurFiltGain_idx = 0x7014,
  13.     LocRef_idx = 0x7016,
  14.     LimitSpd_idx = 0x7017,
  15.     LimitCur_idx = 0x7018,
  16.     MechPos_idx = 0x7019,
  17.     IqFilt_idx = 0x701A,
  18.     MechVel_idx = 0x701B,
  19.     Vbus_idx = 0x701C,
  20.     Rotation_idx = 0x701D,
  21.     LocKp_idx = 0x701E,
  22.     SpdKp_idx = 0x7020,
  23.     SpdKi_idx = 0x7020
  24. } ControlTable;
  25.  
  26. typedef enum {
  27.     RunMode_Typ = 's', 
  28.     IqRef_Typ = 'f',
  29.     SpdRef_Typ = 'f',
  30.     LimitTorque_Typ = 'f',
  31.     CurKp_Typ = 'f',
  32.     CurKi_Typ = 'f',
  33.     CurFiltGain_Typ = 'f',
  34.     LocRef_Typ = 'f',
  35.     LimitSpd_Typ = 'f',
  36.     LimitCur_Typ = 'f',
  37.     MechPos_Typ = 'f',
  38.     IqFilt_Typ = 'f',
  39.     MechVel_Typ = 'f',
  40.     Vbus_Typ = 'f',
  41.     Rotation_Typ = 's',
  42.     LocKp_Typ = 'f',
  43.     SpdKp_Typ = 'f',
  44.     SpdKi_Typ = 'f'
  45. } ControlTableType;
  46.  
  47. //控制参数最值，谨慎更改
  48. #define P_MIN -12.5f
  49. #define P_MAX 12.5f
  50. #define V_MIN -30.0f
  51. #define V_MAX 30.0f
  52. #define KP_MIN 0.0f
  53. #define KP_MAX 500.0f
  54. #define KD_MIN 0.0f
  55. #define KD_MAX 5.0f
  56. #define T_MIN -12.0f
  57. #define T_MAX 12.0f
  58. #define MAX_P 720
  59. #define MIN_P -720
  60.  
  61. //控制命令宏定义
  62. #define Communication_Type_GetID 0x00           //获取设备的ID和64位MCU唯一标识符
  63. #define Communication_Type_MotionControl 0x01 	//用来向主机发送控制指令
  64. #define Communication_Type_MotorRequest 0x02	//用来向主机反馈电机运行状态
  65. #define Communication_Type_MotorEnable 0x03	    //电机使能运行
  66. #define Communication_Type_MotorStop 0x04	    //电机停止运行
  67. #define Communication_Type_SetPosZero 0x06	    //设置电机机械零位
  68. #define Communication_Type_CanID 0x07	        //更改当前电机CAN_ID
  69. #define Communication_Type_Control_Mode 0x12
  70. #define Communication_Type_GetSingleParameter 0x11	//读取单个参数
  71. #define Communication_Type_SetSingleParameter 0x12	//设定单个参数
  72. #define Communication_Type_ErrorFeedback 0x15
  73.  
  74. enum CONTROL_MODE   //控制模式定义
  75. {
  76.     Motion_mode = 0,//运控模式
  77.     Position_mode,  //位置模式
  78.     Velcity_mode,     //位置模式
  79.     Current_mode    //电流模式
  80. };
  81. enum ERROR_TAG      //错误回传对照
  82. {
  83.     OK                 = 0,//无故障
  84.     BAT_LOW_ERR        = 1,//欠压故障
  85.     OVER_CURRENT_ERR   = 2,//过流
  86.     OVER_TEMP_ERR      = 3,//过温
  87.     MAGNETIC_ERR       = 4,//磁编码故障
  88.     HALL_ERR_ERR       = 5,//HALL编码故障
  89.     NO_CALIBRATION_ERR = 6//未标定
  90. };
  91.  
  92. #endif

cybergear.h，协议部分（含设置电机等），暂未提供电脑接收数据相关设置（后续补充）。

注：修正了参考博客中设置电机零位中的错误，参考电机说明书内容

  1. #ifndef __CYBERGEAR_H
  2. #define __CYBERGEAR_H
  3.  
  4. #include "main.h"
  5. #include "can.h"
  6. #include "xiaomi_driver.h"
  7. #include "pc_communication.h"
  8.  
  9. #ifdef __cplusplus
  10. extern "C" {
  11. #endif
  12.     
  13. extern volatile float motor_info[4];
  14.  
  15. //主机CANID设置
  16. #define Master_CAN_ID 0x00                      //主机ID
  17.  
  18. //参数读取宏定义
  19.  
  20. #define Gain_Angle 720/32767.0
  21. #define Bias_Angle 0x8000
  22. #define Gain_Speed 30/32767.0
  23. #define Bias_Speed 0x8000
  24. #define Gain_Torque 12/32767.0
  25. #define Bias_Torque 0x8000
  26. #define Temp_Gain   0.1
  27.  
  28. #define Motor_Error 0x00
  29. #define Motor_OK 0X01
  30.  
  31. typedef enum 
  32. {
  33. 	Unit_Default,
  34. 	Unit_Deg=1,
  35. 	Unit_RPM=1,
  36. 	Unit_mA=1
  37. }Unit;
  38.  
  39. typedef struct{
  40.     
  41.     volatile float freq;
  42.     volatile float amp;//角度值
  43.     volatile float delay;
  44.     volatile float offset;
  45.     volatile float downRatio;
  46.     
  47. } SinMotion;
  48.  
  49. typedef struct{           //小米电机结构体
  50. 	uint8_t CAN_ID;       //CAN ID
  51.     uint8_t MCU_ID;       //MCU唯一标识符[后8位，共64位]
  52.  
  53. 	float des_cur;
  54. 	float des_vel;
  55. 	float des_pos;
  56.     
  57.     float pre_cur;
  58.     float pre_vel;
  59.     float pre_pos;
  60.     float pre_tor;
  61.     float pre_temperature;
  62.     uint8_t error_code;
  63.     
  64.     //零位选择通过Init_CyberZero()设置
  65.     float ini_tor;
  66.     float ini_vel;
  67.     float ini_pos;
  68.     float ini_kp, sin_kp;
  69.     float ini_kd, sin_kd;
  70.     
  71.     //最大限制
  72.     float max_tor;
  73.     float max_pos;
  74.     float max_vel;
  75.     float tor_threshold;
  76.     
  77. 	float zero_pos;
  78.     
  79.     SinMotion MotionPara;
  80.     
  81.  
  82. }Cyber_Motor;
  83.  
  84.  
  85.     
  86. extern Cyber_Motor Cyber;
  87. /*****************************初始化*****************************/
  88. void Init_Cyber(Cyber_Motor *Motor, uint8_t Can_Id);
  89. void Start_Cyber(Cyber_Motor *Motor);
  90. void Stop_Cyber(Cyber_Motor *Motor, uint8_t clear_error);
  91.  
  92. /*****************************设置电机参数*****************************/
  93. void Set_Cyber_Mode(Cyber_Motor *Motor, uint8_t Mode);
  94. void Set_Cyber_ZeroPos(Cyber_Motor *Motor);
  95. void Set_Cyber_Pos(Cyber_Motor *Motor, float value);//仅位置模式, value单位为Deg
  96. void Set_Cyber_limitSp(Cyber_Motor *Motor, float value);//同时设置电机结构体中的max_spd, value单位为RPM
  97. void Set_Cyber_limitTor(Cyber_Motor *Motor, float value);//同时设置电机结构体中的max_tor, value单位为Nm
  98. void Set_Cyber_RotNum(Cyber_Motor *Motor, float value);//设置电机当前圈数
  99. void Read_Cyber_Pos(Cyber_Motor *Motor);//获得反馈，发送位置可以时刻获得位置，这个呢
  100. /*****************************运动控制模式*****************************/
  101. void Cyber_ControlMode(Cyber_Motor *Motor,float tor, float vel_rads, float pos_rad, float kp, float kd);//仅运控模式
  102.  
  103. /*****************************反馈帧处理回调函数 负责接回传信息 *****************************/
  104. void request_motor_feedback(Cyber_Motor *Motor);
  105. void Motor_Data_Handler(Cyber_Motor *Motor,uint8_t DataFrame[8],uint32_t IDFrame);
  106.  
  107.  
  108. /*****************************暂时没用，电机参数读取设置*****************************/
  109. void Check_Cyber(uint8_t ID);
  110.  
  111. void set_CANID_cybergear(Cyber_Motor *Motor, uint8_t CAN_ID);
  112. void Set_Cyber_Cur(Cyber_Motor *Motor, float Current);
  113. uint32_t Get_Motor_ID(uint32_t CAN_ID_Frame);
  114.  
  115. /*****************************电机协议*****************************/
  116. float uint16_to_float(uint16_t x,float x_min,float x_max,int bits);
  117. int float_to_uint(float x, float x_min, float x_max, int bits);
  118. void Set_Cyber_Parameter(Cyber_Motor *Motor,uint16_t Index,float Value,char Value_type);
  119. void Set_Cyber_Parameter(Cyber_Motor *Motor,uint16_t Index,float Value,char Value_type);
  120. void Read_Cyber_Parameter(Cyber_Motor *Motor,uint16_t Index);
  121.  
  122. #ifdef __cplusplus
  123.   }
  124. #endif
  125.   
  126. #endif

cybergear.c

  1. #include "main.h"
  2. #include "can.h"
  3. #include "cybergear.h"
  4. #include "math.h"
  5.  
  6. Cyber_Motor Cyber;//小米电机定义
  7. CAN_RxHeaderTypeDef rxMsg;//发送接收结构体
  8. CAN_TxHeaderTypeDef txMsg;//发送配置结构体
  9. uint8_t rx_data[8];       //接收数据
  10. uint32_t Motor_Can_ID;    //接收数据电机ID
  11. uint8_t byte[4];          //转换临时数据
  12. uint32_t send_mail_box = {0};//NONE
  13.  
  14. #define can_txd() HAL_CAN_AddTxMessage(&hcan1, &txMsg, tx_data, &send_mail_box)//CAN发送宏定义
  15.  
  16. const float pi = 3.14159265358979323846f;
  17.  
  18. /***************************************初始化**************************************/
  19. //@brief          小米电机初始化参数
  20. void Init_Cyber(Cyber_Motor *Motor,uint8_t Can_Id)
  21. {
  22.     txMsg.StdId = 0;            //配置CAN发送：标准帧清零 
  23.     txMsg.ExtId = 0;            //配置CAN发送：扩展帧清零     
  24.     txMsg.IDE = CAN_ID_EXT;     //配置CAN发送：扩展帧
  25.     txMsg.RTR = CAN_RTR_DATA;   //配置CAN发送：数据帧
  26.     txMsg.DLC = 0x08;           //配置CAN发送：数据长度
  27.     
  28.     Motor->CAN_ID=Can_Id;       //ID设置 
  29.  
  30. }
  31.  
  32. //@brief          使能电机
  33. void Start_Cyber(Cyber_Motor *Motor)
  34. {
  35.     uint8_t tx_data[8] = {0}; 
  36.     txMsg.ExtId = Communication_Type_MotorEnable<<24|Master_CAN_ID<<8|Motor->CAN_ID;
  37.     can_txd();
  38. }
  39.  
  40. //@brief          停止电机
  41. void Stop_Cyber(Cyber_Motor *Motor,uint8_t clear_error)
  42. {
  43. 	uint8_t tx_data[8]={0};
  44. 	tx_data[0]=clear_error;//清除错误位设置 clear_error=1时清除故障
  45. 	txMsg.ExtId = Communication_Type_MotorStop<<24|Master_CAN_ID<<8|Motor->CAN_ID;
  46.     can_txd();
  47. }
  48.  
  49.  
  50. /*****************************设置电机参数*****************************/
  51. //@brief          设置电机模式(必须停止时调整！)
  52. void Set_Cyber_Mode(Cyber_Motor *Motor,uint8_t Mode)
  53. {	
  54. 	Set_Cyber_Parameter(Motor,RunMode_idx,Mode,'s');
  55. }
  56.  
  57. //@brief          设置电机位置，单位弧度；补充value为角度时的内容
  58. void Set_Cyber_Pos(Cyber_Motor *Motor, float value) 
  59. {
  60.     float pos = value * pi / 180;
  61.     Set_Cyber_Parameter(Motor, LocRef_idx, pos, 'f');
  62.     Motor->des_pos = value;
  63. }
  64.  
  65. //@brief          设置电机当前位置为零点
  66. void Set_Cyber_ZeroPos(Cyber_Motor *Motor)
  67. {
  68.     uint8_t tx_data[8]={0};
  69.     tx_data[0]=1;
  70.     txMsg.ExtId = Communication_Type_SetPosZero<<24|Master_CAN_ID<<8|Motor->CAN_ID;
  71.     can_txd();
  72. }
  73.  
  74. //@brief          设置电机运动限速，单位弧度/s
  75. void Set_Cyber_limitSp(Cyber_Motor *Motor, float value) {
  76.     Set_Cyber_Parameter(Motor, LimitSpd_idx, value, 'f');
  77.     Motor->max_vel = value;
  78. }
  79.  
  80. //@brief          设置电机运动最大扭矩，单位Nm
  81. void Set_Cyber_limitTor(Cyber_Motor *Motor, float value)
  82. {
  83.     Set_Cyber_Parameter(Motor, LimitTorque_idx, value, 'f');
  84.     Motor->max_tor = value;
  85. }
  86.  
  87. //@brief          设置电机当前圈数
  88. void Set_Cyber_RotNum(Cyber_Motor *Motor, float value)
  89. {
  90.     Set_Cyber_Parameter(Motor, Rotation_idx, value, 'f');
  91.     //Motor->max_tor = value;
  92. }
  93.  
  94. void Read_Cyber_Pos(Cyber_Motor *Motor)
  95. {
  96.     Read_Cyber_Parameter(Motor, LocRef_idx);
  97. }
  98.  
  99. /***************************************反馈帧处理回调函数 负责接回传信息 ***************************************/
  100. //反馈电机信息
  101. void request_motor_feedback(Cyber_Motor *Motor)
  102. {
  103.     uint8_t tx_data[8]={0};
  104. 	txMsg.ExtId = Communication_Type_MotorRequest<<24|Master_CAN_ID<<8|Motor->CAN_ID;
  105.     can_txd();
  106. }
  107. /**
  108.   * @brief          电机回复帧数据处理函数
  109.   * @param[in]      Motor:对应控制电机结构体   
  110.   * @param[in]      DataFrame:数据帧
  111.   * @param[in]      IDFrame:扩展ID帧
  112.   * @retval         None
  113.   */
  114. void Motor_Data_Handler(Cyber_Motor *Motor,uint8_t DataFrame[8],uint32_t IDFrame)
  115. {	
  116.     Motor->pre_pos=uint16_to_float(DataFrame[0]<<8|DataFrame[1],MIN_P,MAX_P,16);//DataFrame[0]<<8|DataFrame[1]低8位和高8位合并
  117.     Motor->pre_vel=uint16_to_float(DataFrame[2]<<8|DataFrame[3],V_MIN,V_MAX,16);
  118.     Motor->pre_tor=uint16_to_float(DataFrame[4]<<8|DataFrame[5],T_MIN,T_MAX,16);
  119.     Motor->pre_temperature=(DataFrame[6]<<8|DataFrame[7])*Temp_Gain;
  120.     Motor->error_code=(IDFrame&0x1F0000)>>16;
  121. }
  122.  
  123. /***************************************电机信息接受和发送**************************************/
  124.  
  125. /**
  126.   * @brief          hal库CAN回调函数,接收电机数据
  127.   * @param[in]      hcan:CAN句柄指针
  128.   * @retval         none
  129.   */
  130. void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan)
  131. {
  132.     HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &rxMsg, rx_data);//接收数据  
  133.     Motor_Data_Handler(&Cyber,rx_data,rxMsg.ExtId);
  134. }
  135.  
  136.  
  137.  
  138. //@brief          小米运控模式指令
  139. void Cyber_ControlMode(Cyber_Motor *Motor,float tor, float vel_rpm, float pos_deg, float kp, float kd)
  140. {
  141.     float pos_rad = pos_deg * pi / 180;
  142.     float vel_rads = vel_rpm * 30 / pi;
  143.     
  144.     uint8_t tx_data[8];//发送数据初始化
  145.     //装填发送数据
  146.     tx_data[0]=float_to_uint(pos_rad,P_MIN,P_MAX,16)>>8;  
  147.     tx_data[1]=float_to_uint(pos_rad,P_MIN,P_MAX,16);  
  148.     tx_data[2]=float_to_uint(vel_rads,V_MIN,V_MAX,16)>>8;  
  149.     tx_data[3]=float_to_uint(vel_rads,V_MIN,V_MAX,16);  
  150.     tx_data[4]=float_to_uint(kp,KP_MIN,KP_MAX,16)>>8;  
  151.     tx_data[5]=float_to_uint(kp,KP_MIN,KP_MAX,16);  
  152.     tx_data[6]=float_to_uint(kd,KD_MIN,KD_MAX,16)>>8;  
  153.     tx_data[7]=float_to_uint(kd,KD_MIN,KD_MAX,16); 
  154.     
  155.     txMsg.ExtId = Communication_Type_MotionControl<<24|float_to_uint(tor,T_MIN,T_MAX,16)<<8|Motor->CAN_ID;//装填扩展帧数据
  156.     can_txd();
  157. }
  158.  
  159.  
  160.  
  161. /***************************************暂时不用***************************************/
  162. //@brief          设置电机CANID
  163. void set_CANID_cybergear(Cyber_Motor *Motor,uint8_t CAN_ID)
  164. {
  165. 	uint8_t tx_data[8]={0};
  166. 	txMsg.ExtId = Communication_Type_CanID<<24|CAN_ID<<16|Master_CAN_ID<<8|Motor->CAN_ID;
  167.     Motor->CAN_ID = CAN_ID;//将新的ID导入电机结构体
  168.     can_txd();	
  169. }
  170. //@brief          电流控制模式下设置电流
  171. void Set_Cyber_Cur(Cyber_Motor *Motor,float Current)
  172. {
  173. 	Set_Cyber_Parameter(Motor,IqRef_idx,Current,'f');
  174. }
  175.  
  176. /**
  177.   * @brief          提取电机回复帧扩展ID中的电机CANID
  178.   * @param[in]      CAN_ID_Frame:电机回复帧中的扩展CANID   
  179.   * @retval         电机CANID
  180.   */
  181. uint32_t Get_Motor_ID(uint32_t CAN_ID_Frame)
  182. {
  183. 	return (CAN_ID_Frame&0xFFFF)>>8;
  184. }
  185.  
  186.  
  187. //@brief          小米电机ID检查
  188. void Check_cyber(uint8_t ID)
  189. {
  190.     uint8_t tx_data[8] = {0};
  191.     txMsg.ExtId = Communication_Type_GetID<<24|Master_CAN_ID<<8|ID;
  192.     can_txd();
  193. }
  194.  
  195.  
  196. /***************************************电机内部协议***************************************/
  197. /**
  198.   * @brief          浮点数转4字节函数
  199.   * @param[in]      f:浮点数
  200.   * @retval         4字节数组
  201.   * @description  : IEEE 754 协议
  202.   */
  203. uint8_t* Float_to_Byte(float f)
  204. {
  205. 	unsigned long longdata = 0;
  206. 	longdata = *(unsigned long*)&f;       
  207. 	byte[0] = (longdata & 0xFF000000) >> 24;
  208. 	byte[1] = (longdata & 0x00FF0000) >> 16;
  209. 	byte[2] = (longdata & 0x0000FF00) >> 8;
  210. 	byte[3] = (longdata & 0x000000FF);
  211. 	return byte;
  212. }
  213.  
  214. /**
  215.   * @brief          小米电机回文16位数据转浮点
  216.   * @param[in]      x:16位回文
  217.   * @param[in]      x_min:对应参数下限
  218.   * @param[in]      x_max:对应参数上限
  219.   * @param[in]      bits:参数位数
  220.   * @retval         返回浮点值
  221.   */
  222. float uint16_to_float(uint16_t x,float x_min,float x_max,int bits)
  223. {
  224.     uint32_t span = (1 << bits) - 1;
  225.     float offset = x_max - x_min;
  226.     return offset * x / span + x_min;
  227. }
  228.  
  229. /**
  230.   * @brief          小米电机发送浮点转16位数据
  231.   * @param[in]      x:浮点
  232.   * @param[in]      x_min:对应参数下限
  233.   * @param[in]      x_max:对应参数上限
  234.   * @param[in]      bits:参数位数
  235.   * @retval         返回浮点值
  236.   */
  237. int float_to_uint(float x, float x_min, float x_max, int bits)
  238. {
  239.   float span = x_max - x_min;
  240.   float offset = x_min;
  241.   if(x > x_max) x=x_max;
  242.   else if(x < x_min) x= x_min;
  243.   return (int) ((x-offset)*((float)((1<<bits)-1))/span);
  244. }
  245.  
  246. /**
  247.   * @brief          写入电机参数
  248.   * @param[in]      Motor:对应控制电机结构体
  249.   * @param[in]      Index:写入参数对应地址
  250.   * @param[in]      Value:写入参数值
  251.   * @param[in]      Value_type:写入参数数据类型
  252.   * @retval         none
  253.   */
  254. void Set_Cyber_Parameter(Cyber_Motor *Motor,uint16_t Index,float Value,char Value_type)
  255. {
  256. 	uint8_t tx_data[8]= {0};
  257. 	txMsg.ExtId = Communication_Type_SetSingleParameter<<24|Master_CAN_ID<<8|Motor->CAN_ID;
  258.     memcpy(&tx_data[0],&Index,2);
  259. 	if(Value_type == 'f')
  260.     {
  261.         memcpy(&tx_data[4], &Value, sizeof(Value));
  262. 	}
  263. 	else if(Value_type == 's')
  264.         {
  265. 		tx_data[4]=(uint8_t)Value;
  266. 	}
  267. 	can_txd();	
  268.     HAL_Delay(1);
  269. }
  270.  
  271. /**
  272.   * @brief          读取电机参数
  273.   * @param[in]      Motor:对应控制电机结构体
  274.   * @param[in]      Index:写入参数对应地址
  275.   * @param[in]      Value:写入参数值
  276.   * @param[in]      Value_type:写入参数数据类型
  277.   * @retval         none
  278.   */
  279. void Read_Cyber_Parameter(Cyber_Motor *Motor,uint16_t Index)
  280. {
  281. 	uint8_t tx_data[8]= {0};
  282. 	txMsg.ExtId = Communication_Type_GetSingleParameter<<24|Master_CAN_ID<<8|Motor->CAN_ID;
  283. 	tx_data[0]=Index;
  284. 	tx_data[1]=Index>>8;
  285. 	can_txd();	
  286. }

2）具体控制完成零点设置以及位置模式下的正弦运动

注：修改模式为运控模式，并将set_cyber_pos修改为cybergear.h/c中的cyber_controlMode同样可以实现正弦运动函数；

cybermotor.h

  1. #ifndef __CYBERMOTOR_H
  2. #define __CYBERMOTOR_H
  3.  
  4. #include "cybergear.h"
  5. #include <math.h>
  6. //该文件应该包括 绝对零点设置；正弦运动设置；
  7. #ifdef __cplusplus
  8. extern "C" {
  9. #endif
  10.     
  11. /*****************************获取期望零位*****************************/
  12. void Init_CyberZero(Cyber_Motor *Motor);
  13. void Init_Sin(Cyber_Motor *Motor);
  14.  
  15. void Setting_AbsoluteZero(Cyber_Motor *Motor);
  16. void Motion_CyberSin(Cyber_Motor *Motor);
  17.     
  18. #ifdef __cplusplus
  19. }
  20. #endif
  21. #endif

cybermotor.cpp

  1. #include "cybermotor.h"
  2.  
  3. const float pi = 3.14159265358979323846f;
  4.  
  5.  
  6. //@brief   绝对零点设置，包括电机初始化，运控模式达到极限值；
  7. void Setting_AbsoluteZero(Cyber_Motor *Motor)
  8. {
  9.     Init_Cyber(Motor, 0x7F);
  10.     //设置当前位置为0位
  11.     Init_CyberZero(Motor);//初始化电机结构体，即运控模式的参数Cyber_ControlMode
  12.     Set_Cyber_Mode(Motor,Position_mode);
  13.     //Set_Cyber_RotNum(Motor, 0);
  14.     Start_Cyber(Motor);//使能电机
  15.     Set_Cyber_RotNum(Motor, 0);
  16.     Set_Cyber_ZeroPos(Motor);
  17.     Set_Cyber_limitSp(Motor,1);
  18.     Set_Cyber_limitTor(Motor,0.2);
  19.     HAL_Delay(100);
  20.     Set_Cyber_Pos(Motor, 10);
  21.     HAL_Delay(100);
  22.     Set_Cyber_Pos(Motor, 20);
  23.     HAL_Delay(100);
  24.     Set_Cyber_Pos(Motor, 40);
  25.     HAL_Delay(100);
  26.     Set_Cyber_Pos(Motor, 130);
  27.     HAL_Delay(100);
  28.     
  29.     
  30.     
  31.     while (1)
  32.     {
  33.         //Set_Cyber_Pos(Motor, 30);
  34.         Set_Cyber_RotNum(Motor, 0);
  35.         
  36.         // 检查力矩和速度
  37.         if (Motor->pre_vel < 0.01)
  38.         {
  39.             Set_Cyber_ZeroPos(Motor);
  40.             Set_Cyber_limitSp(Motor,6);
  41.             Set_Cyber_Pos(Motor, -60);
  42.             HAL_Delay(3000);
  43.             //Set_Cyber_ZeroPos(Motor);
  44.             //break;
  45.  
  46.             Motion_CyberSin(Motor);
  47.  
  48.         }
  49.     }
  50. }
  51.  
  52.  
  53.  
  54. // 控制电机正弦运动的函数
  55. void Motion_CyberSin(Cyber_Motor *Motor) 
  56. {
  57.     Init_Sin(Motor);
  58.     uint32_t current_time;
  59.     float position;
  60.     //HAL_Delay(1000);
  61.     
  62.     float period = 1/Motor->MotionPara.freq;
  63.     //float periodPositive = period * Motor->downRatio;
  64. 	//float periodNegative = period - periodPositive;
  65.     
  66. //    Init_Cyber(Motor, 0x7F);
  67. //    Set_Cyber_ZeroPos(Motor);
  68.     Set_Cyber_limitSp(Motor, 5);
  69.  
  70.     // 循环持续指定的时间
  71.     while(1)
  72.     {
  73.         //float amp_rad = Motor->MotionPara.amp * pi/180;
  74.         current_time = HAL_GetTick(); // 更新当前时间
  75.         position = Motor->MotionPara.amp * sinf(2 * pi * Motor->MotionPara.freq * (current_time / 1000.0f))-60;
  76.         Motor->des_pos = position / pi * 180;
  77.  
  78.         Set_Cyber_Pos(Motor, position);
  79.  
  80.         // 添加延时来控制更新频率
  81.         HAL_Delay(3);
  82.     }
  83. }
  84.  
  85.  
  86. /*****************************零位初始化*****************************/
  87. void Init_CyberZero(Cyber_Motor *Motor)
  88. {
  89.     Motor->ini_tor=0.1;
  90.     Motor->ini_vel=0;
  91.     Motor->ini_pos=120;
  92.     Motor->ini_kp=0.8;
  93.     Motor->ini_kd=0.3;
  94.     Motor->tor_threshold=0.15;
  95. }
  96.  
  97. /*****************************实际驱动函数*****************************/
  98. void Init_Sin(Cyber_Motor *Motor)
  99. {
  100.     Motor->sin_kp = 3;
  101.     Motor->sin_kd = 0.5;
  102.     Motor->MotionPara.amp=20;//角度值
  103.     Motor->MotionPara.freq=3;
  104.     Motor->MotionPara.delay=0;
  105.     Motor->MotionPara.offset=0;
  106.     Motor->MotionPara.downRatio=0.5;//上下扑动的duration相同
  107. }

3)pc_communication.h/c，发送当前位置和电流或其他数据到PC

在定时器回调函数中调用curve_Display函数实现发送；使用VOFA+（一个上位机软件，类似串口助手）的FireWater显示数据，通过curve_Display实现向电脑端的发送，不使用curveDisplay，直接printf也是可以的

pc_communication.h

  1. #ifndef __PCCOM_H
  2. #define __PCCOM_H
  3.  
  4. #include "stm32f4xx_hal.h"  // 根据你的STM32系列调整，如stm32f1xx_hal.h
  5. #include "dma.h"            // 包含 DMA 初始化和配置的头文件
  6. #include <string.h>         // 用于 strlen 函数
  7. #include <stdarg.h>         // 用于可变参数函数
  8. #include <stdio.h>
  9. #include "usart.h"
  10.  
  11. #ifdef __cplusplus
  12. extern "C" {
  13. #endif
  14.     
  15. void PC_Communication(UART_HandleTypeDef* huart, const char* format, ...);
  16. void curve_Display(float a, float b, float c, float d);
  17.     
  18. #ifdef __cplusplus
  19. }
  20. #endif
  21.  
  22. #endif

pc_communication.c

  1. #include "pc_communication.h"
  2.  
  3. volatile float motor_info[5];//不能设置为uint_8，必须要和实际的Motor结构体的一致，否则无法发送
  4. #ifdef __cplusplus
  5. extern "C" {
  6. #endif
  7.     
  8. void PC_Communication(UART_HandleTypeDef* huart, const char* format, ...) {
  9.     static char buffer[1000];
  10.     va_list args;
  11.     va_start(args, format);
  12.     vsnprintf(buffer, sizeof(buffer), format, args);
  13.     va_end(args);
  14.     
  15.     HAL_UART_Transmit_DMA(huart, (uint8_t*)buffer, strlen(buffer));
  16. }
  17.  
  18. void curve_Display(float a, float b, float c, float d)
  19. {
  20.     PC_Communication(&huart1, "d: %.2f, %.2f, %.2f, %.2f\r\n", a, b, c, d);
  21. }
  22.  
  23. #ifdef __cplusplus
  24. |
  25. #endif
  26.

4）定时器回调函数，补充在tim.c最下面

  1. /**
  2.   * @brief          hal库TIM回调函数,发送数据到电脑
  3.   * @param[in]      hcan:TIM句柄指针
  4.   * @retval         none
  5.   */
  6. void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
  7. {
  8.     if (htim->Instance == TIM2) // 检查是哪个定时器产生了中断
  9.     {
  10.         motor_info[0] = Cyber.des_pos;
  11.         motor_info[1] = Cyber.pre_pos;
  12.         motor_info[2] = Cyber.pre_vel;
  13.         motor_info[3] = Cyber.pre_tor;
  14.         //motor_info[4] = Cyber.pre_temperature;
  15.         //printf("Desired Position: %f\n", motor_info[0]);
  16.        //printf("Present Position: %f\n", motor_info[1]);
  17.         //printf("Present Velocity: %f\n", motor_info[2]);
  18.         //printf("Present Current: %f\n", motor_info[3]);
  19.         curve_Display(motor_info[0],motor_info[1],motor_info[2],motor_info[3]);
  20.     }
  21. }

每隔10ms，向电机反馈电机的数据

注意：根据说明书，发送一次，只能在发送的时刻瞬间收到一次电机反馈。

例如，如果仅单次发送设置位置1rad，仅在发送的瞬间反馈一次位置命令。如果想要获取电机的运动曲线，必须在while中发送才能绘制相关运动曲线图。

5.绝对零点实现方式解释

1 具体实现在CyberMotor.h/cpp中Setting_AbsoluteZero：

设置运动模式->使能电机->设置当前位置为零点->设置位置模式限速/扭矩->设置位置->进入循环

循环中：设置电机圈数（目的，获取电机反馈（参考说明书，获取通信类型2的反馈），进而获取电机的位置速度扭矩信息）->判断当前速度大小是否小于一个很小的值->如果小于说明电机达到限位，即进入if条件句中->设置当前位置为零点->调整电机限速/扭矩->回到我需要正弦运动的位置（-60的位置）->delay等待达到实际位置（可以修改一下判断是否达到当前位置，通过反馈，而不是delay）->正弦运动函数

2. 遇到的一些问题

使能电机，并设置一个目标位置后，电机会先顺逆运动（方向每一次都不一样）1s多，才会按实际要求运动。因为此，笔者以为每次的位置模式启动后，正角度对应的不是固定顺时针/逆时针；实际是对应的。

可能原因：

1）初始Kp,Kd设置不当； 2）本身传感器可能存在误差； 3）电机本身控制算法中存在误差修正或者启动过程中的初始调整

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/运维做开发/article/detail/894612
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号