From 4637cb8297c3b650489901c3c58033ed2d1f5b35 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Wed, 14 Dec 2022 13:33:19 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E6=88=90=E4=BA=86HTMotor=E5=92=8CLKMo?= =?UTF-8?q?tor=E6=A8=A1=E5=9D=97,=E5=B9=B6=E4=B8=BAmotor=5Fdef=E5=A2=9E?= =?UTF-8?q?=E5=8A=A0=E4=BA=86=E5=89=8D=E9=A6=88=E6=8C=87=E4=BB=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 1 + bsp/bsp_iic.c | 0 bsp/bsp_iic.h | 0 bsp/bsp_iic.md | 0 bsp/bsp_spi.c | 0 bsp/bsp_spi.h | 0 bsp/bsp_spi.md | 0 modules/can_comm/can_comm.c | 28 ++++----- modules/message_center/message_center.c | 2 + modules/motor/HT04.c | 60 +++++++++++++++++-- modules/motor/HT04.h | 4 ++ modules/motor/LK9025.c | 79 ++++++++++++++++++++++--- modules/motor/LK9025.h | 4 +- modules/motor/dji_motor.c | 2 +- modules/motor/motor_def.h | 13 +++- modules/super_cap/super_cap.c | 1 + 16 files changed, 160 insertions(+), 34 deletions(-) create mode 100644 bsp/bsp_iic.c create mode 100644 bsp/bsp_iic.h create mode 100644 bsp/bsp_iic.md create mode 100644 bsp/bsp_spi.c create mode 100644 bsp/bsp_spi.h create mode 100644 bsp/bsp_spi.md diff --git a/.vscode/settings.json b/.vscode/settings.json index 4decdde..7516960 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -45,4 +45,5 @@ "ht04.h": "c" }, "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", + "C_Cpp.intelliSenseEngineFallback": "enabled", } \ No newline at end of file diff --git a/bsp/bsp_iic.c b/bsp/bsp_iic.c new file mode 100644 index 0000000..e69de29 diff --git a/bsp/bsp_iic.h b/bsp/bsp_iic.h new file mode 100644 index 0000000..e69de29 diff --git a/bsp/bsp_iic.md b/bsp/bsp_iic.md new file mode 100644 index 0000000..e69de29 diff --git a/bsp/bsp_spi.c b/bsp/bsp_spi.c new file mode 100644 index 0000000..e69de29 diff --git a/bsp/bsp_spi.h b/bsp/bsp_spi.h new file mode 100644 index 0000000..e69de29 diff --git a/bsp/bsp_spi.md b/bsp/bsp_spi.md new file mode 100644 index 0000000..e69de29 diff --git a/modules/can_comm/can_comm.c b/modules/can_comm/can_comm.c index efe2eec..c13a916 100644 --- a/modules/can_comm/can_comm.c +++ b/modules/can_comm/can_comm.c @@ -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) diff --git a/modules/message_center/message_center.c b/modules/message_center/message_center.c index 443bff8..06fd1a3 100644 --- a/modules/message_center/message_center.c +++ b/modules/message_center/message_center.c @@ -64,6 +64,8 @@ void SubscribeEvent(char *name, void **data_ptr) s_pptr[idx++] = data_ptr; } + + /* ----------------------------------链表-队列版的实现----------------------------------- */ /* message_center是fake node,是方便链表编写的技巧,这样不需要处理链表头的特殊情况 */ diff --git a/modules/motor/HT04.c b/modules/motor/HT04.c index f3a7e74..7eb249a 100644 --- a/modules/motor/HT04.c +++ b/modules/motor/HT04.c @@ -89,15 +89,63 @@ void HTMotorSetRef(HTMotorInstance *motor, float ref) void HTMotorControl() { + static float set, pid_measure, pid_ref; static uint16_t tmp; + 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++) - { - // 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); + { // 先获取地址避免反复寻址 + 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) diff --git a/modules/motor/HT04.h b/modules/motor/HT04.h index b3a0538..4279771 100644 --- a/modules/motor/HT04.h +++ b/modules/motor/HT04.h @@ -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; diff --git a/modules/motor/LK9025.c b/modules/motor/LK9025.c index c265faf..fd98fa3 100644 --- a/modules/motor/LK9025.c +++ b/modules/motor/LK9025.c @@ -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; diff --git a/modules/motor/LK9025.h b/modules/motor/LK9025.h index e77e5d8..e5e46b6 100644 --- a/modules/motor/LK9025.h +++ b/modules/motor/LK9025.h @@ -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; diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index bda25c2..38487ee 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -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; diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index 2650f38..04ebaa1 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -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; diff --git a/modules/super_cap/super_cap.c b/modules/super_cap/super_cap.c index 6fe82e2..9c47809 100644 --- a/modules/super_cap/super_cap.c +++ b/modules/super_cap/super_cap.c @@ -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)