增加转台

drive_rs04.c

@@ -0,0 +1,212 @@
+#include "interface.h"
+#include "drive_rs04.h"
+
+// 限制值在最小值和最大值之间
+float constrain(float value, float min_val, float max_val)
+{
+    if (value < min_val)
+    {
+        return min_val;
+    }
+    else if (value > max_val)
+    {
+        return max_val;
+    }
+    else
+    {
+        return value;
+    }
+}
+
+
+
+/**
+ * @brief 带死区的原始数据到物理量转换函数（简单版）
+ * @param raw_value 原始16位无符号整数值 [0, 65535]
+ * @param min 物理量最小值（如 -10.0）
+ * @param max 物理量最大值（如 +10.0）
+ * @param deadzone 死区范围（物理量单位，如 1.0 表示 ±1.0 内为死区）
+ * @return 转换后的物理量值（若在死区内返回0，否则返回实际值）
+ */
+float convertPhysical(uint16_t raw_value, float min, float max, float deadzone) 
+{
+    // 1. 计算实际物理量值
+    float physical_value = min + ((float)raw_value / 65535.0f) * (max - min);
+
+    // 2. 判断是否在死区内（绝对值 ≤ deadzone）
+    if (fabs(physical_value) <= deadzone) 
+    {
+        return 0.0f;  // 死区内返回0
+    } 
+    else 
+    {
+        return physical_value;  // 死区外返回实际值
+    }
+}
+
+
+/**
+ * @brief 将浮点数转换为uint32_t（按小端序存储）
+ * @param num 输入的浮点数
+ * @return 转换后的uint32_t值（直接内存拷贝结果）
+ * @note 此函数通过内存直接拷贝实现转换，不进行数值计算，结果受平台字节序影响
+ */
+uint32_t floatToUint32(float num) 
+{
+    uint32_t result;
+    // 将浮点数的内存数据直接拷贝到uint32_t变量
+    memcpy(&result, &num, sizeof(num));
+    return result;
+}
+
+
+/**
+ * @brief 电机失能函数（停止电机运行）
+ * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
+ * @param master_id 主控制器ID (用于标识发送方)
+ * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
+ * @return 0: 成功, -1: 参数无效
+ * @note 此函数会修改unsdodata指向的结构体内容，调用后需及时发送CAN报文
+ */
+int8_t motorDisable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata)
+{
+    /* 参数有效性检查 */
+    if (unsdodata == NULL) {
+        return -1;
+    }
+
+    /* 设置CAN报文ID域 */
+    unsdodata->tx_can_id.bits.mode = 3;           /* 通信模式3：电机失能 */
+    unsdodata->tx_can_id.bits.motor_id = motor_id;  /* 目标电机ID */
+    unsdodata->tx_can_id.bits.res = 0;            /* 保留位清零 */
+    unsdodata->tx_can_id.bits.data = master_id;   /* 主控制器ID */
+    
+    /* 清零数据域 */
+    unsdodata->tx_can_data.bit_data.data = 0;
+    unsdodata->tx_can_data.bit_data.index = 0;
+    unsdodata->tx_can_data.bit_data.object_index = 0;    
+    
+    return 0;
+}
+
+/**
+ * @brief 电机使能函数（启动电机运行）
+ * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
+ * @param master_id 主控制器ID (用于标识发送方)
+ * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
+ * @return 0: 成功, -1: 参数无效
+ * @note 通信模式4：电机使能
+ */
+int8_t motorEnable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata)
+{
+    /* 参数有效性检查 */
+    if (unsdodata == NULL) {
+        return -1;
+    }
+
+    /* 设置CAN报文ID域 */
+    unsdodata->tx_can_id.bits.mode = 3;           /* 通信模式4：电机使能 */
+    unsdodata->tx_can_id.bits.motor_id = motor_id;  /* 目标电机ID */
+    unsdodata->tx_can_id.bits.res = 0;            /* 保留位清零 */
+    unsdodata->tx_can_id.bits.data = master_id;   /* 主控制器ID */
+    
+    /* 清零数据域 */
+    unsdodata->tx_can_data.bit_data.data = 0;
+    unsdodata->tx_can_data.bit_data.index = 0;
+    unsdodata->tx_can_data.bit_data.object_index = 0;  
+    
+    return 0;
+}
+
+/**
+ * @brief 设置电机运行模式
+ * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
+ * @param master_id 主控制器ID (用于标识发送方)
+ * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
+ * @param mode 要设置的模式值 (具体含义需参考电机协议文档)
+ * @return 0: 成功, -1: 参数无效
+ * @note RUM_MODE应为预定义的宏，表示运行模式索引
+ */
+int8_t setMotorMode(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint8_t mode)
+{
+    /* 参数有效性检查 */
+    if (unsdodata == NULL) {
+        return -1;
+    }
+
+    /* 设置CAN报文ID域 */
+    unsdodata->tx_can_id.bits.mode = 0x12;        /* 通信模式0x12：参数写入 */
+    unsdodata->tx_can_id.bits.motor_id = motor_id;  /* 目标电机ID */
+    unsdodata->tx_can_id.bits.res = 0;            /* 保留位清零 */
+    unsdodata->tx_can_id.bits.data = master_id;   /* 主控制器ID */
+    
+    /* 设置数据域 */
+    unsdodata->tx_can_data.bit_data.index = RUM_MODE;      /* 运行模式索引 */
+    unsdodata->tx_can_data.bit_data.object_index = 0;      /* 子索引通常为0 */
+    unsdodata->tx_can_data.bit_data.data = mode;           /* 模式值 */
+    
+    return 0;
+}
+
+/**
+ * @brief 写入电机参数
+ * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
+ * @param master_id 主控制器ID (用于标识发送方)
+ * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
+ * @param index 要写入的参数索引 (具体含义需参考电机协议文档)
+ * @param ref 要写入的参数值 (浮点数，会自动转换为uint32_t)
+ * @return 0: 成功, -1: 参数无效
+ * @note 使用floatToUint32函数转换浮点参数
+ */
+int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint16_t index, float ref)
+{
+    /* 参数有效性检查 */
+    if (unsdodata == NULL) {
+        return -1;
+    }
+
+    /* 设置CAN报文ID域 */
+    unsdodata->tx_can_id.bits.mode = 0x12;        /* 通信模式0x12：参数写入 */
+    unsdodata->tx_can_id.bits.motor_id = motor_id;  /* 目标电机ID */
+    unsdodata->tx_can_id.bits.res = 0;            /* 保留位清零 */
+    unsdodata->tx_can_id.bits.data = master_id;   /* 主控制器ID */
+    
+    /* 设置数据域 */
+    unsdodata->tx_can_data.bit_data.index = index;         /* 参数索引 */
+    unsdodata->tx_can_data.bit_data.object_index = 0;      /* 子索引通常为0 */
+    unsdodata->tx_can_data.bit_data.data = floatToUint32(ref); /* 转换并写入参数值 */
+    
+    return 0;
+}
+
+
+/**
+ * @brief 动态斜率限制（支持变时间间隔）
+ * @param last_command   上一次的电流指令值
+ * @param target_current 本次目标电流指令
+ * @param delta_time     距离上一次调用的时间间隔 (s)
+ * @return              限制后的安全电流指令
+ */
+float dynamic_current_limit(float *last_command, float target_current, float delta_time) 
+{
+    // 计算期望的变化量
+    float desired_change = target_current - *last_command;
+
+    // 计算两种限制
+    float step_limit = MAX_STEP;
+    float time_limit = MAX_DI_DT * delta_time; // 动态计算时间限制
+
+    // 选择更严格的限制
+    float max_allowed_change = (step_limit < time_limit) ? step_limit : time_limit;
+
+    // 应用限制并返回新指令
+    float actual_change = constrain(desired_change, -max_allowed_change, max_allowed_change);
+    *last_command = *last_command + actual_change;//更新过去值
+
+    return *last_command; 
+}
+
+
+
+
+