add lk motor drv

User_Code/module/motor/can_motors/lk_motor/lk_motor.c

@@ -0,0 +1,177 @@
+#include "lk_motor.h"
+#include "stdlib.h"
+#include "general_def.h"
+#include "daemon.h"
+#include "bsp_dwt.h"
+#include "bsp_log.h"
+
+static uint8_t idx;
+static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
+static FDCANInstance *sender_instance; // 多电机发送时使用的caninstance(当前保存的是注册的第一个电机的caninstance)
+// 后续考虑兼容单电机和多电机指令.
+
+/**
+ * @brief 电机反馈报文解析
+ *
+ * @param _instance 发生中断的caninstance
+ */
+static void LKMotorDecode(FDCANInstance *_instance)
+{
+    LKMotorInstance *motor = (LKMotorInstance *)_instance->id; // 通过caninstance保存的father id获取对应的motorinstance
+    LKMotor_Measure_t *measure = &motor->measure;
+    uint8_t *rx_buff = _instance->rx_buff;
+
+    DaemonReload(motor->daemon); // 喂狗
+    measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt);
+
+    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_rads = (1 - SPEED_SMOOTH_COEF) * measure->speed_rads +
+                          DEGREE_2_RAD * 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->temperature = rx_buff[1];
+
+    if (measure->ecd - measure->last_ecd > 65536)//MFV2是18bit编码器,这里用65536判断是否发生了跨越零点
+        measure->total_round--;
+    else if (measure->ecd - measure->last_ecd < -65536)
+        measure->total_round++;
+    measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
+}
+
+static void LKMotorLostCallback(void *motor_ptr)
+{
+    LKMotorInstance *motor = (LKMotorInstance *)motor_ptr;
+    LOGWARNING("[LKMotor] motor lost, id: %d", motor->motor_can_ins->tx_id);
+}
+
+LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
+{
+    LKMotorInstance *motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
+    motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
+    memset(motor, 0, sizeof(LKMotorInstance));
+
+    motor->motor_settings = config->controller_setting_init_config;
+    PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
+    PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
+    PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
+    motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
+    motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
+
+    config->fdcan_init_config.id = motor;
+    config->fdcan_init_config.can_module_callback = LKMotorDecode;
+    config->fdcan_init_config.rx_id = 0x140 + config->fdcan_init_config.tx_id;
+    config->fdcan_init_config.tx_id = config->fdcan_init_config.tx_id + 0x280 - 1; // 这样在发送写入buffer的时候更方便,因为下标从0开始,LK多电机发送id为0x280
+    motor->motor_can_ins = CANRegister(&config->fdcan_init_config);
+
+    if (idx == 0) // 用第一个电机的can instance发送数据
+    {
+        sender_instance = motor->motor_can_ins;
+        sender_instance->tx_id = 0x280; //  修改tx_id为0x280,用于多电机发送,不用管其他LKMotorInstance的tx_id,它们仅作初始化用
+    }
+
+    LKMotorEnable(motor);
+    DWT_GetDeltaT(&motor->measure.feed_dwt_cnt);
+    lkmotor_instance[idx++] = motor;
+
+    Daemon_Init_Config_s daemon_config = {
+        .callback = LKMotorLostCallback,
+        .owner_id = motor,
+        .reload_count = 5, // 50ms
+    };
+    motor->daemon = DaemonRegister(&daemon_config);
+
+    return motor;
+}
+
+/* 第一个电机的can instance用于发送数据,向其tx_buff填充数据 */
+void LKMotorControl()
+{
+    float pid_measure, pid_ref;
+    int16_t set;
+    LKMotorInstance *motor;
+    LKMotor_Measure_t *measure;
+    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->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
+            pid_ref *= -1;
+
+        // 角度环计算
+        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->total_angle;  // 修正:使用角度反馈
+            pid_ref = PIDCalculate(&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->speed_feedback_source == OTHER_FEED)  // 修正:判断speed_feedback_source
+                pid_measure = *motor->other_speed_feedback_ptr;
+            else
+                pid_measure = measure->speed_rads;  // 修正:使用速度反馈
+            pid_ref = PIDCalculate(&motor->speed_PID, pid_measure, pid_ref);  // 修正:使用speed_PID
+            if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
+                pid_ref += *motor->current_feedforward_ptr;
+        }
+
+        // 电流环计算
+        if (setting->close_loop_type & CURRENT_LOOP)
+        {
+            pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
+        }
+
+        // 反馈方向反转
+        if (setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
+            pid_ref *= -1;
+
+        set = (int16_t)pid_ref;
+
+        memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t));
+
+        if (motor->stop_flag == MOTOR_STOP)
+        {
+            memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, 0, sizeof(uint16_t));
+        }
+    }
+
+    if (idx)
+        CANTransmit(sender_instance, 0.2);
+}
+
+void LKMotorStop(LKMotorInstance *motor)
+{
+    motor->stop_flag = MOTOR_STOP;
+}
+
+void LKMotorEnable(LKMotorInstance *motor)
+{
+    motor->stop_flag = MOTOR_ENALBED;
+}
+
+void LKMotorSetRef(LKMotorInstance *motor, float ref)
+{
+    motor->pid_ref = ref;
+}
+
+uint8_t LKMotorIsOnline(LKMotorInstance *motor)
+{
+    return DaemonIsOnline(motor->daemon);
+}

User_Code/module/motor/can_motors/lk_motor/lk_motor.h

@@ -0,0 +1,98 @@
+#ifndef LK_MOTOR_H
+#define LK_MOTOR_H
+
+#include "stdint.h"
+#include "bsp_fdcan.h"
+#include "pid.h"
+#include "motor_def.h"
+#include "daemon.h"
+
+#define LK_MOTOR_MX_CNT 4 // 最多允许4个LK电机使用多电机指令,挂载在一条总线上
+
+#define I_MIN -2000
+#define I_MAX 2000
+#define CURRENT_SMOOTH_COEF 0.9f
+#define SPEED_SMOOTH_COEF 0.85f
+#define REDUCTION_RATIO_DRIVEN 1
+#define ECD_ANGLE_COEF_LK (360.0f / 65536.0f)
+#define CURRENT_TORQUE_COEF_LK 0.003645f // 电流设定值转换成扭矩的系数,算出来的设定值除以这个系数就是扭矩值
+
+typedef struct // 9025
+{
+    uint16_t last_ecd;        // 上一次读取的编码器值
+    uint16_t ecd;             // 当前编码器值
+    float angle_single_round; // 单圈角度
+    float speed_rads;         // speed rad/s
+    int16_t real_current;     // 实际电流
+    uint8_t temperature;      // 温度,C°
+
+    float total_angle;   // 总角度
+    int32_t total_round; // 总圈数
+
+    float feed_dt;
+    uint32_t feed_dwt_cnt;
+} LKMotor_Measure_t;
+
+typedef struct
+{
+    LKMotor_Measure_t measure;
+
+    Motor_Control_Setting_s motor_settings;
+
+    float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
+    float *other_speed_feedback_ptr;
+    float *speed_feedforward_ptr;   // 速度前馈数据指针,可以通过此指针设置速度前馈值,或LQR等时作为速度状态变量的输入
+    float *current_feedforward_ptr; // 电流前馈指针
+    PIDInstance current_PID;
+    PIDInstance speed_PID;
+    PIDInstance angle_PID;
+    float pid_ref;
+
+    Motor_Working_Type_e stop_flag; // 启停标志
+
+    FDCANInstance *motor_can_ins;
+
+    Daemon_Instance *daemon;
+
+} LKMotorInstance;
+
+/**
+ * @brief 初始化LK电机
+ *
+ * @param config 电机配置
+ * @return LKMotorInstance* 返回实例指针
+ */
+LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config);
+
+/**
+ * @brief 设置参考值
+ * @attention 注意此函数设定的ref是最外层闭环的输入,若要设定内层闭环的值请通过前馈数据指针设置
+ *
+ * @param motor 要设置的电机
+ * @param ref 设定值
+ */
+void LKMotorSetRef(LKMotorInstance *motor, float ref);
+
+/**
+ * @brief 为所有LK电机计算pid/反转/模式控制,并通过bspcan发送电流值(发送CAN报文)
+ *
+ */
+void LKMotorControl();
+
+/**
+ * @brief 停止LK电机,之后电机不会响应任何指令
+ *
+ * @param motor
+ */
+void LKMotorStop(LKMotorInstance *motor);
+
+/**
+ * @brief 启动LK电机
+ *
+ * @param motor
+ */
+void LKMotorEnable(LKMotorInstance *motor);
+
+uint8_t LKMotorIsOnline(LKMotorInstance *motor);
+
+#endif // LK_MOTOR_H