diff --git a/CMakeLists.txt b/CMakeLists.txt index 68ba1d1..d7caa0f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -48,44 +48,55 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE # Add sources to executable target_sources(${CMAKE_PROJECT_NAME} PRIVATE # Add user sources here - modules/sensor/imu/j901p.c - modules/sensor/imu/j901p_hal.c - modules/sensor/imu/j901p_module.c + 3rdparty/jy901p/wit_c_sdk.c modules/device/motor/mf4010v2.c modules/device/led/led.c modules/device/can/can_device.c modules/control/pid/pid.c + modules/bus/soft_i2c/soft_i2c.c + modules/bus/soft_i2c/soft_i2c_hal.c bsp/stm32f4/can/bsp_can.c bsp/stm32f4/led/bsp_led.c bsp/stm32f4/i2c/bsp_i2c.c + bsp/stm32f4/gpio/bsp_gpio.c hal/stm32f4/can/hal_can.c hal/stm32f4/led/hal_led.c hal/stm32f4/i2c/hal_i2c.c - app/app.c + hal/stm32f4/gpio/hal_gpio.c + app/app_init.c + app/control_task.c + app/sensor_task.c + app/sensor_i2c_port.c interfaces/led/led_if.c interfaces/can/can_if.c interfaces/i2c/i2c_if.c + interfaces/gpio/gpio_if.c ) # Add include paths target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE # Add user defined include paths + ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/jy901p ${CMAKE_CURRENT_SOURCE_DIR}/modules/device/led ${CMAKE_CURRENT_SOURCE_DIR}/modules/device/motor ${CMAKE_CURRENT_SOURCE_DIR}/modules/device/can ${CMAKE_CURRENT_SOURCE_DIR}/modules/sensor/imu ${CMAKE_CURRENT_SOURCE_DIR}/modules/control/pid + ${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/soft_i2c ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/can ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/led ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/i2c + ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/gpio ${CMAKE_CURRENT_SOURCE_DIR}/app ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/led ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/can ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/i2c + ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/gpio ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/led ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/can ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/i2c + ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/gpio ) # Add project symbols (macros) diff --git a/app/control_task.c b/app/control_task.c new file mode 100644 index 0000000..f096826 --- /dev/null +++ b/app/control_task.c @@ -0,0 +1,162 @@ +/** + * @file control_task.c + * @brief 二轴云台增稳控制任务 + * @note 目标：保持 Roll=0, Pitch=0，Yaw 用于方向控制 + * + * 控制架构： + * IMU 数据 → PID 控制器 → 电机补偿角度 → 抵消基座倾斜 + */ + +#include "app.h" +#include "cmsis_os2.h" +#include "mf4010v2.h" +#include + +/* ============================================================================ + * 外部变量 (来自 sensor_task) + * ============================================================================ */ +extern float fAcc[3], fGyro[3], fAngle[3]; /* fAngle[0]=Roll, fAngle[1]=Pitch, fAngle[2]=Yaw */ + +/* ============================================================================ + * 云台配置 + * ============================================================================ */ + +/* 目标角度：保持水平 */ +static float g_target_roll = 0.0f; +static float g_target_pitch = 0.0f; +static float g_target_yaw = 0.0f; /* Yaw 目标可通过外部修改 */ + +/* 电机实例 */ +static mf4010v2_t motor_yaw; /* Yaw 轴电机（外框） */ +static mf4010v2_t motor_pitch; /* Pitch 轴电机（内框） */ + +/* PID 控制器 */ +typedef struct { + float Kp, Ki, Kd; + float integral; + float prev_error; + float output_max; +} PID_t; + +static PID_t g_roll_pid = {0}; +static PID_t g_pitch_pid = {0}; + +/* ============================================================================ + * PID 控制函数 + * ============================================================================ */ + +void PID_Init(PID_t *pid, float kp, float ki, float kd, float max) +{ + pid->Kp = kp; + pid->Ki = ki; + pid->Kd = kd; + pid->output_max = max; + pid->integral = 0; + pid->prev_error = 0; +} + +float PID_Compute(PID_t *pid, float target, float actual) +{ + float error = target - actual; + float output; + + /* 比例项 */ + output = pid->Kp * error; + + /* 积分项（带抗饱和） */ + pid->integral += error; + if (pid->integral > pid->output_max) { + pid->integral = pid->output_max; + } else if (pid->integral < -pid->output_max) { + pid->integral = -pid->output_max; + } + output += pid->Ki * pid->integral; + + /* 微分项 */ + float derivative = error - pid->prev_error; + output += pid->Kd * derivative; + pid->prev_error = error; + + /* 输出限幅 */ + if (output > pid->output_max) { + output = pid->output_max; + } else if (output < -pid->output_max) { + output = -pid->output_max; + } + + return output; +} + +/* ============================================================================ + * 控制任务 + * ============================================================================ */ + +void control_task(void) +{ + int32_t yaw_cmd, pitch_cmd; + float roll_comp, pitch_comp; + + printf("[gimbal] starting stabilization control\n"); + + /* 1. 初始化 PID 参数 */ + /* Roll 轴 PID：控制 Yaw 电机来补偿 Roll 倾斜 */ + PID_Init(&g_roll_pid, 0.5f, 0.01f, 0.1f, 18000.0f); /* 最大补偿 180 度 */ + + /* Pitch 轴 PID：控制 Pitch 电机补偿 Pitch 倾斜 */ + PID_Init(&g_pitch_pid, 0.5f, 0.01f, 0.1f, 9000.0f); /* 最大补偿 90 度 */ + + /* 2. 初始化电机 */ + mf4010v2_init(&motor_yaw, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */ + mf4010v2_init(&motor_pitch, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */ + + /* 3. 电机关机状态启动 */ + mf4010v2_close(&motor_yaw); + mf4010v2_close(&motor_pitch); + + /* 等待 IMU 稳定 */ + osDelay(pdMS_TO_TICKS(1000)); + + /* 4. 使能电机 */ + mf4010v2_run(&motor_yaw); + mf4010v2_run(&motor_pitch); + + printf("[gimbal] stabilization enabled\n"); + + /* 主控制循环 (100Hz) */ + while (1) { + /* 读取 IMU 数据 */ + float current_roll = fAngle[0]; /* Roll 角 */ + float current_pitch = fAngle[1]; /* Pitch 角 */ + + /* PID 计算补偿量 */ + /* 目标：Roll=0, Pitch=0 */ + roll_comp = PID_Compute(&g_roll_pid, g_target_roll, current_roll); + pitch_comp = PID_Compute(&g_pitch_pid, g_target_pitch, current_pitch); + + /* 运动学映射：补偿量 -> 电机角度 */ + /* 对于 Pitch-Yaw 云台： + * - Pitch 倾斜 → Pitch 电机反向补偿 + * - Roll 倾斜 → Yaw 电机 + Pitch 电机联合补偿（简化为 Yaw 电机） + */ + yaw_cmd = (int32_t)(roll_comp * 100.0f); /* 转为 0.01 度 */ + pitch_cmd = (int32_t)(pitch_comp * 100.0f); + + /* 发送位置指令到电机 */ + /* 使用增量位置控制，直接叠加补偿量 */ + mf4010v2_set_angle(&motor_yaw, motor_yaw.current_angle + yaw_cmd); + mf4010v2_set_angle(&motor_pitch, motor_pitch.current_angle + pitch_cmd); + + /* 100Hz 控制循环 */ + osDelay(pdMS_TO_TICKS(10)); + } +} + +/* ============================================================================ + * 外部 API + * ============================================================================ */ + +/* 设置 Yaw 目标方向 */ +void gimbal_set_yaw_target(float yaw_deg) +{ + g_target_yaw = yaw_deg; +} diff --git a/app/sensor_i2c_port.c b/app/sensor_i2c_port.c new file mode 100644 index 0000000..35d3cc6 --- /dev/null +++ b/app/sensor_i2c_port.c @@ -0,0 +1,83 @@ +/** + * @file sensor_i2c_port.c + * @brief Sensor I2C 端口适配层（基于 Soft I2C） + * @note 替换原有的 IIC_* 函数，使用 Soft I2C 实现 + */ + +#include "soft_i2c.h" +#include + +/* ============================================================================ + * 配置区域 + * ============================================================================ */ + +/** + * @brief Soft I2C 实例配置 + * @note 根据实际硬件连接修改引脚 + */ +#define SOFT_I2C_SDA_GPIO 0 /* SDA 引脚通道 */ +#define SOFT_I2C_SCL_GPIO 1 /* SCL 引脚通道 */ +#define SOFT_I2C_DELAY_US 5 /* 5us = 约 100kHz */ + +/* 全局 Soft I2C 实例 */ +static soft_i2c_t g_sensor_i2c; +static int g_i2c_initialized = 0; + +/* ============================================================================ + * I2C 底层函数实现（替换原有的 IIC_* 函数） + * ============================================================================ */ + +/** + * @brief 初始化 Soft I2C（单次调用） + */ +static void SoftI2C_EnsureInit(void) +{ + if (!g_i2c_initialized) { + soft_i2c_config_t config = { + .sda_gpio_ch = SOFT_I2C_SDA_GPIO, + .scl_gpio_ch = SOFT_I2C_SCL_GPIO, + .delay_us = SOFT_I2C_DELAY_US, + .open_drain = true + }; + soft_i2c_init(&g_sensor_i2c, &config); + g_i2c_initialized = 1; + } +} + +/** + * @brief 写入多个字节（带寄存器地址） + * @param dev: 设备地址（7 位） + * @param reg: 寄存器地址 + * @param data: 数据缓冲区 + * @param length: 数据长度 + * @retval 成功返回 1，失败返回 0 + */ +int32_t IICwriteBytes(uint8_t dev, uint8_t reg, uint8_t* data, uint32_t length) +{ + int ret; + + SoftI2C_EnsureInit(); + + ret = soft_i2c_mem_write(&g_sensor_i2c, dev, reg, 1, data, length, 0); + + return (ret == SOFT_I2C_OK) ? 1 : 0; +} + +/** + * @brief 读取多个字节（带寄存器地址） + * @param dev: 设备地址（7 位） + * @param reg: 寄存器地址 + * @param data: 数据缓冲区 + * @param length: 数据长度 + * @retval 成功返回 1，失败返回 0 + */ +int32_t IICreadBytes(uint8_t dev, uint8_t reg, uint8_t *data, uint32_t length) +{ + int ret; + + SoftI2C_EnsureInit(); + + ret = soft_i2c_mem_read(&g_sensor_i2c, dev, reg, 1, data, length, 0); + + return (ret == SOFT_I2C_OK) ? 1 : 0; +} diff --git a/app/sensor_i2c_port.h b/app/sensor_i2c_port.h new file mode 100644 index 0000000..b6b14f5 --- /dev/null +++ b/app/sensor_i2c_port.h @@ -0,0 +1,40 @@ +/** + * @file sensor_i2c_port.h + * @brief Sensor I2C 端口适配层头文件 + * @note 替换原有的 IIC_* 函数声明 + */ + +#ifndef __SENSOR_I2C_PORT_H__ +#define __SENSOR_I2C_PORT_H__ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief 写入多个字节（带寄存器地址） + * @param dev: 设备地址（7 位） + * @param reg: 寄存器地址 + * @param data: 数据缓冲区 + * @param length: 数据长度 + * @retval 成功返回 1，失败返回 0 + */ +int32_t IICwriteBytes(uint8_t dev, uint8_t reg, uint8_t* data, uint32_t length); + +/** + * @brief 读取多个字节（带寄存器地址） + * @param dev: 设备地址（7 位） + * @param reg: 寄存器地址 + * @param data: 数据缓冲区 + * @param length: 数据长度 + * @retval 成功返回 1，失败返回 0 + */ +int32_t IICreadBytes(uint8_t dev, uint8_t reg, uint8_t *data, uint32_t length); + +#ifdef __cplusplus +} +#endif + +#endif /* __SENSOR_I2C_PORT_H__ */ diff --git a/modules/bus/soft_i2c/soft_i2c.c b/modules/bus/soft_i2c/soft_i2c.c new file mode 100644 index 0000000..998f984 --- /dev/null +++ b/modules/bus/soft_i2c/soft_i2c.c @@ -0,0 +1,454 @@ +/** + * @file soft_i2c.c + * @brief 软件 I2C（位模拟）驱动程序实现 + * @note 基于 GPIO 位时序实现 + */ + +#include "soft_i2c.h" +#include "cmsis_os2.h" +#include "gpio_if.h" +#include "cmsis_compiler.h" +#include "projdefs.h" + +/* ============================================================================ + * 内部辅助函数 + * ============================================================================ */ + +/** + * @brief 微秒级延时 + * @note 简单软件延时，实际时间取决于编译器优化和主频 + * 对于精确时序，建议替换为定时器延时 + */ +void soft_i2c_delay_us(uint32_t us) +{ +#ifndef + volatile uint32_t count; + /* 假设 168MHz 主频，每个循环约 1us */ + /* 根据实际主频调整系数 */ + while (us--) { + for (count = 0; count < 21; count++) { + __NOP(); + } + } +#else + osDelay(pdMS_TO_TICKS(us/1000u)); +#endif +} + +/** + * @brief 设置 SDA 输出 + */ +static void soft_i2c_sda_out(soft_i2c_t *i2c, uint8_t val) +{ + gpio_if_write(i2c->config.sda_gpio_ch, val); +} + +/** + * @brief 读取 SDA 输入（释放为输入模式） + * @note 软件 I2C 需要在读取前释放 SDA 线 + */ +static uint8_t soft_i2c_sda_in(soft_i2c_t *i2c) +{ + /* 读取时 SDA 应为输入模式 */ + /* 注意：当前 gpio_if 没有输入/输出模式切换 */ + /* 假设 GPIO 已配置为合适的模式 */ + return (uint8_t)gpio_if_read(i2c->config.sda_gpio_ch); +} + +/** + * @brief 设置 SCL 输出 + */ +static void soft_i2c_scl_out(soft_i2c_t *i2c, uint8_t val) +{ + gpio_if_write(i2c->config.scl_gpio_ch, val); +} + +/** + * @brief 读取 SCL 输入（未使用，保留用于时钟拉伸支持） + */ +#if 0 +static uint8_t soft_i2c_scl_in(soft_i2c_t *i2c) +{ + return (uint8_t)gpio_if_read(i2c->config.scl_gpio_ch); +} +#endif + +/** + * @brief 产生半个 I2C 周期延时 + */ +static inline void soft_i2c_half_cycle(soft_i2c_t *i2c) +{ + soft_i2c_delay_us(i2c->config.delay_us); +} + +/* ============================================================================ + * 核心 API 实现 + * ============================================================================ */ + +int soft_i2c_init(soft_i2c_t *i2c, const soft_i2c_config_t *config) +{ + if (!i2c || !config) { + return SOFT_I2C_ERR_INVALID_PARAM; + } + + /* 复制配置 */ + i2c->config = *config; + i2c->bus_busy = false; + i2c->last_error = 0; + + /* 初始化 GPIO */ + /* 注意：GPIO 模式应在外部配置好（开漏输出 + 上拉） */ + if (gpio_if_init(config->sda_gpio_ch) != 0) { + return SOFT_I2C_ERR_INVALID_PARAM; + } + if (gpio_if_init(config->scl_gpio_ch) != 0) { + return SOFT_I2C_ERR_INVALID_PARAM; + } + + /* 初始状态：SDA 和 SCL 为高（空闲状态）*/ + soft_i2c_sda_out(i2c, 1); + soft_i2c_scl_out(i2c, 1); + soft_i2c_half_cycle(i2c); + + return SOFT_I2C_OK; +} + +int soft_i2c_start(soft_i2c_t *i2c) +{ + if (!i2c) { + return SOFT_I2C_ERR_INVALID_PARAM; + } + + /* 检查总线是否忙 */ + if (i2c->bus_busy) { + /* 可选：总线忙处理 */ + } + + /* SCL 高期间，SDA 由高变低 -> 起始条件 */ + soft_i2c_sda_out(i2c, 1); + soft_i2c_half_cycle(i2c); + soft_i2c_scl_out(i2c, 1); + soft_i2c_half_cycle(i2c); + soft_i2c_sda_out(i2c, 0); + soft_i2c_half_cycle(i2c); + soft_i2c_scl_out(i2c, 0); + soft_i2c_half_cycle(i2c); + + i2c->bus_busy = true; + + return SOFT_I2C_OK; +} + +int soft_i2c_stop(soft_i2c_t *i2c) +{ + if (!i2c) { + return SOFT_I2C_ERR_INVALID_PARAM; + } + + /* SCL 高期间，SDA 由低变高 -> 停止条件 */ + soft_i2c_sda_out(i2c, 0); + soft_i2c_half_cycle(i2c); + soft_i2c_scl_out(i2c, 1); + soft_i2c_half_cycle(i2c); + soft_i2c_sda_out(i2c, 1); + soft_i2c_half_cycle(i2c); + + i2c->bus_busy = false; + + return SOFT_I2C_OK; +} + +int soft_i2c_write_byte(soft_i2c_t *i2c, uint8_t data) +{ + uint8_t i; + uint8_t ack; + + if (!i2c) { + return SOFT_I2C_ERR_INVALID_PARAM; + } + + /* 发送 8 位数据，MSB 在前 */ + for (i = 0; i < 8; i++) { + /* SCL 低期间改变 SDA */ + soft_i2c_scl_out(i2c, 0); + + /* 输出数据位 */ + if (data & 0x80) { + soft_i2c_sda_out(i2c, 1); + } else { + soft_i2c_sda_out(i2c, 0); + } + data <<= 1; + + soft_i2c_half_cycle(i2c); + + /* SCL 高期间数据有效 */ + soft_i2c_scl_out(i2c, 1); + soft_i2c_half_cycle(i2c); + soft_i2c_scl_out(i2c, 0); + } + + /* 释放 SDA，准备读取 ACK */ + soft_i2c_sda_out(i2c, 1); + soft_i2c_half_cycle(i2c); + + /* 读取 ACK/NACK */ + soft_i2c_scl_out(i2c, 1); + soft_i2c_half_cycle(i2c); + ack = soft_i2c_sda_in(i2c); + soft_i2c_scl_out(i2c, 0); + soft_i2c_half_cycle(i2c); + + if (ack) { + return SOFT_I2C_ERR_NACK; + } + + return SOFT_I2C_OK; +} + +int soft_i2c_read_byte(soft_i2c_t *i2c, bool ack, uint8_t *data) +{ + uint8_t i; + uint8_t result = 0; + + if (!i2c || !data) { + return SOFT_I2C_ERR_INVALID_PARAM; + } + + /* 释放 SDA（作为输入）*/ + soft_i2c_sda_out(i2c, 1); + soft_i2c_half_cycle(i2c); + + /* 读取 8 位数据，MSB 在前 */ + for (i = 0; i < 8; i++) { + /* SCL 高期间读取数据 */ + soft_i2c_scl_out(i2c, 1); + soft_i2c_half_cycle(i2c); + + result <<= 1; + if (soft_i2c_sda_in(i2c)) { + result |= 0x01; + } + + soft_i2c_scl_out(i2c, 0); + soft_i2c_half_cycle(i2c); + } + + *data = result; + + /* 发送 ACK/NACK */ + if (ack) { + soft_i2c_sda_out(i2c, 0); /* ACK */ + } else { + soft_i2c_sda_out(i2c, 1); /* NACK */ + } + soft_i2c_half_cycle(i2c); + soft_i2c_scl_out(i2c, 1); + soft_i2c_half_cycle(i2c); + soft_i2c_scl_out(i2c, 0); + soft_i2c_half_cycle(i2c); + + return SOFT_I2C_OK; +} + +int soft_i2c_send_addr(soft_i2c_t *i2c, uint8_t addr, bool write) +{ + uint8_t addr_byte; + + if (!i2c) { + return SOFT_I2C_ERR_INVALID_PARAM; + } + + /* 7 位地址 + 读写位 */ + addr_byte = (addr << 1) | (write ? 0x00 : 0x01); + + return soft_i2c_write_byte(i2c, addr_byte); +} + +/* ============================================================================ + * 高级 API 实现 + * ============================================================================ */ + +int soft_i2c_mem_write(soft_i2c_t *i2c, uint16_t dev_addr, uint16_t mem_addr, + uint16_t mem_addr_size, const uint8_t *data, uint16_t size, + uint32_t timeout) +{ + uint16_t i; + int ret; + + (void)timeout; /* 未使用 */ + + if (!i2c || !data || (mem_addr_size != 1 && mem_addr_size != 2)) { + return SOFT_I2C_ERR_INVALID_PARAM; + } + + ret = soft_i2c_start(i2c); + if (ret != SOFT_I2C_OK) return ret; + + /* 发送设备地址（写）*/ + ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, true); + if (ret != SOFT_I2C_OK) { + soft_i2c_stop(i2c); + return ret; + } + + /* 发送寄存器/内存地址 */ + if (mem_addr_size == 2) { + ret = soft_i2c_write_byte(i2c, (uint8_t)(mem_addr >> 8)); + if (ret != SOFT_I2C_OK) { + soft_i2c_stop(i2c); + return ret; + } + } + ret = soft_i2c_write_byte(i2c, (uint8_t)mem_addr); + if (ret != SOFT_I2C_OK) { + soft_i2c_stop(i2c); + return ret; + } + + /* 发送数据 */ + for (i = 0; i < size; i++) { + ret = soft_i2c_write_byte(i2c, data[i]); + if (ret != SOFT_I2C_OK) { + soft_i2c_stop(i2c); + return ret; + } + } + + ret = soft_i2c_stop(i2c); + return ret; +} + +int soft_i2c_mem_read(soft_i2c_t *i2c, uint16_t dev_addr, uint16_t mem_addr, + uint16_t mem_addr_size, uint8_t *data, uint16_t size, + uint32_t timeout) +{ + uint16_t i; + int ret; + + (void)timeout; /* 未使用 */ + + if (!i2c || !data || (mem_addr_size != 1 && mem_addr_size != 2)) { + return SOFT_I2C_ERR_INVALID_PARAM; + } + + /* 第一阶段：写入寄存器地址 */ + ret = soft_i2c_start(i2c); + if (ret != SOFT_I2C_OK) return ret; + + /* 发送设备地址（写）*/ + ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, true); + if (ret != SOFT_I2C_OK) { + soft_i2c_stop(i2c); + return ret; + } + + /* 发送寄存器/内存地址 */ + if (mem_addr_size == 2) { + ret = soft_i2c_write_byte(i2c, (uint8_t)(mem_addr >> 8)); + if (ret != SOFT_I2C_OK) { + soft_i2c_stop(i2c); + return ret; + } + } + ret = soft_i2c_write_byte(i2c, (uint8_t)mem_addr); + if (ret != SOFT_I2C_OK) { + soft_i2c_stop(i2c); + return ret; + } + + /* 重复起始条件（Repeated Start）*/ + ret = soft_i2c_start(i2c); + if (ret != SOFT_I2C_OK) return ret; + + /* 第二阶段：读取数据 */ + /* 发送设备地址（读）*/ + ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, false); + if (ret != SOFT_I2C_OK) { + soft_i2c_stop(i2c); + return ret; + } + + /* 读取数据 */ + for (i = 0; i < size; i++) { + /* 最后一个字节发送 NACK，否则发送 ACK */ + ret = soft_i2c_read_byte(i2c, (i < (size - 1)), &data[i]); + if (ret != SOFT_I2C_OK) { + soft_i2c_stop(i2c); + return ret; + } + } + + ret = soft_i2c_stop(i2c); + return ret; +} + +int soft_i2c_write(soft_i2c_t *i2c, uint16_t dev_addr, const uint8_t *data, + uint16_t size, uint32_t timeout) +{ + uint16_t i; + int ret; + + (void)timeout; /* 未使用 */ + + if (!i2c || !data) { + return SOFT_I2C_ERR_INVALID_PARAM; + } + + ret = soft_i2c_start(i2c); + if (ret != SOFT_I2C_OK) return ret; + + /* 发送设备地址（写）*/ + ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, true); + if (ret != SOFT_I2C_OK) { + soft_i2c_stop(i2c); + return ret; + } + + /* 发送数据 */ + for (i = 0; i < size; i++) { + ret = soft_i2c_write_byte(i2c, data[i]); + if (ret != SOFT_I2C_OK) { + soft_i2c_stop(i2c); + return ret; + } + } + + ret = soft_i2c_stop(i2c); + return ret; +} + +int soft_i2c_read(soft_i2c_t *i2c, uint16_t dev_addr, uint8_t *data, + uint16_t size, uint32_t timeout) +{ + uint16_t i; + int ret; + + (void)timeout; /* 未使用 */ + + if (!i2c || !data) { + return SOFT_I2C_ERR_INVALID_PARAM; + } + + ret = soft_i2c_start(i2c); + if (ret != SOFT_I2C_OK) return ret; + + /* 发送设备地址（读）*/ + ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, false); + if (ret != SOFT_I2C_OK) { + soft_i2c_stop(i2c); + return ret; + } + + /* 读取数据 */ + for (i = 0; i < size; i++) { + ret = soft_i2c_read_byte(i2c, (i < (size - 1)), &data[i]); + if (ret != SOFT_I2C_OK) { + soft_i2c_stop(i2c); + return ret; + } + } + + ret = soft_i2c_stop(i2c); + return ret; +} diff --git a/modules/bus/soft_i2c/soft_i2c.h b/modules/bus/soft_i2c/soft_i2c.h new file mode 100644 index 0000000..c82b0da --- /dev/null +++ b/modules/bus/soft_i2c/soft_i2c.h @@ -0,0 +1,192 @@ +#ifndef __SOFT_I2C_H__ +#define __SOFT_I2C_H__ + +/** + * @file soft_i2c.h + * @brief 软件 I2C（位模拟）驱动程序 + * @note 基于 GPIO 位时序实现的 I2C 总线驱动 + * - 支持多实例（多个独立的 soft I2C 总线） + * - 标准模式 (100kHz) / 快速模式 (400kHz) + * - 7 位设备地址 + */ + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* ============================================================================ + * 状态码和错误定义 + * ============================================================================ */ + +/** + * @brief Soft I2C 状态码 + */ +typedef enum { + SOFT_I2C_OK = 0, /**< 成功 */ + SOFT_I2C_ERR_TIMEOUT = -1, /**< 超时错误 */ + SOFT_I2C_ERR_NACK = -2, /**< NACK 错误（设备无应答） */ + SOFT_I2C_ERR_INVALID_PARAM = -3, /**< 无效参数 */ + SOFT_I2C_ERR_BUS_ERROR = -4, /**< 总线错误 */ + SOFT_I2C_ERR_ARBITRATION = -5 /**< 仲裁丢失 */ +} soft_i2c_status_t; + +/* ============================================================================ + * 配置结构体 + * ============================================================================ */ + +/** + * @brief Soft I2C 配置结构体 + */ +typedef struct { + int sda_gpio_ch; /**< SDA GPIO 通道号 */ + int scl_gpio_ch; /**< SCL GPIO 通道号 */ + uint32_t delay_us; /**< 半周期延时（微秒），决定总线速率 */ + /**< 例：5us = 100kHz, 2.5us = 200kHz, 1.25us = 400kHz */ + bool open_drain; /**< 是否开漏模式（默认 true）*/ +} soft_i2c_config_t; + +/** + * @brief Soft I2C 主机结构体 + */ +typedef struct { + soft_i2c_config_t config; /**< 配置参数 */ + bool bus_busy; /**< 总线忙标志 */ + uint8_t last_error; /**< 最后错误码 */ +} soft_i2c_t; + +/* ============================================================================ + * 核心 API 函数 + * ============================================================================ */ + +/** + * @brief 初始化 Soft I2C 主机 + * @param i2c: Soft I2C 结构体指针 + * @param config: 配置参数 + * @retval 成功返回 SOFT_I2C_OK，失败返回错误码 + */ +int soft_i2c_init(soft_i2c_t *i2c, const soft_i2c_config_t *config); + +/** + * @brief 产生 I2C 起始条件 + * @param i2c: Soft I2C 结构体指针 + * @retval 成功返回 SOFT_I2C_OK + */ +int soft_i2c_start(soft_i2c_t *i2c); + +/** + * @brief 产生 I2C 停止条件 + * @param i2c: Soft I2C 结构体指针 + * @retval 成功返回 SOFT_I2C_OK + */ +int soft_i2c_stop(soft_i2c_t *i2c); + +/** + * @brief 发送一个字节 + * @param i2c: Soft I2C 结构体指针 + * @param data: 要发送的数据 + * @retval 成功返回 SOFT_I2C_OK，NACK 返回 SOFT_I2C_ERR_NACK + */ +int soft_i2c_write_byte(soft_i2c_t *i2c, uint8_t data); + +/** + * @brief 读取一个字节 + * @param i2c: Soft I2C 结构体指针 + * @param ack: 是否发送 ACK (true=发送 ACK, false=发送 NACK) + * @param data: 读取到的数据（输出） + * @retval 成功返回 SOFT_I2C_OK + */ +int soft_i2c_read_byte(soft_i2c_t *i2c, bool ack, uint8_t *data); + +/** + * @brief 发送设备地址（含读写位） + * @param i2c: Soft I2C 结构体指针 + * @param addr: 7 位设备地址 + * @param write: 读写方向（true=写，false=读） + * @retval 成功返回 SOFT_I2C_OK，NACK 返回 SOFT_I2C_ERR_NACK + */ +int soft_i2c_send_addr(soft_i2c_t *i2c, uint8_t addr, bool write); + +/* ============================================================================ + * 高级 API 函数（类似硬件 I2C 的操作） + * ============================================================================ */ + +/** + * @brief I2C 内存写入（带寄存器地址） + * @param i2c: Soft I2C 结构体指针 + * @param dev_addr: 7 位设备地址 + * @param mem_addr: 寄存器/内存地址 + * @param mem_addr_size: 寄存器地址字节数（1 或 2） + * @param data: 要写入的数据 + * @param size: 数据长度 + * @param timeout: 超时时间（未使用，预留） + * @retval 成功返回 SOFT_I2C_OK + */ +int soft_i2c_mem_write(soft_i2c_t *i2c, uint16_t dev_addr, uint16_t mem_addr, + uint16_t mem_addr_size, const uint8_t *data, uint16_t size, + uint32_t timeout); + +/** + * @brief I2C 内存读取（带寄存器地址） + * @param i2c: Soft I2C 结构体指针 + * @param dev_addr: 7 位设备地址 + * @param mem_addr: 寄存器/内存地址 + * @param mem_addr_size: 寄存器地址字节数（1 或 2） + * @param data: 读取的数据缓冲区 + * @param size: 数据长度 + * @param timeout: 超时时间（未使用，预留） + * @retval 成功返回 SOFT_I2C_OK + */ +int soft_i2c_mem_read(soft_i2c_t *i2c, uint16_t dev_addr, uint16_t mem_addr, + uint16_t mem_addr_size, uint8_t *data, uint16_t size, + uint32_t timeout); + +/** + * @brief I2C 写入（不带寄存器地址） + * @param i2c: Soft I2C 结构体指针 + * @param dev_addr: 7 位设备地址 + * @param data: 要写入的数据 + * @param size: 数据长度 + * @param timeout: 超时时间（未使用，预留） + * @retval 成功返回 SOFT_I2C_OK + */ +int soft_i2c_write(soft_i2c_t *i2c, uint16_t dev_addr, const uint8_t *data, + uint16_t size, uint32_t timeout); + +/** + * @brief I2C 读取（不带寄存器地址） + * @param i2c: Soft I2C 结构体指针 + * @param dev_addr: 7 位设备地址 + * @param data: 读取的数据缓冲区 + * @param size: 数据长度 + * @param timeout: 超时时间（未使用，预留） + * @retval 成功返回 SOFT_I2C_OK + */ +int soft_i2c_read(soft_i2c_t *i2c, uint16_t dev_addr, uint8_t *data, + uint16_t size, uint32_t timeout); + +/* ============================================================================ + * 辅助函数 + * ============================================================================ */ + +/** + * @brief 产生 I2C 时钟脉冲 + * @param i2c: Soft I2C 结构体指针 + */ +void soft_i2c_generate_clock(soft_i2c_t *i2c); + +/** + * @brief 延时函数（微秒级） + * @param us: 延时微秒数 + * @note 可替换为实际的延时实现 + */ +void soft_i2c_delay_us(uint32_t us); + +#ifdef __cplusplus +} +#endif + +#endif /* __SOFT_I2C_H__ */ diff --git a/modules/bus/soft_i2c/soft_i2c_example.c b/modules/bus/soft_i2c/soft_i2c_example.c new file mode 100644 index 0000000..f28a268 --- /dev/null +++ b/modules/bus/soft_i2c/soft_i2c_example.c @@ -0,0 +1,106 @@ +/** + * @file soft_i2c_example.c + * @brief Soft I2C 使用示例 + * @note 演示如何使用 soft_i2c 模块 + */ + +#include "soft_i2c.h" +#include "soft_i2c_hal.h" +#include "i2c_if.h" +#include + +/* ============================================================================ + * 示例 1: 直接使用 soft_i2c API + * ============================================================================ */ + +void soft_i2c_example_direct(void) +{ + soft_i2c_t i2c; + soft_i2c_config_t config = { + .sda_gpio_ch = 0, /* 根据实际硬件修改 */ + .scl_gpio_ch = 1, + .delay_us = 5, /* 100kHz */ + .open_drain = true + }; + uint8_t data[2]; + + /* 初始化 */ + soft_i2c_init(&i2c, &config); + + /* 示例：读取 AT24C02 EEPROM (地址 0x50) 寄存器 0x00 的 2 个字节 */ + soft_i2c_mem_read(&i2c, 0x50, 0x00, 1, data, 2, 0); + + /* 示例：写入 2 个字节 */ + soft_i2c_mem_write(&i2c, 0x50, 0x00, 1, data, 2, 0); +} + +/* ============================================================================ + * 示例 2: 使用 HAL 适配器（通过 i2c_if 接口） + * ============================================================================ */ + +void soft_i2c_example_hal(void) +{ + uint8_t data[16]; + + /* 初始化 soft_i2c HAL，从通道 2 开始 */ + /* 调用后，通道 2 和 3 对应 soft_i2c 总线 */ + soft_i2c_hal_init(2); + + /* 现在可以使用标准 i2c_if 接口 */ + /* 读取第一个 soft_i2c 总线上的设备 */ + i2c_if_mem_read(2, 0x50, 0x00, 1, data, 16, 100); + + /* 读取第二个 soft_i2c 总线上的设备 */ + i2c_if_mem_read(3, 0x68, 0x00, 1, data, 16, 100); +} + +/* ============================================================================ + * 示例 3: MPU6050 读取示例 + * ============================================================================ */ + +void soft_i2c_example_mpu6050(soft_i2c_t *i2c) +{ + uint8_t who_am_i; + uint8_t data[14]; + + /* 读取 WHO_AM_I 寄存器 (0x75) */ + soft_i2c_mem_read(i2c, 0x68, 0x75, 1, &who_am_i, 1, 0); + + /* 读取加速度计和陀螺仪数据 (0x3B-0x48) */ + soft_i2c_mem_read(i2c, 0x68, 0x3B, 1, data, 14, 0); + + /* ACCEL_XOUT_H */ + int16_t accel_x = (int16_t)((data[0] << 8) | data[1]); + (void)accel_x; +} + +/* ============================================================================ + * 示例 4: OLED 显示写入示例 (SSD1306) + * ============================================================================ */ + +void soft_i2c_example_oled_command(soft_i2c_t *i2c, uint8_t cmd) +{ + uint8_t buf[2]; + + /* SSD1306 命令模式：第一字节 0x00 = 命令 */ + buf[0] = 0x00; + buf[1] = cmd; + + soft_i2c_write(i2c, 0x3C, buf, 2, 0); +} + +void soft_i2c_example_oled_data(soft_i2c_t *i2c, const uint8_t *data, uint16_t size) +{ + uint8_t static_buf[129]; /* 静态缓冲区，最大 128 字节数据 + 1 字节命令 */ + uint16_t i; + + if (size > 128) return; /* 限制最大长度 */ + + /* SSD1306 数据模式：第一字节 0x40 = 数据 */ + static_buf[0] = 0x40; + for (i = 0; i < size; i++) { + static_buf[i + 1] = data[i]; + } + + soft_i2c_write(i2c, 0x3C, static_buf, size + 1, 0); +} diff --git a/modules/bus/soft_i2c/soft_i2c_hal.c b/modules/bus/soft_i2c/soft_i2c_hal.c new file mode 100644 index 0000000..e5464bb --- /dev/null +++ b/modules/bus/soft_i2c/soft_i2c_hal.c @@ -0,0 +1,172 @@ +/** + * @file soft_i2c_hal.c + * @brief Soft I2C HAL 适配器实现 + * @note 将 soft_i2c 模块适配到现有的 i2c_ops_t 接口 + * + * 使用示例： + * @code + * // 初始化 2 个 soft_i2c 总线（通道 2 和 3） + * soft_i2c_hal_init(2); + * + * // 通过标准接口访问 + * i2c_if_read(2, 0x50, buffer, 16, 100); // 读取第一个 soft_i2c 总线 + * @endcode + */ + +#include "soft_i2c_hal.h" +#include "soft_i2c.h" +#include "i2c_if.h" +#include "gpio_if.h" +#include + +/* ============================================================================ + * 配置区域 + * ============================================================================ */ + +/** + * @brief 最大支持的 soft_i2c 总线数量 + */ +#ifndef SOFT_I2C_MAX_CHANNELS +#define SOFT_I2C_MAX_CHANNELS 2 +#endif + +/** + * @brief Soft I2C 总线配置表 + * @note 根据实际硬件连接修改引脚配置 + */ +static const soft_i2c_config_t soft_i2c_configs[SOFT_I2C_MAX_CHANNELS] = { + { + .sda_gpio_ch = 0, /* SDA1 - 修改为实际 GPIO 通道 */ + .scl_gpio_ch = 1, /* SCL1 */ + .delay_us = 5, /* 5us = 约 100kHz */ + .open_drain = true + }, + { + .sda_gpio_ch = 2, /* SDA2 - 修改为实际 GPIO 通道 */ + .scl_gpio_ch = 3, /* SCL2 */ + .delay_us = 2, /* 2us = 约 250kHz */ + .open_drain = true + } +}; + +/** + * @brief Soft I2C 主机实例 + */ +static soft_i2c_t soft_i2c_hosts[SOFT_I2C_MAX_CHANNELS]; + +/** + * @brief 通道偏移量（soft_i2c 在 i2c_if 中的起始通道号） + */ +static int g_channel_offset = -1; + +/* ============================================================================ + * i2c_ops_t 接口实现 + * ============================================================================ */ + +static int soft_i2c_hal_init_impl(int ch) +{ + int local_ch = ch - g_channel_offset; + + if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) { + return -1; + } + + return soft_i2c_init(&soft_i2c_hosts[local_ch], &soft_i2c_configs[local_ch]); +} + +static int soft_i2c_hal_mem_write(int ch, uint16_t dev_addr, uint16_t mem_addr, + uint16_t mem_addr_size, const uint8_t *data, + uint16_t size, uint32_t timeout) +{ + int local_ch = ch - g_channel_offset; + + if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) { + return -1; + } + + return soft_i2c_mem_write(&soft_i2c_hosts[local_ch], dev_addr, mem_addr, + mem_addr_size, data, size, timeout); +} + +static int soft_i2c_hal_mem_read(int ch, uint16_t dev_addr, uint16_t mem_addr, + uint16_t mem_addr_size, uint8_t *data, + uint16_t size, uint32_t timeout) +{ + int local_ch = ch - g_channel_offset; + + if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) { + return -1; + } + + return soft_i2c_mem_read(&soft_i2c_hosts[local_ch], dev_addr, mem_addr, + mem_addr_size, data, size, timeout); +} + +static int soft_i2c_hal_write(int ch, uint16_t dev_addr, const uint8_t *data, + uint16_t size, uint32_t timeout) +{ + int local_ch = ch - g_channel_offset; + + if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) { + return -1; + } + + return soft_i2c_write(&soft_i2c_hosts[local_ch], dev_addr, data, size, timeout); +} + +static int soft_i2c_hal_read(int ch, uint16_t dev_addr, uint8_t *data, + uint16_t size, uint32_t timeout) +{ + int local_ch = ch - g_channel_offset; + + if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) { + return -1; + } + + return soft_i2c_read(&soft_i2c_hosts[local_ch], dev_addr, data, size, timeout); +} + +/** + * @brief soft_i2c 的 i2c_ops_t 实现 + */ +static const i2c_ops_t soft_i2c_ops = { + .init = soft_i2c_hal_init_impl, + .mem_write = soft_i2c_hal_mem_write, + .mem_read = soft_i2c_hal_mem_read, + .write = soft_i2c_hal_write, + .read = soft_i2c_hal_read +}; + +/* ============================================================================ + * 公共 API 实现 + * ============================================================================ */ + +int soft_i2c_hal_init(int channel_offset) +{ + int i; + + if (g_channel_offset >= 0) { + /* 已初始化 */ + return -1; + } + + g_channel_offset = channel_offset; + + /* 初始化所有 soft_i2c 通道 */ + for (i = 0; i < SOFT_I2C_MAX_CHANNELS; i++) { + int ret = soft_i2c_init(&soft_i2c_hosts[i], &soft_i2c_configs[i]); + if (ret != SOFT_I2C_OK) { + /* 初始化失败，但仍继续 */ + } + } + + /* 注册到 i2c_if */ + i2c_register_ops(&soft_i2c_ops); + + return 0; +} + +int soft_i2c_hal_get_channel_count(void) +{ + return SOFT_I2C_MAX_CHANNELS; +} diff --git a/modules/bus/soft_i2c/soft_i2c_hal.h b/modules/bus/soft_i2c/soft_i2c_hal.h new file mode 100644 index 0000000..39ea5cf --- /dev/null +++ b/modules/bus/soft_i2c/soft_i2c_hal.h @@ -0,0 +1,34 @@ +/** + * @file soft_i2c_hal.h + * @brief Soft I2C HAL 适配器头文件 + * @note 将 soft_i2c 适配到现有的 i2c_ops_t 接口 + */ + +#ifndef __SOFT_I2C_HAL_H__ +#define __SOFT_I2C_HAL_H__ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief 初始化 soft_i2c HAL 层并注册到 i2c_if + * @param channel_offset: 通道偏移量（soft_i2c 从该通道号开始） + * @retval 成功返回 0，失败返回 -1 + * @note 调用后，soft_i2c 可通过 i2c_if_* 接口访问 + * 例如：i2c_if_read(channel_offset, dev_addr, data, size, timeout) + */ +int soft_i2c_hal_init(int channel_offset); + +/** + * @brief 获取 soft_i2c 通道数量 + */ +int soft_i2c_hal_get_channel_count(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __SOFT_I2C_HAL_H__ */ diff --git a/modules/device/motor/mf4010v2.c b/modules/device/motor/mf4010v2.c index 9f3dd99..61e4d7b 100644 --- a/modules/device/motor/mf4010v2.c +++ b/modules/device/motor/mf4010v2.c @@ -33,6 +33,7 @@ int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch) motor->target_angle = 0; motor->current_angle = 0; memset(motor->last_response, 0, sizeof(motor->last_response)); + can_if_init(motor->can_ch); motor->last_comm_time = 0; motor->error_count = 0; motor->success_count = 0; @@ -55,6 +56,12 @@ int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8]) printf("Error: Failed to send CAN message (ID 0x%03X)\r\n", motor->id); return MF4010_ERR_CAN_SEND; } + can_if_recv(motor->can_ch, &msg); + if (msg.id != motor->id) { + motor->error_count++; + return MF4010_ERR_CAN_RECV; + } + memcpy(motor->last_response, msg.data, 8); motor->success_count++; return MF4010_OK; } @@ -80,7 +87,7 @@ int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg) cmd_position_add(cmd, delta); motor->target_angle = angle_0_01deg; - return mf4010v2_send_command(motor, motor->can_ch, cmd); + return mf4010v2_send_command(motor, cmd); } /** @@ -94,7 +101,7 @@ int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps) uint8_t cmd[8]; cmd_speed_control(cmd, speed_0_01dps); - return mf4010v2_send_command(motor, motor->can_ch, cmd); + return mf4010v2_send_command(motor, cmd); } /** @@ -104,7 +111,7 @@ void mf4010v2_run(mf4010v2_t* motor) { if (!motor) return; uint8_t cmd[8] = COMMAND_MOTOR_RUNNING; - mf4010v2_send_command(motor, motor->can_ch, cmd); + mf4010v2_send_command(motor, cmd); motor->flags |= MF4010_FLAG_RUNNING; } @@ -115,17 +122,177 @@ void mf4010v2_stop(mf4010v2_t* motor) { if (!motor) return; uint8_t cmd[8] = COMMAND_MOTOR_STOP; - mf4010v2_send_command(motor, motor->can_ch, cmd); + mf4010v2_send_command(motor, cmd); motor->flags &= ~MF4010_FLAG_RUNNING; } -/** - * @brief 获取当前速度 (返回目标速度，因为实际速度需要从编码器反馈) - */ -int32_t mf4010v2_get_speed(mf4010v2_t* motor) -{ - if (!motor) return 0; - uint8_t cmd[8] = - mf4010v2_send_command(motor, motor->can_ch, cmd); - return motor->target_angle; // 占位返回 +void mf4010v2_close(mf4010v2_t* motor) { + if (!motor) return; + uint8_t cmd[8] = COMMAND_MOTOR_CLOSE; + mf4010v2_send_command(motor, cmd); + motor->flags &= ~MF4010_FLAG_ENABLED; } + +int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid) { + if (!motor) return; + uint8_t cmd[8] = {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + mf4010v2_send_command(motor, cmd); + pid->angle_kp = motor->last_response[2]; + pid->angle_ki = motor->last_response[3]; + pid->speed_kp = motor->last_response[4]; + pid->speed_ki = motor->last_response[5]; + pid->iq_kp = motor->last_response[6]; + pid->iq_ki = motor->last_response[7]; + return 0; +} +/* ============================================================================ + * 命令生成函数实现 + * ============================================================================ */ + +void cmd_torque_control(uint8_t* buf, int16_t iq) +{ + buf[0] = 0xA1; + buf[1] = 0x00; + buf[2] = 0x00; + buf[3] = 0x00; + buf[4] = (uint8_t)(iq & 0xFF); + buf[5] = (uint8_t)((iq >> 8) & 0xFF); + buf[6] = 0x00; + buf[7] = 0x00; +} + + + +void cmd_speed_control(uint8_t* buf, int32_t speed) +{ + buf[0] = 0xA2; + buf[1] = 0x00; + buf[2] = 0x00; + buf[3] = 0x00; + buf[4] = (uint8_t)(speed & 0xFF); + buf[5] = (uint8_t)((speed >> 8) & 0xFF); + buf[6] = (uint8_t)((speed >> 16) & 0xFF); + buf[7] = (uint8_t)((speed >> 24) & 0xFF); +} + + + +void cmd_mult_ring_pos(uint8_t* buf, int32_t angleControl) { + buf[0] = 0xA3; + buf[1] = 0x00; + buf[2] = 0x00; + buf[3] = 0x00; + buf[4] = *(uint8_t *)(&angleControl); + buf[5] = *((uint8_t *)(&angleControl)+1); + buf[6] = *((uint8_t *)(&angleControl)+2); + buf[7] = *((uint8_t *)(&angleControl)+3); +} + +void cmd_mult_ring_pos_with_speed(uint8_t* buf, uint16_t maxSpeed, int32_t angleControl) { + buf[0] = 0xA4; + buf[1] = 0x00; + buf[2] = *(uint8_t *)(&maxSpeed); + buf[3] = *((uint8_t *)(&maxSpeed)+1); + buf[4] = *(uint8_t *)(&angleControl); + buf[5] = *((uint8_t *)(&angleControl)+1); + buf[6] = *((uint8_t *)(&angleControl)+2); + buf[7] = *((uint8_t *)(&angleControl)+3); +} + +void cmd_single_ring_pos(uint8_t* buf, uint8_t direction, uint32_t angle) +{ + buf[0] = 0xA5; + buf[1] = direction & 0x01; + buf[2] = 0x00; + buf[3] = 0x00; + buf[4] = (uint8_t)(angle & 0xFF); + buf[5] = (uint8_t)((angle >> 8) & 0xFF); + buf[6] = (uint8_t)((angle >> 16) & 0xFF); + buf[7] = (uint8_t)((angle >> 24) & 0xFF); +} + +void cmd_single_ring_pos_with_speed(uint8_t* buf, uint8_t spinDirection, uint16_t maxSpeed, uint32_t angleControl) +{ + buf[0] = 0xA6; + buf[1] = spinDirection & 0x01; + buf[2] = (uint8_t)(maxSpeed & 0xFF); + buf[3] = (uint8_t)((maxSpeed >> 8) & 0xFF); + buf[4] = (uint8_t)(angleControl & 0xFF); + buf[5] = (uint8_t)((angleControl >> 8) & 0xFF); + buf[6] = (uint8_t)((angleControl >> 16) & 0xFF); + buf[7] = (uint8_t)((angleControl >> 24) & 0xFF); +} + +void cmd_incr_pos(uint8_t* buf, int32_t angle_increment) +{ + buf[0] = 0xA7; + buf[1] = 0x00; + buf[2] = 0x00; + buf[3] = 0x00; + buf[4] = (uint8_t)(angle_increment & 0xFF); + buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF); + buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF); + buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF); +} + +void cmd_incr_pos_with_speed(uint8_t* buf, uint16_t max_speed, int32_t angle_increment) { + buf[0] = 0xA8; + buf[1] = 0x00; + buf[2] = (uint8_t)(maxSpeed & 0xFF); + buf[3] = (uint8_t)((maxSpeed >> 8) & 0xFF); + buf[4] = (uint8_t)(angle_increment & 0xFF); + buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF); + buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF); + buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF); +} + +void cmd_write_pid_params(uint8_t* buf, uint8_t location, + uint8_t angle_kp, uint8_t angle_ki, + uint8_t speed_kp, uint8_t speed_ki, + uint8_t iq_kp, uint8_t iq_ki) +{ + buf[0] = location; + buf[1] = 0x00; + buf[2] = angle_kp; + buf[3] = angle_ki; + buf[4] = speed_kp; + buf[5] = speed_ki; + buf[6] = iq_kp; + buf[7] = iq_ki; +} + +/* ============================================================================ + * 辅助函数实现 + * ============================================================================ */ + +bool mf4010v2_is_connected(mf4010v2_t* motor) +{ + return (motor != NULL) && (motor->flags & MF4010_FLAG_CONNECTED); +} + +bool mf4010v2_is_running(mf4010v2_t* motor) +{ + return (motor != NULL) && (motor->flags & MF4010_FLAG_RUNNING); +} + +uint16_t mf4010v2_get_error_count(mf4010v2_t* motor) +{ + return (motor != NULL) ? motor->error_count : 0; +} + +void mf4010v2_reset_errors(mf4010v2_t* motor) +{ + if(motor != NULL) { + motor->error_count = 0; + } +} + +bool mf4010v2_response_has_error(mf4010v2_t* motor) +{ + return (motor != NULL) && ((motor->last_response[0] & 0x80) != 0); +} + +uint8_t mf4010v2_get_error_code(mf4010v2_t* motor) +{ + return (motor != NULL) ? (motor->last_response[0] & 0x7F) : 0; +} \ No newline at end of file diff --git a/modules/device/motor/mf4010v2.h b/modules/device/motor/mf4010v2.h index fc53117..a775bca 100644 --- a/modules/device/motor/mf4010v2.h +++ b/modules/device/motor/mf4010v2.h @@ -40,7 +40,8 @@ typedef enum { MF4010_ERR_BUS_OFF = -5, /**< CAN 总线关闭 */ MF4010_ERR_CRC = -6, /**< CRC 校验错误 */ MF4010_ERR_HARDWARE = -7, /**< 硬件错误 */ - MF4010_ERR_CAN_SEND = -8 /**< CAN 发送失败 */ + MF4010_ERR_CAN_SEND = -8, /**< CAN 发送失败 */ + MF4010_ERR_CAN_RECV = -9 /**< CAN 接收失败 */ } mf4010_status_t; /** @@ -63,7 +64,7 @@ typedef enum { */ -struct __mf4010v2_t { +typedef struct __mf4010v2_t { int can_ch; /**< CAN 通道 (0 或 1) */ uint16_t id; /**< CAN ID (0x000-0x7FF) */ uint8_t last_response[8]; /**< 最后一次响应数据 */ @@ -73,10 +74,17 @@ struct __mf4010v2_t { uint8_t flags; /**< 状态标志 (@ref mf4010_flag_t) */ int32_t target_angle; /**< 目标角度 (0.01 度) */ int32_t current_angle; /**< 当前角度 (0.01 度) - 从编码器读取 */ -}; - +} mf4010v2_t; +typedef struct { + uint8_t angle_kp; + uint8_t angle_ki; + uint8_t speed_kp; + uint8_t speed_ki; + uint8_t iq_kp; + uint8_t iq_ki; +} pid_param; /* ============================================================================ @@ -95,16 +103,18 @@ int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch); /** * @brief 发送命令并接收响应（带超时） * @param motor: 电机结构体指针 - * @param can_ch: CAN 通道 * @param command: 8 字节命令数据 * @retval 成功返回 MF4010_OK，失败返回错误码 */ -int mf4010v2_send_command(mf4010v2_t* motor, int can_ch, const uint8_t command[8]); +int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8]); void mf4010v2_run(mf4010v2_t* motor); void mf4010v2_stop(mf4010v2_t* motor); +void mf4010v2_close(mf4010v2_t* motor); int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps); +int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg); +int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid); int32_t mf4010v2_get_speed(mf4010v2_t* motor); /* ============================================================================ @@ -145,17 +155,7 @@ int32_t mf4010v2_get_speed(mf4010v2_t* motor); * @param iq: 转矩控制量 (-2048 ~ 2048) * @note 分辨率：1 LSB = 1 单位 */ -static inline void cmd_torque_control(uint8_t* buf, int16_t iq) -{ - buf[0] = 0xA1; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = 0x00; - buf[4] = (uint8_t)(iq & 0xFF); - buf[5] = (uint8_t)((iq >> 8) & 0xFF); - buf[6] = 0x00; - buf[7] = 0x00; -} +void cmd_torque_control(uint8_t* buf, int16_t iq); /** * @brief 速度闭环控制命令 @@ -167,17 +167,26 @@ static inline void cmd_torque_control(uint8_t* buf, int16_t iq) * uint8_t cmd[8]; * cmd_speed_control(cmd, 10000); // 100 DPS */ -static inline void cmd_speed_control(uint8_t* buf, int32_t speed) -{ - buf[0] = 0xA2; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = 0x00; - buf[4] = (uint8_t)(speed & 0xFF); - buf[5] = (uint8_t)((speed >> 8) & 0xFF); - buf[6] = (uint8_t)((speed >> 16) & 0xFF); - buf[7] = (uint8_t)((speed >> 24) & 0xFF); -} +void cmd_speed_control(uint8_t* buf, int32_t speed); + + + +/** + * @brief 多圈位置闭环控制命令 + * @param buf + * @param angleControl 0.01degree/LSB 36000表示360° + */ + +void cmd_mult_ring_pos(uint8_t* buf, int32_t angleControl); + +/** + * @brief 多圈位置闭环控制命令带速度限制 + * @param buf + * @param maxSpeed 表示实际转速1dp/LSB 360表示360 dps + * @param angleControl 0.01degree/LSB 36000表示360° + */ + +void cmd_mult_ring_pos_with_speed(uint8_t* buf, uint16_t maxSpeed, int32_t angleControl); /** * @brief 单圈位置闭环控制命令 @@ -185,37 +194,16 @@ static inline void cmd_speed_control(uint8_t* buf, int32_t speed) * @param direction: 旋转方向 (0x00=顺时针，0x01=逆时针) * @param angle: 目标角度 (单位：0.01°, 如 36000=360°) */ -static inline void cmd_single_ring_pos(uint8_t* buf, uint8_t direction, uint32_t angle) -{ - buf[0] = 0xA5; - buf[1] = direction & 0x01; - buf[2] = 0x00; - buf[3] = 0x00; - buf[4] = (uint8_t)(angle & 0xFF); - buf[5] = (uint8_t)((angle >> 8) & 0xFF); - buf[6] = (uint8_t)((angle >> 16) & 0xFF); - buf[7] = (uint8_t)((angle >> 24) & 0xFF); -} - +void cmd_single_ring_pos(uint8_t* buf, uint8_t direction, uint32_t angle); /** - * @brief 增量位置闭环控制命令（带最大速度） + * @brief 单圈位置闭环控制命令（带最大速度） * @param buf: 8 字节缓冲区（输出） - * @param direction: 旋转方向 (0x00=顺时针，0x01=逆时针) - * @param max_speed: 最大速度 (单位：DPS, 如 360=360 DPS) - * @param angle: 目标角度 (单位：0.01°, 如 36000=360°) + * @param spinDirection: 旋转方向 (0x00=顺时针，0x01=逆时针) + * @param maxSpeed: 最大速度 (单位：DPS, 如 360=360 DPS) + * @param angleControl: 目标角度 (单位：0.01°, 如 36000=360°) */ -static inline void cmd_incr_pos_with_speed(uint8_t* buf, uint8_t direction, - uint16_t max_speed, uint32_t angle) -{ - buf[0] = 0xA6; - buf[1] = direction & 0x01; - buf[2] = (uint8_t)(max_speed & 0xFF); - buf[3] = (uint8_t)((max_speed >> 8) & 0xFF); - buf[4] = (uint8_t)(angle & 0xFF); - buf[5] = (uint8_t)((angle >> 8) & 0xFF); - buf[6] = (uint8_t)((angle >> 16) & 0xFF); - buf[7] = (uint8_t)((angle >> 24) & 0xFF); -} +void cmd_single_ring_pos_with_speed(uint8_t* buf, uint8_t spinDirection, + uint16_t maxSpeed, uint32_t angleControl); /** * @brief 增量位置闭环控制命令 @@ -223,17 +211,18 @@ static inline void cmd_incr_pos_with_speed(uint8_t* buf, uint8_t direction, * @param angle_increment: 角度增量 (单位：0.01°, 如 36000=360°) * @note 正值 = 逆时针，负值 = 顺时针 */ -static inline void cmd_position_add(uint8_t* buf, int32_t angle_increment) -{ - buf[0] = 0xA7; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = 0x00; - buf[4] = (uint8_t)(angle_increment & 0xFF); - buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF); - buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF); - buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF); -} +void cmd_incr_pos(uint8_t* buf, int32_t angle_increment); + +/** + * @brief 增量位置闭环控制命令（带最大速度） + * @param buf: 8 字节缓冲区（输出） + * @param max_speed 1dps/LSB + * @param angle_increment: 角度增量 (单位：0.01°, 如 36000=360°) + * @note 正值 = 逆时针，负值 = 顺时针 + */ +void cmd_incr_pos_with_speed(uint8_t* buf, uint16_t max_speed, int32_t angle_increment); + + /** * @brief PID 参数写入命令 @@ -246,20 +235,10 @@ static inline void cmd_position_add(uint8_t* buf, int32_t angle_increment) * @param iq_kp: 电流环 Kp (0-255) * @param iq_ki: 电流环 Ki (0-255) */ -static inline void cmd_write_pid_params(uint8_t* buf, uint8_t location, - uint8_t angle_kp, uint8_t angle_ki, - uint8_t speed_kp, uint8_t speed_ki, - uint8_t iq_kp, uint8_t iq_ki) -{ - buf[0] = location; - buf[1] = 0x00; - buf[2] = angle_kp; - buf[3] = angle_ki; - buf[4] = speed_kp; - buf[5] = speed_ki; - buf[6] = iq_kp; - buf[7] = iq_ki; -} +void cmd_write_pid_params(uint8_t* buf, uint8_t location, + uint8_t angle_kp, uint8_t angle_ki, + uint8_t speed_kp, uint8_t speed_ki, + uint8_t iq_kp, uint8_t iq_ki); /* ============================================================================ * 辅助函数和宏 @@ -268,36 +247,22 @@ static inline void cmd_write_pid_params(uint8_t* buf, uint8_t location, /** * @brief 检查电机是否在线 */ -static inline bool mf4010v2_is_connected(mf4010v2_t* motor) -{ - return (motor != NULL) && (motor->flags & MF4010_FLAG_CONNECTED); -} +bool mf4010v2_is_connected(mf4010v2_t* motor); /** * @brief 检查电机是否运行 */ -static inline bool mf4010v2_is_running(mf4010v2_t* motor) -{ - return (motor != NULL) && (motor->flags & MF4010_FLAG_RUNNING); -} +bool mf4010v2_is_running(mf4010v2_t* motor); /** * @brief 获取电机错误计数 */ -static inline uint16_t mf4010v2_get_error_count(mf4010v2_t* motor) -{ - return (motor != NULL) ? motor->error_count : 0; -} +uint16_t mf4010v2_get_error_count(mf4010v2_t* motor); /** * @brief 重置错误计数 */ -static inline void mf4010v2_reset_errors(mf4010v2_t* motor) -{ - if(motor != NULL) { - motor->error_count = 0; - } -} +void mf4010v2_reset_errors(mf4010v2_t* motor); /** * @brief 获取响应中的命令字节 @@ -313,18 +278,12 @@ static inline void mf4010v2_reset_errors(mf4010v2_t* motor) * @brief 检查响应是否包含错误 * @note 如果响应字节 0 的最高位为 1，表示有错误 */ -static inline bool mf4010v2_response_has_error(mf4010v2_t* motor) -{ - return (motor != NULL) && ((motor->last_response[0] & 0x80) != 0); -} +bool mf4010v2_response_has_error(mf4010v2_t* motor); /** * @brief 获取错误码（从响应中） */ -static inline uint8_t mf4010v2_get_error_code(mf4010v2_t* motor) -{ - return (motor != NULL) ? (motor->last_response[0] & 0x7F) : 0; -} +uint8_t mf4010v2_get_error_code(mf4010v2_t* motor); #ifdef __cplusplus }