第一次提交

2026-05-29 17:36:34 +08:00
commit 5b3333125d
1318 changed files with 722705 additions and 0 deletions
@@ -0,0 +1,235 @@
+#include "HT04.h"
+#include "memory.h"
+#include "general_def.h"
+#include "user_lib.h"
+#include "cmsis_os.h"
+#include "string.h"
+#include "daemon.h"
+#include "stdlib.h"
+#include "bsp_log.h"
+
+static uint8_t idx;
+static HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT];
+static osThreadId ht_task_handle[HT_MOTOR_CNT];
+static uint8_t zero_buff[6] = {0};
+
+/**
+ * @brief 设置电机模式,报文内容[0xff,0xff,0xff,0xff,0xff,0xff,0xff,cmd]
+ *
+ * @param cmd
+ * @param motor
+ */
+static void HTMotorSetMode(HTMotor_Mode_t cmd, HTMotorInstance *motor)
+{
+    memset(motor->motor_can_instace->tx_buff, 0xff, 7);  // 发送电机指令的时候前面7bytes都是0xff
+    motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
+    CANTransmit(motor->motor_can_instace, 1);
+    memcpy(motor->motor_can_instace->tx_buff, zero_buff, 6);
+}
+/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
+static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
+{
+    float span = x_max - x_min;
+    float offset = x_min;
+    return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
+}
+static float uint_to_float(int x_int, float x_min, float x_max, int bits)
+{
+    float span = x_max - x_min;
+    float offset = x_min;
+    return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
+}
+
+/**
+ * @brief 解析电机反馈值
+ *
+ * @param motor_can 收到
+ */
+static void HTMotorDecode(CANInstance *motor_can)
+{
+    uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
+    uint8_t const *rxbuff = motor_can->rx_buff;
+    HTMotorInstance *motor = (HTMotorInstance *)motor_can->id;
+    HTMotor_Measure_t *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
+
+    DaemonReload(motor->motor_daemon);
+    measure->feed_dt = DWT_GetDeltaT(&measure->feed_cnt);
+
+    measure->last_angle = measure->total_angle;
+    tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
+    measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16);
+
+    tmp = (uint16_t)(rxbuff[3] << 4) | (rxbuff[4] >> 4);
+    measure->speed_rads = AverageFilter((uint_to_float(tmp, V_MIN, V_MAX, 12) - HT_SPEED_BIAS), measure->speed_buff, SPEED_BUFFER_SIZE);
+
+    tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
+    measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
+                            (1 - CURRENT_SMOOTH_COEF) * measure->real_current;
+}
+
+static void HTMotorLostCallback(void *motor_ptr)
+{
+    HTMotorInstance *motor = (HTMotorInstance *)motor_ptr;
+    LOGWARNING("[ht_motor] motor %d lost\n", motor->motor_can_instace->tx_id);
+    if (++motor->lost_cnt % 10 != 0)
+        HTMotorSetMode(CMD_MOTOR_MODE, motor); // 尝试重新让电机进入控制模式
+}
+
+/* 海泰电机一生黑,什么垃圾协议! */
+void HTMotorCalibEncoder(HTMotorInstance *motor)
+{
+    uint16_t p, v, kp, kd, t;
+    p = float_to_uint(0, P_MIN, P_MAX, 16);
+    v = float_to_uint(0, V_MIN, V_MAX, 12);
+    kp = float_to_uint(0, KP_MIN, KP_MAX, 12);
+    kd = float_to_uint(0, KD_MIN, KD_MAX, 12);
+    t = float_to_uint(0, T_MIN, T_MAX, 12);
+
+    uint8_t *buf = motor->motor_can_instace->tx_buff;
+    buf[0] = p >> 8;
+    buf[1] = p & 0xFF;
+    buf[2] = v >> 4;
+    buf[3] = ((v & 0xF) << 4) | (kp >> 8);
+    buf[4] = kp & 0xFF;
+    buf[5] = kd >> 4;
+    buf[6] = ((kd & 0xF) << 4) | (t >> 8);
+    buf[7] = t & 0xff;
+    memcpy(zero_buff, buf, 6); // 初始化的时候至少调用一次,故将其他指令为0时发送的报文保存一下,详见ht04电机说明
+    CANTransmit(motor->motor_can_instace, 1);
+    DWT_Delay(0.005);
+    HTMotorSetMode(CMD_ZERO_POSITION, motor); // sb 玩意校准完了编码器也不为0
+    DWT_Delay(0.005);
+    // HTMotorSetMode(CMD_MOTOR_MODE, motor);
+}
+
+HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
+{
+    HTMotorInstance *motor = (HTMotorInstance *)malloc(sizeof(HTMotorInstance));
+    memset(motor, 0, sizeof(HTMotorInstance));
+
+    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->can_init_config.can_module_callback = HTMotorDecode;
+    config->can_init_config.id = motor;
+    motor->motor_can_instace = CANRegister(&config->can_init_config);
+
+    Daemon_Init_Config_s conf = {
+        .callback = HTMotorLostCallback,
+        .owner_id = motor,
+        .reload_count = 5, // 20ms
+    };
+    motor->motor_daemon = DaemonRegister(&conf);
+
+    HTMotorEnable(motor);
+    HTMotorSetMode(CMD_MOTOR_MODE, motor); // 确保电机已经上电并执行电机模式
+    DWT_Delay(0.05f);
+    HTMotorCalibEncoder(motor); // 将当前编码器位置作为零位
+    DWT_Delay(0.05f);           // 保证下一个电机发送时CAN是空闲的,注意应用在初始化模块的时候不应该进入中断
+    ht_motor_instance[idx++] = motor;
+    return motor;
+}
+
+void HTMotorSetRef(HTMotorInstance *motor, float ref)
+{
+    motor->pid_ref = ref;
+}
+
+/**
+ * @brief 为了避免总线堵塞,为每个电机创建一个发送任务
+ * @param argument 传入的电机指针
+ */
+__attribute__((noreturn)) void HTMotorTask(void const *argument)
+{
+    float set, pid_measure, pid_ref;
+    HTMotorInstance *motor = (HTMotorInstance *)argument;
+    HTMotor_Measure_t *measure = &motor->measure;
+    Motor_Control_Setting_s *setting = &motor->motor_settings;
+    CANInstance *motor_can = motor->motor_can_instace;
+    uint16_t tmp;
+
+    while (1)
+    {
+        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->total_angle;
+            // measure单位是rad,ref是角度,统一到angle下计算,方便建模
+            pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_DEGREE, pid_ref);
+        }
+
+        if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
+        {
+            if (setting->feedforward_flag & SPEED_FEEDFORWARD)
+                pid_ref += *motor->speed_feedforward_ptr;
+
+            if (setting->angle_feedback_source == OTHER_FEED)
+                pid_measure = *motor->other_speed_feedback_ptr;
+            else
+                pid_measure = measure->speed_rads;
+            // measure单位是rad / s ,ref是angle per sec,统一到angle下计算
+            pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_DEGREE, pid_ref);
+        }
+
+        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);
+        }
+
+        set = pid_ref;
+        if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
+            set *= -1;
+
+        LIMIT_MIN_MAX(set, T_MIN, T_MAX);
+        tmp = float_to_uint(set, T_MIN, T_MAX, 12);
+        if (motor->stop_flag == MOTOR_STOP)
+            tmp = float_to_uint(0, T_MIN, T_MAX, 12);
+        motor_can->tx_buff[6] = (tmp >> 8);
+        motor_can->tx_buff[7] = tmp & 0xff;
+
+        CANTransmit(motor_can, 0.5);
+
+        osDelay(1);
+    }
+}
+
+void HTMotorControlInit()
+{
+    char ht_task_name[5] = "ht";
+    // 遍历所有电机实例,创建任务
+    if (!idx)
+        return;
+    for (size_t i = 0; i < idx; i++)
+    {
+        char ht_id_buff[2] = {0};
+        __itoa(i, ht_id_buff, 10);
+        strcat(ht_task_name, ht_id_buff); // 似乎没什么吊用,osthreaddef会把第一个变量当作宏字符串传入,作为任务名
+        // @todo 还需要一个更优雅的方案来区分不同的电机任务
+        osThreadDef(ht_task_name, HTMotorTask, osPriorityNormal, 0, 128);
+        ht_task_handle[i] = osThreadCreate(osThread(ht_task_name), ht_motor_instance[i]);
+    }
+}
+
+void HTMotorStop(HTMotorInstance *motor)
+{
+    motor->stop_flag = MOTOR_STOP;
+}
+
+void HTMotorEnable(HTMotorInstance *motor)
+{
+    motor->stop_flag = MOTOR_ENALBED;
+}
+
+void HTMotorOuterLoop(HTMotorInstance *motor, Closeloop_Type_e type)
+{
+    motor->motor_settings.outer_loop_type = type;
+}
@@ -0,0 +1,123 @@
+#ifndef HT04_H
+#define HT04_H
+
+#include <stdint.h>
+#include "bsp_can.h"
+#include "controller.h"
+#include "motor_def.h"
+#include "daemon.h"
+
+#define HT_MOTOR_CNT 4
+#define CURRENT_SMOOTH_COEF 0.9f
+#define SPEED_BUFFER_SIZE 5
+#define HT_SPEED_BIAS -0.0109901428f // 电机速度偏差,单位rad/s
+
+#define P_MIN -95.5f // Radians
+#define P_MAX 95.5f
+#define V_MIN -45.0f // Rad/s
+#define V_MAX 45.0f
+#define T_MIN -18.0f // N·m
+#define T_MAX 18.0f
+#define KP_MIN 0.0f // N-m/rad
+#define KP_MAX 500.0f
+#define KD_MIN 0.0f // N-m/rad/s
+#define KD_MAX 5.0f
+
+typedef struct // HT04
+{
+    float total_angle; // 角度为多圈角度,范围是-95.5~95.5,单位为rad
+    float last_angle;
+
+    float speed_rads;
+    float speed_buff[SPEED_BUFFER_SIZE];
+
+    float real_current;
+
+    float feed_dt;
+    uint32_t feed_cnt;
+} HTMotor_Measure_t;
+
+/* HT电机类型定义*/
+typedef struct
+{
+    HTMotor_Measure_t measure;
+
+    Motor_Control_Setting_s motor_settings;
+
+    PIDInstance current_PID;
+    PIDInstance speed_PID;
+    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;
+
+    DaemonInstance *motor_daemon;
+    uint32_t lost_cnt;
+} HTMotorInstance;
+
+/* HT电机模式,初始化时自动进入CMD_MOTOR_MODE*/
+typedef enum
+{
+    CMD_MOTOR_MODE = 0xfc,   // 使能,会响应指令
+    CMD_RESET_MODE = 0xfd,   // 停止
+    CMD_ZERO_POSITION = 0xfe // 将当前的位置设置为编码器零位
+} HTMotor_Mode_t;
+
+/**
+ * @brief
+ *
+ * @param config
+ * @return HTMotorInstance*
+ */
+HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config);
+
+/**
+ * @brief 设定电机的参考值
+ *
+ * @param motor 要设定的电机
+ * @param current   设定值
+ */
+void HTMotorSetRef(HTMotorInstance *motor, float ref);
+
+/**
+ * @brief 初始化电机任务,若要使用,需要在motortask的死循环前调用
+ *
+ */
+void HTMotorControlInit();
+
+/**
+ * @brief 停止电机,之后电机不会响应HTMotorSetRef设定的值
+ *
+ * @param motor
+ */
+void HTMotorStop(HTMotorInstance *motor);
+
+/**
+ * @brief 启动电机
+ *
+ * @param motor 要启动的电机
+ */
+void HTMotorEnable(HTMotorInstance *motor);
+
+/**
+ * @brief 修改电机闭环对象
+ *
+ * @param motor
+ * @param type
+ */
+void HTMotorOuterLoop(HTMotorInstance *motor, Closeloop_Type_e type);
+
+/**
+ * @brief 将当前编码器位置置零
+ *
+ * @param motor
+ */
+void HTMotorCalibEncoder(HTMotorInstance *motor);
+
+#endif // !HT04_H#define HT04_H
@@ -0,0 +1,44 @@
+- 在通过串口设置电机的时候，注意发送指令不要追加回车\n，否则电机端不响应。可以使用sscom，其会将你的按键直接当作ascii字符发送出去。
+
+- **此电机的CAN线序和RoboMaster开发板相反，注意单独制作CAN线**
+
+- 电机控制和反馈报文：
+  ![控制报文](%E6%8E%A7%E5%88%B6%E6%8A%A5%E6%96%87.png)
+
+注意，我们的代码实现不使用其电调协议的位置PD算法；自行实现了三环，并在最后给出电流参考值，发送给电调。因此command packed structure中的**pos cmd & vel cmd & Kp & Kd均为零**。报文均为小端，**低位在前**。
+
+请注意发送和反馈数据的**单位**。
+
+> ~~HT的电机协议做的真不行，纯纯直接抄mit还抄不明白，之后全部换成LK！~~
+
+另一种上电时校准编码器的方法:
+
+```c
+static void HTMotorDecode(CANInstance *motor_can)
+{
+    uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
+    uint8_t *rxbuff = motor_can->rx_buff;
+    HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->measure; // 将can实例中保存的id转换成电机实例的指针
+
+    measure->last_angle = measure->total_angle;
+    tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
+    measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16) - measure->angle_bias;
+
+    tmp = (uint16_t)(rxbuff[3] << 4) | (rxbuff[4] >> 4);
+    measure->speed_rads = AverageFilter((uint_to_float(tmp, V_MIN, V_MAX, 12) - HT_SPEED_BIAS), measure->speed_buff, SPEED_BUFFER_SIZE);
+
+    tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
+    measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
+                            (1 - CURRENT_SMOOTH_COEF) * measure->real_current;
+
+    if (!measure->first_feedback_flag) // 初始化的时候设置偏置
+    {
+        measure->first_feedback_flag = 1;
+        measure->angle_bias = measure->total_angle;
+        measure->total_angle = 0;
+        measure->speed_rads = 0;
+    }
+}
+```
+
+第一次收到数据时默认电机处于限位处,将速度和角度都设置为零,记录当前的编码器数据,之后每次收到都减去该值.