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

.vscode/settings.json

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

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));
+    CANCommInstance* ins = (CANCommInstance *)malloc(sizeof(CANCommInstance));
-    memset(can_comm_instance[idx], 0, sizeof(CANCommInstance));
+    memset(ins, 0, sizeof(CANCommInstance));
-    can_comm_instance[idx]->recv_data_len = comm_config->recv_data_len;
+    ins->recv_data_len = comm_config->recv_data_len;
-    can_comm_instance[idx]->recv_buf_len = comm_config->recv_data_len + CAN_COMM_OFFSET_BYTES;
+    ins->recv_buf_len = comm_config->recv_data_len + CAN_COMM_OFFSET_BYTES;
-    can_comm_instance[idx]->send_data_len = comm_config->send_data_len;
+    ins->send_data_len = comm_config->send_data_len;
-    can_comm_instance[idx]->send_buf_len = comm_config->send_data_len + CAN_COMM_OFFSET_BYTES;
+    ins->send_buf_len = comm_config->send_data_len + CAN_COMM_OFFSET_BYTES;
-    can_comm_instance[idx]->raw_sendbuf[0] = CAN_COMM_HEADER;
+    ins->raw_sendbuf[0] = CAN_COMM_HEADER;
-    can_comm_instance[idx]->raw_sendbuf[1] = comm_config->send_data_len;
+    ins->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;
+    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);
+    ins->can_ins = CANRegister(&comm_config->can_config);
-    return can_comm_instance[idx++];
+    return ins;
 }
 
 void CANCommSend(CANCommInstance *instance, uint8_t *data)

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,是方便链表编写的技巧,这样不需要处理链表头的特殊情况 */

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()
+        motor = ht_motor_info[i];
-        // _instance->motor_can_instace->rx_buff[6] = tmp >> 8;
+        measure = &motor->motor_measure;
-        // _instance->motor_can_instace->rx_buff[7] = tmp & 0xff;
+        setting = &motor->motor_settings;
-        // CANTransmit(_instance->motor_can_instace);
+        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)

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;
 

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;

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;

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;

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 *speed_feedforward_ptr;   // 速度前馈数据指针
-    float *current_feedforward_ptr;  // 电流前馈数据指针
+    float *current_feedforward_ptr; // 电流前馈数据指针
 
     PID_Init_config_s current_PID;
     PID_Init_config_s speed_PID;

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)