HFUT_Firmament_EC
/
sentry_gimbal_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

更新dji_motor初始化的配置,优化参数传递

This commit is contained in:
NeoZng 2022-11-12 12:39:36 +08:00
parent 55e12955b7
commit 0fb67070b8
12 changed files with 234 additions and 174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
HAL_N_Middlewares/Src/main.c
View File

@ -116,31 +116,31 @@ int main(void)
MX_USART6_UART_Init();
/* USER CODE BEGIN 2 */
RC_init(&huart3);
can_instance_config can_config = {.can_handle = &hcan1,
.tx_id = 1};
Motor_Control_Setting_s motor_config = {.angle_feedback_source = MOTOR_FEED,
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
.speed_feedback_source = MOTOR_FEED,
.reverse_flag = MOTOR_DIRECTION_NORMAL};
Motor_Controller_Init_s controller_init = {.current_PID = {
.Improve = 0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.DeadBand = 0,
.MaxOut = 4000,
},
.speed_PID = {
.Improve = 0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.DeadBand = 0,
.MaxOut = 4000,
}};
Motor_Type_e type = M3508;
DWT_Init(168);
dji_motor_instance *djimotor = DJIMotorInit(can_config, motor_config, controller_init, type);
Motor_Init_Config_s config = {
.motor_type = M3508,
.can_init_config = {
.can_handle = &hcan1,
.tx_id = 1},
.controller_setting_init_config = {.angle_feedback_source = MOTOR_FEED,
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
.speed_feedback_source = MOTOR_FEED,
.reverse_flag = MOTOR_DIRECTION_NORMAL},
.controller_param_init_config = {.current_PID = {
.Improve = 0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.DeadBand = 0,
.MaxOut = 4000},
.speed_PID = {
.Improve = 0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.DeadBand = 0,
.MaxOut = 4000}}};
dji_motor_instance *djimotor = DJIMotorInit(config);
/* USER CODE END 2 */
/* Call init function for freertos objects (in freertos.c) */

2
bsp/bsp_can.c
View File

@ -57,7 +57,7 @@ static void CANServiceInit()
/* -----------------------two extern callable function -----------------------*/
void CANRegister(can_instance *ins, can_instance_config config)
void CANRegister(can_instance *ins, can_instance_config_s config)
{
static uint8_t idx;
if (!idx)

30
bsp/bsp_can.h
View File

@ -9,50 +9,44 @@
#define MX_CAN_FILTER_CNT (2 * 14) // temporarily useless
#define DEVICE_CAN_CNT 2 // CAN1,CAN2
/* can instance typedef, every module registered to CAN should have this variable */
typedef struct _
{
CAN_HandleTypeDef* can_handle;
CAN_HandleTypeDef *can_handle;
CAN_TxHeaderTypeDef txconf;
uint32_t tx_id;
uint32_t tx_mailbox;
uint8_t tx_buff[8];
uint8_t tx_buff[8];
uint8_t rx_buff[8];
uint32_t rx_id;
void (*can_module_callback)(struct _*); // callback needs an instance to tell among registered ones
void (*can_module_callback)(struct _ *); // callback needs an instance to tell among registered ones
} can_instance;
/* this structure is used as initialization*/
typedef struct
typedef struct
{
CAN_HandleTypeDef* can_handle;
CAN_HandleTypeDef *can_handle;
uint32_t tx_id;
uint32_t rx_id;
void (*can_module_callback)(can_instance*);
} can_instance_config;
void (*can_module_callback)(can_instance *);
} can_instance_config_s;
/* module callback,which resolve protocol when new mesg arrives*/
typedef void (*can_callback)(can_instance*);
typedef void (*can_callback)(can_instance *);
/**
* @brief transmit mesg through CAN device
*
*
* @param _instance can instance owned by module
*/
void CANTransmit(can_instance* _instance);
void CANTransmit(can_instance *_instance);
/**
* @brief Register a module to CAN service,remember to call this before using a CAN device
*
*
* @param config init config
* @return can_instance* can instance owned by module
*/
void CANRegister(can_instance* instance, can_instance_config config);
void CANRegister(can_instance *instance, can_instance_config_s config);
#endif

9
modules/master_machine/湖南大学RoboMaster电控组通信协议.md
View File

@ -1,7 +1,16 @@
# 湖南大学RoboMaster电控组通信协议
<p align='right'>Seasky LIUWei</p>
<p align='right'>modified by neozng1@hnu.edu.cn</p>
可用于视觉，以及其他自研模块（仅限串口通信）
> TODO:
>
> 1. 利用F4自带的硬件CRC模块计算校验码，提高速度
> 2. 增加更多的数据类型支持，使得数据类型也为可配置的
## 一、串口配置
通信方式是串口，配置为波特率 115200，8 位数据位，1 位停止位，无硬件流控，无校验位。

40
modules/motor/HT04.c
View File

@ -1,63 +1,63 @@
#include "HT04.h"
#include "memory.h"
joint_instance* joint_motor_info[HT_MOTOR_CNT];
joint_instance *joint_motor_info[HT_MOTOR_CNT];
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
float span = x_max - x_min;
float offset = x_min;
return (uint16_t) ((x-offset)*((float)((1<<bits)-1))/span);
return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
}
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
float span = x_max - x_min;
float offset = x_min;
return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}
static void DecodeJoint(can_instance* motor_instance)
static void DecodeJoint(can_instance *motor_instance)
{
uint16_t tmp;
for (size_t i = 0; i < HT_MOTOR_CNT; i++)
{
if(joint_motor_info[i]->motor_can_instace==motor_instance)
if (joint_motor_info[i]->motor_can_instace == motor_instance)
{
tmp = (motor_instance->rx_buff[1] << 8) | motor_instance->rx_buff[2];
joint_motor_info[i]->last_ecd=joint_motor_info[i]->ecd;
joint_motor_info[i]->ecd=uint_to_float(tmp,P_MAX,P_MIN,16);
joint_motor_info[i]->last_ecd = joint_motor_info[i]->ecd;
joint_motor_info[i]->ecd = uint_to_float(tmp, P_MAX, P_MIN, 16);
tmp = (motor_instance->rx_buff[3] << 4) | (motor_instance->rx_buff[4] >> 4);
joint_motor_info[i]->speed_rpm= uint_to_float(tmp,V_MAX,V_MIN,12);
tmp=((motor_instance->rx_buff[4]&0xf)<<8) | motor_instance->rx_buff[5];
joint_motor_info[i]->given_current=uint_to_float(tmp,T_MAX,T_MIN,12);
joint_motor_info[i]->speed_rpm = uint_to_float(tmp, V_MAX, V_MIN, 12);
tmp = ((motor_instance->rx_buff[4] & 0xf) << 8) | motor_instance->rx_buff[5];
joint_motor_info[i]->given_current = uint_to_float(tmp, T_MAX, T_MIN, 12);
break;
}
}
}
joint_instance* HTMotorInit(can_instance_config config)
joint_instance *HTMotorInit(can_instance_config_s config)
{
static uint8_t idx;
joint_motor_info[idx]=(joint_instance*)malloc(sizeof(joint_instance));
CANRegister(joint_motor_info[idx++]->motor_can_instace,config);
joint_motor_info[idx] = (joint_instance *)malloc(sizeof(joint_instance));
CANRegister(joint_motor_info[idx++]->motor_can_instace, config);
return joint_motor_info[idx++];
}
void JointControl(joint_instance* _instance,float current)
void JointControl(joint_instance *_instance, float current)
{
uint16_t tmp;
LIMIT_MIN_MAX(current, T_MIN, T_MAX);
LIMIT_MIN_MAX(current, T_MIN, T_MAX);
tmp = float_to_uint(current, T_MIN, T_MAX, 12);
_instance->motor_can_instace->rx_buff[6] = tmp>>8;
_instance->motor_can_instace->rx_buff[7] = tmp&0xff;
_instance->motor_can_instace->rx_buff[6] = tmp >> 8;
_instance->motor_can_instace->rx_buff[7] = tmp & 0xff;
CANTransmit(_instance->motor_can_instace);
}
void SetJointMode(joint_mode cmd,joint_instance* _instance)
void SetJointMode(joint_mode cmd, joint_instance *_instance)
{
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
buf[7]=(uint8_t)cmd;
memcpy(_instance->motor_can_instace->rx_buff,buf,8*sizeof(uint8_t));
buf[7] = (uint8_t)cmd;
memcpy(_instance->motor_can_instace->rx_buff, buf, 8 * sizeof(uint8_t));
CANTransmit(_instance->motor_can_instace);
}

18
modules/motor/HT04.h
View File

@ -7,14 +7,14 @@
#define HT_MOTOR_CNT 4
#define P_MIN -95.5f // Radians
#define P_MIN -95.5f // Radians
#define P_MAX 95.5f
#define V_MIN -45.0f // Rad/s
#define V_MIN -45.0f // Rad/s
#define V_MAX 45.0f
#define T_MIN -18.0f
#define T_MAX 18.0f
typedef struct //HT04
typedef struct // HT04
{
float last_ecd;
float ecd;
@ -22,20 +22,20 @@ typedef struct //HT04
float given_current;
PID_t pid;
can_instance* motor_can_instace;
can_instance *motor_can_instace;
} joint_instance;
typedef enum
{
CMD_MOTOR_MODE = 0xfc,
CMD_RESET_MODE = 0xfd,
CMD_MOTOR_MODE = 0xfc,
CMD_RESET_MODE = 0xfd,
CMD_ZERO_POSITION = 0xfe
} joint_mode;
joint_instance* HTMotorInit(can_instance_config config);
joint_instance *HTMotorInit(can_instance_config_s config);
void JointControl(joint_instance* _instance,float current);
void JointControl(joint_instance *_instance, float current);
void SetJointMode(joint_mode cmd,joint_instance* _instance);
void SetJointMode(joint_mode cmd, joint_instance *_instance);
#endif // !HT04_H#define HT04_H

2
modules/motor/LK9025.c
View File

@ -18,7 +18,7 @@ static void DecodeDriven(can_instance *_instance)
}
}
driven_instance *LKMotroInit(can_instance_config config)
driven_instance *LKMotroInit(can_instance_config_s config)
{
static uint8_t idx;
driven_motor_info[idx] = (driven_instance *)malloc(sizeof(driven_instance));

15
modules/motor/LK9025.h
View File

@ -9,10 +9,9 @@
#define I_MIN -2000
#define I_MAX 2000
#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
#define LIMIT_MIN_MAX(x, min, max) (x) = (((x) <= (min)) ? (min) : (((x) >= (max)) ? (max) : (x)))
typedef struct //9025
typedef struct // 9025
{
uint16_t last_ecd;
uint16_t ecd;
@ -20,8 +19,8 @@ typedef struct //9025
int16_t given_current;
uint8_t temperate;
PID_t* pid;
can_instance* motor_can_instance;
PID_t *pid;
can_instance *motor_can_instance;
} driven_instance;
@ -30,10 +29,10 @@ typedef enum
unused = 0,
} driven_mode;
driven_instance* LKMotroInit(can_instance_config config);
driven_instance *LKMotroInit(can_instance_config_s config);
void DrivenControl(int16_t motor1_current,int16_t motor2_current);
void DrivenControl(int16_t motor1_current, int16_t motor2_current);
void SetDrivenMode(driven_mode cmd,uint16_t motor_id);
void SetDrivenMode(driven_mode cmd, uint16_t motor_id);
#endif // LK9025_H

27
modules/motor/dji_motor.c
View File

@ -48,7 +48,7 @@ static void IDcrash_Handler(uint8_t conflict_motor_idx, uint8_t temp_motor_idx)
*
* @param config
*/
static void MotorSenderGrouping(can_instance_config *config)
static void MotorSenderGrouping(can_instance_config_s *config)
{
uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值
uint8_t motor_send_num;
@ -146,29 +146,26 @@ static void DecodeDJIMotor(can_instance *_instance)
}
// 电机初始化,返回一个电机实例
dji_motor_instance *DJIMotorInit(can_instance_config can_config,
Motor_Control_Setting_s motor_setting,
Motor_Controller_Init_s controller_init,
Motor_Type_e type)
dji_motor_instance *DJIMotorInit(Motor_Init_Config_s config)
{
dji_motor_info[idx] = (dji_motor_instance *)malloc(sizeof(dji_motor_instance));
memset(dji_motor_info[idx], 0, sizeof(dji_motor_instance));
// motor basic setting
dji_motor_info[idx]->motor_type = type;
dji_motor_info[idx]->motor_settings = motor_setting;
dji_motor_info[idx]->motor_type = config.motor_type;
dji_motor_info[idx]->motor_settings = config.controller_setting_init_config;
// motor controller init
PID_Init(&dji_motor_info[idx]->motor_controller.current_PID, &controller_init.current_PID);
PID_Init(&dji_motor_info[idx]->motor_controller.speed_PID, &controller_init.speed_PID);
PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &controller_init.angle_PID);
dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = controller_init.other_angle_feedback_ptr;
dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = controller_init.other_speed_feedback_ptr;
PID_Init(&dji_motor_info[idx]->motor_controller.current_PID, &config.controller_param_init_config.current_PID);
PID_Init(&dji_motor_info[idx]->motor_controller.speed_PID, &config.controller_param_init_config.speed_PID);
PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &config.controller_param_init_config.angle_PID);
dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = config.controller_param_init_config.other_angle_feedback_ptr;
dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = config.controller_param_init_config.other_speed_feedback_ptr;
// group motors, because 4 motors share the same CAN control message
MotorSenderGrouping(&can_config);
MotorSenderGrouping(&config.can_init_config);
// register motor to CAN bus
can_config.can_module_callback = DecodeDJIMotor; // set callback
CANRegister(&dji_motor_info[idx]->motor_can_instance, can_config);
config.can_init_config.can_module_callback = DecodeDJIMotor; // set callback
CANRegister(&dji_motor_info[idx]->motor_can_instance, config.can_init_config);
return dji_motor_info[idx++];
}

21
modules/motor/dji_motor.h
View File

@ -75,23 +75,12 @@ typedef struct
*
* @attention M3508和M2006的反馈报文都是0x200+id,而GM6020的反馈是0x204+id,请注意前两者和后者的id不要冲突.
* 如果产生冲突,在初始化电机的时候会进入IDcrash_Handler(),可以通过debug来判断是否出现冲突.
*
* @param config 电机can设置,对于DJI电机仅需要将tx_id设置为电调闪动次数(c620,615,610)或拨码开关的值(GM6020)
* 你不需要自己查表计算发送和接收id,我们会帮你处理好!
*
* @param motor_setting 电机的控制设置,包括是否反转,闭环类型和是否使用编码器之外的反馈值
*
* @param controller_init 电机控制器的参数设置,包括其他的反馈来源数据指针和三环PID的参数.
* 如果不需要其他数据来源或不需要三个环,将不需要指针置为NULL即可
*
* @param type 电机的类型枚举,包括m2006,m3508和gm6020
*
* @return dji_motor_instance* 返回一个电机实例指针给应用,方便其对电机的参考值进行设置,即调用DJIMotorSetRef()
*
* @param config 电机初始化结构体,包含了电机控制设置,电机PID参数设置,电机类型以及电机挂载的CAN设置
*
* @return dji_motor_instance*
*/
dji_motor_instance *DJIMotorInit(can_instance_config config,
Motor_Control_Setting_s motor_setting,
Motor_Controller_Init_s controller_init,
Motor_Type_e type);
dji_motor_instance *DJIMotorInit(Motor_Init_Config_s config);
/**
* @brief 被application层的应用调用,给电机设定参考值.

187
modules/motor/dji_motor.md
View File

@ -20,9 +20,14 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详
初始化电机时，你需要传入的参数包括：
- 电机挂载的总线设置：CAN1 or CAN2，以及电机的id
- **电机挂载的CAN总线设置**，CAN1 or CAN2，以及电机的id，使用`can_instance_config_s`封装，只需要设置这两个参数:
- 电机类型：
```c
CAN_HandleTypeDef *can_handle;
uint32_t tx_id; // tx_id设置为电机id,不需要查说明书计算，直接为电调的闪动次数或拨码开关值，为1-8
```
- **电机类型**，使用`Motor_Type_e`：
```c
GM6020 = 0
@ -30,74 +35,122 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详
M2006 = 2
```
- 闭环类型
- **电机控制设置**
```c
OPEN_LOOP
CURRENT_LOOP
SPEED_LOOP
ANGLE_LOOP
CURRENT_LOOP| SPEED_LOOP // 同时对电流和速度闭环
SPEED_LOOP | ANGLE_LOOP // 同时对速度和位置闭环
CURRENT_LOOP| SPEED_LOOP |ANGLE_LOOP // 三环全开
```
- 闭环类型
- 是否反转
```c
OPEN_LOOP
CURRENT_LOOP
SPEED_LOOP
ANGLE_LOOP
CURRENT_LOOP| SPEED_LOOP // 同时对电流和速度闭环
SPEED_LOOP | ANGLE_LOOP // 同时对速度和位置闭环
CURRENT_LOOP| SPEED_LOOP |ANGLE_LOOP // 三环全开
```
```c
MOTOR_DIRECTION_NORMAL
MOTOR_DIRECTION_REVERSE
```
- 是否反转
- 是否其他反馈来源，以及他们对应的数据指针（如果有的话）
```c
MOTOR_DIRECTION_NORMAL
MOTOR_DIRECTION_REVERSE
```
```c
MOTOR_FEED = 0
OTHER_FEED = 1
---
float *other_angle_feedback_ptr
float *other_speed_feedback_ptr
// 电流只能从电机传感器获得所以无法设置其他来源
```
- 是否其他反馈来源，以及他们对应的数据指针（如果有的话）
- 每个环的PID参数以及是否使用改进功能
```c
MOTOR_FEED = 0
OTHER_FEED = 1
---
// 电流只能从电机传感器获得所以无法设置其他来源
```
- 每个环的PID参数以及是否使用改进功能，以及其他反馈来源指针（如果在上一步启用了其他数据来源）
```c
typedef struct // config parameter
{
float Kp;
float Ki;
float Kd;
float MaxOut; // 输出限幅
// 以下是优化参数
float IntegralLimit; // 积分限幅
float DeadBand; // 死区
float CoefA; // For Changing Integral
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
PID_Improvement_e Improve; // 优化环节，定义在下一个代码块
} PID_Init_config_s;
// 只有当你设启用了对应的优化环节，优化参数才会生效
```
```c
typedef enum
{
NONE = 0b00000000,
Integral_Limit = 0b00000001,
Derivative_On_Measurement = 0b00000010,
Trapezoid_Intergral = 0b00000100,
Proportional_On_Measurement = 0b00001000,
OutputFilter = 0b00010000,
ChangingIntegrationRate = 0b00100000,
DerivativeFilter = 0b01000000,
ErrorHandle = 0b10000000,
} PID_Improvement_e;
// 若希望使用多个环节的优化，这样就行：Integral_Limit |Trapezoid_Intergral|...|...
```
```c
float *other_angle_feedback_ptr
float *other_speed_feedback_ptr
```
---
推荐的初始化参数编写格式如下：
```c
Motor_Init_Config_s config = {
.motor_type = M3508, // 要注册的电机为3508电机
.can_init_config = {.can_handle = &hcan1, // 挂载在CAN1
.tx_id = 1}, // C620每隔一段时间闪动1次,设置为1
// 采用电机编码器角度与速度反馈,启用速度环和电流环,不反转
.controller_setting_init_config = {.angle_feedback_source = MOTOR_FEED,
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
.speed_feedback_source = MOTOR_FEED,
.reverse_flag = MOTOR_DIRECTION_NORMAL},
// 电流环和速度环PID参数的设置,不采用计算优化则不需要传入Improve参数
// 不使用其他数据来源(如IMU),不需要传入反馈数据变量指针
.controller_param_init_config = {.current_PID = {.Improve = 0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.DeadBand = 0,
.MaxOut = 4000},
.speed_PID = {.Improve = 0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.DeadBand = 0,
.MaxOut = 4000}}};
dji_motor_instance *djimotor = DJIMotorInit(config); // 设置好参数后进行初始化并保留返回的指针
```
---
```c
typedef struct // config parameter
{
float Kp;
float Ki;
float Kd;
float MaxOut; // 输出限幅
// 以下是优化参数
float IntegralLimit; // 积分限幅
float DeadBand; // 死区
float CoefA; // For Changing Integral
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
PID_Improvement_e Improve; // 优化环节，定义在下一个代码块
} PID_Init_config_s;
// 只有当你设启用了对应的优化环节，优化参数才会生效
```
```c
typedef enum
{
NONE = 0b00000000,
Integral_Limit = 0b00000001,
Derivative_On_Measurement = 0b00000010,
Trapezoid_Intergral = 0b00000100,
Proportional_On_Measurement = 0b00001000,
OutputFilter = 0b00010000,
ChangingIntegrationRate = 0b00100000,
DerivativeFilter = 0b01000000,
ErrorHandle = 0b10000000,
} PID_Improvement_e;
// 若希望使用多个环节的优化，这样就行：Integral_Limit |Trapezoid_Intergral|...|...
```
要控制一个DJI电机，我们提供了2个接口：
@ -113,6 +166,16 @@ void DJIMotorChangeFeed(dji_motor_instance *motor,
调用第二个并设定要修改的反馈环节和反馈类型，它会将反馈数据指针切换到你设定好的变量（需要在初始化的时候设置反馈指针）。
**如果需要获取电机的反馈数据**（如小陀螺模式需要根据麦克纳姆轮逆运动学解算底盘速度），直接通过你拥有的`dji_motor_instance`访问成员变量：
```c
// LeftForwardMotor是一个dji_motor_instance实例
float speed=LeftForwardMotor->motor_measure->speed_rpm;
...
```
***现在，忘记PID的计算和发送、接收以及协议解析，专注于模块之间的逻辑交互吧。***

9
modules/motor/motor_def.h
View File

@ -94,4 +94,13 @@ typedef struct
} Motor_Controller_Init_s;
/* 用于初始化CAN电机的结构体,各类电机通用 */
typedef struct
{
Motor_Controller_Init_s controller_param_init_config;
Motor_Control_Setting_s controller_setting_init_config;
Motor_Type_e motor_type;
can_instance_config_s can_init_config;
} Motor_Init_Config_s;
#endif // !MOTOR_DEF_H