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

完成了HTMotor和LKMotor模块,并为motor_def增加了前馈指令

This commit is contained in:
NeoZng 2022-12-14 13:33:19 +08:00
parent 67fbf9064d
commit 4637cb8297
16 changed files with 160 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.vscode/settings.json vendored
View File

@ -45,4 +45,5 @@
"ht04.h": "c"
},
"C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools",
"C_Cpp.intelliSenseEngineFallback": "enabled",
}

0
bsp/bsp_iic.c Normal file
View File

0
bsp/bsp_iic.h Normal file
View File

0
bsp/bsp_iic.md Normal file
View File

0
bsp/bsp_spi.c Normal file
View File

0
bsp/bsp_spi.h Normal file
View File

0
bsp/bsp_spi.md Normal file
View File

28
modules/can_comm/can_comm.c
View File

@ -3,10 +3,6 @@
#include "stdlib.h"
#include "crc8.h"
/* can_comm用于保存每个实例的指针数组,用于回调函数区分实例 */
static CANCommInstance *can_comm_instance[MX_CAN_COMM_COUNT] = {NULL};
static uint8_t idx; // 配合can_comm_instance的初始化使用,标识当前初始化的是哪一个实例
/**
* @brief 重置CAN comm的接收状态和buffer
*
@ -77,20 +73,20 @@ static void CANCommRxCallback(CANInstance *_instance)
CANCommInstance *CANCommInit(CANComm_Init_Config_s *comm_config)
{
can_comm_instance[idx] = (CANCommInstance *)malloc(sizeof(CANCommInstance));
memset(can_comm_instance[idx], 0, sizeof(CANCommInstance));
can_comm_instance[idx]->recv_data_len = comm_config->recv_data_len;
can_comm_instance[idx]->recv_buf_len = comm_config->recv_data_len + CAN_COMM_OFFSET_BYTES;
can_comm_instance[idx]->send_data_len = comm_config->send_data_len;
can_comm_instance[idx]->send_buf_len = comm_config->send_data_len + CAN_COMM_OFFSET_BYTES;
can_comm_instance[idx]->raw_sendbuf[0] = CAN_COMM_HEADER;
can_comm_instance[idx]->raw_sendbuf[1] = comm_config->send_data_len;
can_comm_instance[idx]->raw_sendbuf[comm_config->send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL;
CANCommInstance* ins = (CANCommInstance *)malloc(sizeof(CANCommInstance));
memset(ins, 0, sizeof(CANCommInstance));
ins->recv_data_len = comm_config->recv_data_len;
ins->recv_buf_len = comm_config->recv_data_len + CAN_COMM_OFFSET_BYTES;
ins->send_data_len = comm_config->send_data_len;
ins->send_buf_len = comm_config->send_data_len + CAN_COMM_OFFSET_BYTES;
ins->raw_sendbuf[0] = CAN_COMM_HEADER;
ins->raw_sendbuf[1] = comm_config->send_data_len;
ins->raw_sendbuf[comm_config->send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL;
comm_config->can_config.id = can_comm_instance[idx];
comm_config->can_config.id = ins;
comm_config->can_config.can_module_callback = CANCommRxCallback;
can_comm_instance[idx]->can_ins = CANRegister(&comm_config->can_config);
return can_comm_instance[idx++];
ins->can_ins = CANRegister(&comm_config->can_config);
return ins;
}
void CANCommSend(CANCommInstance *instance, uint8_t *data)

2
modules/message_center/message_center.c
View File

@ -64,6 +64,8 @@ void SubscribeEvent(char *name, void **data_ptr)
s_pptr[idx++] = data_ptr;
}
/* ----------------------------------链表-队列版的实现----------------------------------- */
/* message_center是fake node,是方便链表编写的技巧,这样不需要处理链表头的特殊情况 */

62
modules/motor/HT04.c
View File

@ -89,15 +89,63 @@ void HTMotorSetRef(HTMotorInstance *motor, float ref)
void HTMotorControl()
{
static float set, pid_measure, pid_ref;
static uint16_t tmp;
for (size_t i = 0; i < idx; i++)
{
// tmp=float_to_uint()
// _instance->motor_can_instace->rx_buff[6] = tmp >> 8;
// _instance->motor_can_instace->rx_buff[7] = tmp & 0xff;
// CANTransmit(_instance->motor_can_instace);
}
static HTMotorInstance *motor;
static HTMotor_Measure_t *measure;
static Motor_Control_Setting_s *setting;
static CANInstance *motor_can;
// 遍历所有电机实例,计算PID
for (size_t i = 0; i < idx; i++)
{ // 先获取地址避免反复寻址
motor = ht_motor_info[i];
measure = &motor->motor_measure;
setting = &motor->motor_settings;
motor_can = motor_can;
pid_ref = motor->pid_ref;
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
{
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_angle_feedback_ptr;
else
pid_measure = measure->real_current;
pid_ref = PID_Calculate(&motor->angle_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor->speed_feedforward_ptr;
}
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
{
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else
pid_measure = measure->speed_aps;
pid_ref = PID_Calculate(&motor->angle_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr;
}
if (setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PID_Calculate(&motor->current_PID, measure->real_current, pid_ref);
}
set = pid_ref;
if (setting->reverse_flag == MOTOR_DIRECTION_REVERSE)
set *= -1;
tmp = float_to_uint(set, T_MIN, T_MAX, 12);
motor_can->tx_buff[6] = tmp >> 8;
motor_can->tx_buff[7] = tmp & 0xff;
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将发送buff置零
memset(motor_can->tx_buff + 6, 0, sizeof(uint16_t));
}
CANTransmit(motor_can);
}
}
void HTMotorStop(HTMotorInstance *motor)

4
modules/motor/HT04.h
View File

@ -37,8 +37,12 @@ typedef struct
PIDInstance angle_PID;
float *other_angle_feedback_ptr;
float *other_speed_feedback_ptr;
float *speed_feedforward_ptr;
float *current_feedforward_ptr;
float pid_ref;
Motor_Working_Type_e stop_flag; // 启停标志
CANInstance *motor_can_instace;
} HTMotorInstance;

79
modules/motor/LK9025.c
View File

@ -3,6 +3,8 @@
static uint8_t idx;
static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
static CANInstance *sender_instance; // 多电机发送时使用的caninstance(当前保存的是注册的第一个电机的caninstance)
// 后续考虑兼容单电机和多电机指令.
static void LKMotorDecode(CANInstance *_instance)
{
@ -13,11 +15,15 @@ static void LKMotorDecode(CANInstance *_instance)
measure->last_ecd = measure->ecd;
measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd;
measure->speed_aps = (1 - SPEED_SMOOTH_COEF) * measure->speed_aps +
SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
measure->real_current = (1 - CURRENT_SMOOTH_COEF) * measure->real_current +
CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
measure->temperate = rx_buff[1];
// 计算多圈角度
@ -28,13 +34,6 @@ static void LKMotorDecode(CANInstance *_instance)
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
}
void LKMotorControl()
{
for (size_t i = 0; i < idx; ++i)
{
}
}
LKMotorInstance *LKMotroInit(Motor_Init_Config_s *config)
{
lkmotor_instance[idx] = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
@ -50,13 +49,77 @@ LKMotorInstance *LKMotroInit(Motor_Init_Config_s *config)
config->can_init_config.id = lkmotor_instance[idx];
config->can_init_config.can_module_callback = LKMotorDecode;
config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id;
config->can_init_config.tx_id = config->can_init_config.tx_id + 0x240;
config->can_init_config.tx_id = config->can_init_config.tx_id + 0x280;
lkmotor_instance[idx]->motor_can_ins = CANRegister(&config->can_init_config);
if (idx == 0)
sender_instance = lkmotor_instance[idx]->motor_can_ins;
LKMotorEnable(lkmotor_instance[idx]);
return lkmotor_instance[idx++];
}
/* 第一个电机的can instance用于发送数据,向其tx_buff填充数据 */
void LKMotorControl()
{
static float pid_measure, pid_ref;
static int16_t set;
static LKMotorInstance *motor;
static LKMotor_Measure_t *measure;
static Motor_Control_Setting_s *setting;
for (size_t i = 0; i < idx; ++i)
{
motor = lkmotor_instance[i];
measure = &motor->measure;
setting = &motor->motor_settings;
pid_ref = motor->pid_ref;
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
{
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_angle_feedback_ptr;
else
pid_measure = measure->real_current;
pid_ref = PID_Calculate(&motor->angle_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor->speed_feedforward_ptr;
}
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
{
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else
pid_measure = measure->speed_aps;
pid_ref = PID_Calculate(&motor->angle_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr;
}
if (setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PID_Calculate(&motor->current_PID, measure->real_current, pid_ref);
}
set = pid_ref;
if (setting->reverse_flag == MOTOR_DIRECTION_REVERSE)
set *= -1;
// 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance
memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280 - 1) * 2, &set, sizeof(uint16_t));
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将发送buff置零
memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280 - 1) * 2, 0, sizeof(uint16_t));
}
}
if (idx) // 如果有电机注册了
{
CANTransmit(sender_instance);
}
}
void LKMotorStop(LKMotorInstance *motor)
{
motor->stop_flag = MOTOR_STOP;

4
modules/motor/LK9025.h
View File

@ -6,7 +6,7 @@
#include "controller.h"
#include "motor_def.h"
#define LK_MOTOR_MX_CNT 4
#define LK_MOTOR_MX_CNT 4 // 最多允许4个LK电机使用多电机指令,挂载在一条总线上
#define I_MIN -2000
#define I_MAX 2000
@ -37,6 +37,8 @@ typedef struct
float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
float *other_speed_feedback_ptr;
float *speed_feedforward_ptr;
float *current_feedforward_ptr;
PIDInstance current_PID;
PIDInstance speed_PID;
PIDInstance angle_PID;

2
modules/motor/dji_motor.c
View File

@ -242,7 +242,7 @@ void DJIMotorControl()
}
// 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type | (ANGLE_LOOP | SPEED_LOOP)))
if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
{
if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_speed_feedback_ptr;

13
modules/motor/motor_def.h
View File

@ -34,6 +34,14 @@ typedef enum
___ = 0b0111
} Closeloop_Type_e;
typedef enum
{
FEEDFORWARD_NONE = 0b00,
CURRENT_FEEDFORWARD = 0b01,
SPEED_FEEDFORWARD = 0b10,
CURRENT_AND_SPEED_FEEDFORWARD = CURRENT_FEEDFORWARD | SPEED_FEEDFORWARD,
} Feedfoward_Type_e;
/* 反馈来源设定,若设为OTHER_FEED则需要指定数据来源指针,详见Motor_Controller_s*/
typedef enum
{
@ -62,6 +70,7 @@ typedef struct
Reverse_Flag_e reverse_flag; // 是否反转
Feedback_Source_e angle_feedback_source; // 角度反馈类型
Feedback_Source_e speed_feedback_source; // 速度反馈类型
Feedfoward_Type_e feedforward_flag; // 前馈标志
} Motor_Control_Setting_s;
@ -102,8 +111,8 @@ typedef struct
float *other_angle_feedback_ptr; // 角度反馈数据指针,注意电机使用total_angle
float *other_speed_feedback_ptr; // 速度反馈数据指针,单位为angle per sec
float *speed_feedforward_ptr; // 速度前馈数据指针
float *current_feedforward_ptr; // 电流前馈数据指针
float *speed_feedforward_ptr; // 速度前馈数据指针
float *current_feedforward_ptr; // 电流前馈数据指针
PID_Init_config_s current_PID;
PID_Init_config_s speed_PID;

1
modules/super_cap/super_cap.c
View File

@ -8,6 +8,7 @@
#include "super_cap.h"
#include "memory.h"
#include "stdlib.h"
static SuperCapInstance *super_cap_instance = NULL;
static void SuperCapRxCallback(CANInstance *_instance)