TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

增加了大量注释

Browse Source
This commit is contained in:
NeoZng
2023-01-01 17:32:22 +08:00
parent c2f8b5c8c3
commit c05513587c
56 changed files with 773 additions and 598 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
modules/motor/dji_motor.c
View File

@@ -10,23 +10,23 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL};
* @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送
* 该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行
*
* C610(m2006)/C620(m3508):0x1ff,0x200; GM6020:0x1ff,0x2ff
* 反馈: GM6020: 0x204+id ; C610/C620: 0x200+id
* C610(m2006)/C620(m3508):0x1ff,0x200;
* GM6020:0x1ff,0x2ff
* 反馈(rx_id): GM6020: 0x204+id ; C610/C620: 0x200+id
* can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
* can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
*/
static CANInstance sender_assignment[6] =
{
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
static CANInstance sender_assignment[6] = {
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
};
/**
* @brief 6个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在 DJIMotorControl() 使用
* @brief 6个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用
* flag的初始化在 MotorSenderGrouping()中进行
*
*/
@@ -38,9 +38,7 @@ static uint8_t sender_enable_flag[6] = {0};
*/
static void IDcrash_Handler(uint8_t conflict_motor_idx, uint8_t temp_motor_idx)
{
while (1)
{
};
while (1);
}
/**
@@ -108,7 +106,7 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config)
break;
default: // other motors should not be registered here
break;
while(1); // 其他电机不应该在这里注册
}
}
@@ -124,9 +122,10 @@ static void DecodeDJIMotor(CANInstance *_instance)
static uint8_t *rxbuff;
static DJI_Motor_Measure_s *measure;
rxbuff = _instance->rx_buff;
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
measure = &((DJIMotorInstance *)_instance->id)->motor_measure; // measure要多次使用,保存指针减小访存开销
// resolve data and apply filter to current and speed
// 解析数据并对电流和速度进行滤波
measure->last_ecd = measure->ecd;
measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd;
@@ -136,7 +135,7 @@ static void DecodeDJIMotor(CANInstance *_instance)
CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
measure->temperate = rxbuff[6];
// multi rounds calc,计算的前提是两次采样间电机转过的角度小于180°
// 多圈角度计算,计算的前提是两次采样间电机转过的角度小于180°
if (measure->ecd - measure->last_ecd > 4096)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -4096)
@@ -150,23 +149,23 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
DJIMotorInstance *instance = (DJIMotorInstance *)malloc(sizeof(DJIMotorInstance));
memset(instance, 0, sizeof(DJIMotorInstance));
// motor basic setting
// motor basic setting 电机基本设置
instance->motor_type = config->motor_type;
instance->motor_settings = config->controller_setting_init_config;
// motor controller init
// motor controller init 电机控制器初始化
PID_Init(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
PID_Init(&instance->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
PID_Init(&instance->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
instance->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
instance->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
// 电机分组,因为至多4个电机可以共用一帧CAN控制报文
MotorSenderGrouping(&config->can_init_config);
// register motor to CAN bus
// 注册电机到CAN总线
config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
config->can_init_config.id = instance; // set id,eq to address(it is identity)
config->can_init_config.id = instance;// set id,eq to address(it is identity)
instance->motor_can_instance = CANRegister(&config->can_init_config);
DJIMotorEnable(instance);
@@ -228,7 +227,7 @@ void DJIMotorControl()
static float pid_measure, pid_ref; // 电机PID测量值和设定值
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
for (size_t i = 0; i < idx; ++i)
{ // 减小访存开销,先保存指针引用
{ // 减小访存开销,先保存指针引用
motor = dji_motor_instance[i];
motor_setting = &motor->motor_settings;
motor_controller = &motor->motor_controller;
@@ -280,7 +279,6 @@ void DJIMotorControl()
{ // 若该电机处于停止状态,直接将buff置零
memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
}
}
// 遍历flag,检查是否要发送这一帧报文

35
modules/motor/dji_motor.h
View File

@@ -22,22 +22,22 @@
#define DJI_MOTOR_CNT 12
/* 滤波系数设置为1的时候即关闭滤波 */
#define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.85
#define CURRENT_SMOOTH_COEF 0.9f // this coef *must* be greater than 0.9
#define ECD_ANGLE_COEF_DJI (360.0f/8192.0f) // ,将编码器值转化为角度制
#define SPEED_SMOOTH_COEF 0.85f // 最好大于0.85
#define CURRENT_SMOOTH_COEF 0.9f // 必须大于0.9
#define ECD_ANGLE_COEF_DJI 0.043945f // (360/8192),将编码器值转化为角度制
/* DJI电机CAN反馈信息*/
typedef struct
{
uint16_t last_ecd; // 上一次读取的编码器值
uint16_t ecd; // 0-8191,刻度总共有8192格
uint16_t last_ecd; // 上一次读取的编码器值
uint16_t ecd; // 0-8191,刻度总共有8192格
float angle_single_round; // 单圈角度
float speed_aps; // 角速度,单位为:度/秒 rounds per minute
int16_t real_current; // 实际电流
uint8_t temperate; // 温度 Celsius
int16_t real_current; // 实际电流
uint8_t temperate; // 温度 Celsius
float total_angle; // 总角度,注意方向
int32_t total_round; // 总圈数,注意方向
float total_angle; // 总角度,注意方向
int32_t total_round; // 总圈数,注意方向
} DJI_Motor_Measure_s;
/**
@@ -46,25 +46,18 @@ typedef struct
*/
typedef struct
{
/* motor measurement recv from CAN feedback */
DJI_Motor_Measure_s motor_measure;
/* basic config of a motor*/
Motor_Control_Setting_s motor_settings;
DJI_Motor_Measure_s motor_measure; // 电机测量值
Motor_Control_Setting_s motor_settings; // 电机设置
Motor_Controller_s motor_controller; // 电机控制器
/* controller used in the motor (3 loops)*/
Motor_Controller_s motor_controller;
/* the CAN instance own by motor instance*/
CANInstance *motor_can_instance;
/* sender assigment*/
CANInstance *motor_can_instance; // 电机CAN实例
// 分组发送设置
uint8_t sender_group;
uint8_t message_num;
Motor_Type_e motor_type; // 电机类型
Motor_Working_Type_e stop_flag; // 启停标志
} DJIMotorInstance;
/**