diff --git a/HAL_N_Middlewares/Src/main.c b/HAL_N_Middlewares/Src/main.c
index fd640c3..a072fef 100644
--- a/HAL_N_Middlewares/Src/main.c
+++ b/HAL_N_Middlewares/Src/main.c
@@ -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) */
diff --git a/bsp/bsp_can.c b/bsp/bsp_can.c
index e1cbfee..104c075 100644
--- a/bsp/bsp_can.c
+++ b/bsp/bsp_can.c
@@ -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)
diff --git a/bsp/bsp_can.h b/bsp/bsp_can.h
index 35f1457..e6faf21 100644
--- a/bsp/bsp_can.h
+++ b/bsp/bsp_can.h
@@ -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
diff --git a/modules/master_machine/湖南大学RoboMaster电控组通信协议.md b/modules/master_machine/湖南大学RoboMaster电控组通信协议.md
index 149a6d9..5becc33 100644
--- a/modules/master_machine/湖南大学RoboMaster电控组通信协议.md
+++ b/modules/master_machine/湖南大学RoboMaster电控组通信协议.md
@@ -1,7 +1,16 @@
# 湖南大学RoboMaster电控组通信协议
+Seasky LIUWei
+
+modified by neozng1@hnu.edu.cn
+
可用于视觉，以及其他自研模块（仅限串口通信）
+> TODO:
+>
+> 1. 利用F4自带的硬件CRC模块计算校验码，提高速度
+> 2. 增加更多的数据类型支持，使得数据类型也为可配置的
+
## 一、串口配置
通信方式是串口，配置为波特率 115200，8 位数据位，1 位停止位，无硬件流控，无校验位。
diff --git a/modules/motor/HT04.c b/modules/motor/HT04.c
index 38aa69d..db4a7e8 100644
--- a/modules/motor/HT04.c
+++ b/modules/motor/HT04.c
@@ -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<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);
}
diff --git a/modules/motor/HT04.h b/modules/motor/HT04.h
index 09965a8..5da5f1f 100644
--- a/modules/motor/HT04.h
+++ b/modules/motor/HT04.h
@@ -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
\ No newline at end of file
diff --git a/modules/motor/LK9025.c b/modules/motor/LK9025.c
index dc24273..7baa307 100644
--- a/modules/motor/LK9025.c
+++ b/modules/motor/LK9025.c
@@ -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));
diff --git a/modules/motor/LK9025.h b/modules/motor/LK9025.h
index 57e0a59..e3092fc 100644
--- a/modules/motor/LK9025.h
+++ b/modules/motor/LK9025.h
@@ -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
diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c
index 4ef9146..a17ca4b 100644
--- a/modules/motor/dji_motor.c
+++ b/modules/motor/dji_motor.c
@@ -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++];
}
diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h
index bc65703..cf4b3e8 100644
--- a/modules/motor/dji_motor.h
+++ b/modules/motor/dji_motor.h
@@ -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层的应用调用,给电机设定参考值.
diff --git a/modules/motor/dji_motor.md b/modules/motor/dji_motor.md
index 77924b4..a931ce8 100644
--- a/modules/motor/dji_motor.md
+++ b/modules/motor/dji_motor.md
@@ -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|motor_measure->speed_rpm;
+...
+```
+
+
+
***现在，忘记PID的计算和发送、接收以及协议解析，专注于模块之间的逻辑交互吧。***
diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h
index bd68887..a46073d 100644
--- a/modules/motor/motor_def.h
+++ b/modules/motor/motor_def.h
@@ -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