# RobotHandExt

**Repository Path**: sxupgljd/robot-hand-ext

## Basic Information

- **Project Name**: RobotHandExt
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: dev
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-06-07
- **Last Updated**: 2026-06-07

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# RobotHandExt - 机械手扩展模块固件

## 项目概述

RobotHandExt 是一个机械手末端执行器扩展模块的嵌入式固件项目，基于 STM32F103CBT6 微控制器开发。该模块负责控制机械手的开合角度（舵机驱动），同时集成 IMU 姿态传感器和 RGB 颜色传感器，通过 CAN 总线与主控制器进行实时通信，实现对机械手末端执行器的精确控制和状态反馈。

### 主要功能

- **舵机控制**: 通过 PWM 信号驱动舵机，实现 0°~180° 角度范围内的精确控制
- **姿态检测**: 集成 MPU9250 九轴 IMU，实时采集三轴加速度和磁力计数据，计算姿态角
- **颜色识别**: 集成 TCS34725 RGB 颜色传感器，支持 RGBC 和 HSL 颜色空间转换
- **CAN 通信**: 基于 CAN 2.0B 扩展帧协议，支持多节点寻址和命令响应
- **调试输出**: 通过 UART 串口输出调试信息，波特率 115200

---

## 硬件平台

### MCU 规格

| 项目 | 参数 |
|------|------|
| **型号** | STM32F103CBT6 |
| **核心** | ARM Cortex-M3 |
| **主频** | 72 MHz (HSE 12MHz + PLL x6) |
| **Flash** | 128 KB |
| **SRAM** | 20 KB |
| **封装** | LQFP48 |
| **工作电压** | 2.0V ~ 3.6V |
| **工作温度** | -40°C ~ 85°C |

### 外设资源

| 外设 | 数量 | 用途 |
|------|------|------|
| CAN | 1 | 与主控制器通信 |
| UART | 1 | 调试串口输出 |
| Timer (PWM) | 1 | 舵机控制信号 |
| GPIO | 多个 | 传感器接口、调试接口 |

### 外部传感器

| 传感器 | 型号 | 接口 | 地址 |
|--------|------|------|------|
| IMU 九轴 | MPU9250 (MPU6500 + AK8963) | 软件 I2C | 0x68 (MPU6500), 0x0C (AK8963) |
| RGB 颜色 | TCS34725 | 软件 I2C | 0x29 |

---

## 技术栈

| 层级 | 技术 | 版本/规格 |
|------|------|-----------|
| **开发语言** | C | C99 |
| **HAL 库** | STM32 HAL | STM32Cube FW_F1 V1.8.4 |
| **CMSIS** | ARM CMSIS | Cortex-M3 支持 |
| **IDE** | Keil MDK-ARM | V5 |
| **配置工具** | STM32CubeMX | 6.6.1 |
| **调试器** | SWD | PA13 (SWDIO), PA14 (SWCLK) |
| **调试软件** | ST-Link / J-Link | 支持 |

---

## 模块架构

### 系统架构图

```
┌─────────────────────────────────────────────────────────────────────┐
│                         主控制器 (上位机)                            │
│                                                                     │
│  ┌─────────────┐    ┌─────────────┐    ┌─────────────┐             │
│  │  运动规划   │    │  状态监控   │    │  任务调度   │             │
│  └──────┬──────┘    └──────┬──────┘    └──────┬──────┘             │
└─────────┼──────────────────┼──────────────────┼─────────────────────┘
          │                  │                  │
          │    CAN Bus (500kbps)                │
          │                  │                  │
┌─────────┼──────────────────┼──────────────────┼─────────────────────┐
│         ▼                  ▼                  ▼                     │
│  ┌─────────────────────────────────────────────────────────────┐   │
│  │                  RobotHandExt 扩展模块                       │   │
│  │                                                             │   │
│  │  ┌──────────────┐    ┌──────────────────────────────────┐  │   │
│  │  │  CAN 收发    │    │         main.c 主程序              │  │   │
│  │  │  (中断驱动)  │◄───│                                  │  │   │
│  │  └──────────────┘    │  ┌────────────────────────────┐  │  │   │
│  │                      │  │      协议解析层              │  │  │   │
│  │  ┌──────────────┐    │  │  ┌──────┐ ┌──────┐ ┌─────┐ │  │  │   │
│  │  │  PWM 输出    │    │  │  │0x02  │ │0x23  │ │0x32 │ │  │  │   │
│  │  │  (TIM2 CH1)  │◄───│  │  │Set   │ │Get   │ │Get  │ │  │  │   │
│  │  │  50Hz PWM    │    │  │  │Pos   │ │Pos   │ │Ver  │ │  │  │   │
│  │  └──────────────┘    │  │  └──────┘ └──────┘ └─────┘ │  │  │   │
│  │                      │  │  ┌──────┐ ┌──────┐         │  │  │   │
│  │  ┌──────────────┐    │  │  │0x42  │ │0x7F  │         │  │  │   │
│  │  │  MPU9250     │◄───│  │  │Get   │ │Reboot│         │  │  │   │
│  │  │  (IMU 9轴)   │    │  │  │IMU   │ │      │         │  │  │   │
│  │  └──────────────┘    │  │  └──────┘ └──────┘         │  │  │   │
│  │                      │  └────────────────────────────┘  │  │   │
│  │  ┌──────────────┐    │                                  │  │   │
│  │  │  TCS34725    │    │  ┌────────────────────────────┐  │  │   │
│  │  │  (RGB 颜色)  │◄───│  │      传感器采集层          │  │  │   │
│  │  └──────────────┘    │  │  ┌──────────┐ ┌──────────┐ │  │  │   │
│  │                      │  │  │ MPU9250  │ │ TCS34725 │ │  │  │   │
│  │  ┌──────────────┐    │  │  │ 姿态滤波 │ │ 颜色转换 │ │  │  │   │
│  │  │  UART 调试   │    │  │  └──────────┘ └──────────┘ │  │  │   │
│  │  │  (115200)    │    │  └────────────────────────────┘  │  │   │
│  │  └──────────────┘    └──────────────────────────────────┘  │   │
│  └─────────────────────────────────────────────────────────────┘   │
└─────────────────────────────────────────────────────────────────────┘
```

### 软件分层

```
┌─────────────────────────────────────────────────┐
│  应用层 (Application Layer)                      │
│  - main.c: 主循环、初始化、CAN 协议解析           │
├─────────────────────────────────────────────────┤
│  驱动层 (Driver Layer)                           │
│  - mpu.c: MPU9250 IMU 驱动                       │
│  - tcs34725.c: TCS34725 颜色传感器驱动            │
│  - bsp_i2c.c: 软件 I2C 通信                      │
├─────────────────────────────────────────────────┤
│  HAL 层 (Hardware Abstraction Layer)             │
│  - STM32 HAL: 外设抽象                           │
│  - CMSIS: ARM 核心支持                           │
├─────────────────────────────────────────────────┤
│  硬件层 (Hardware Layer)                         │
│  - STM32F103CBT6 MCU                             │
│  - MPU9250, TCS34725, 舵机                       │
└─────────────────────────────────────────────────┘
```

---

## 引脚分配

### 详细引脚映射

| 引脚 | GPIO | 功能 | 方向 | 模式 | 说明 |
|------|------|------|------|------|------|
| **PA11** | GPIOA | CAN_RX | 输入 | CAN_Activate | CAN 总线接收 |
| **PA12** | GPIOA | CAN_TX | 输出 | CAN_Activate | CAN 总线发送 |
| **PA11** | GPIOA | I2C_SDA | 双向 | Open-Drain | MPU9250 数据线 (软件 I2C) |
| **PA12** | GPIOA | I2C_SCL | 输出 | Open-Drain | MPU9250 时钟线 (软件 I2C) |
| **PA15** | GPIOA | TIM2_CH1 | 输出 | PWM | 舵机控制 PWM 信号 |
| **PB3** | GPIOB | TCS_SCL | 输出 | Push-Pull | TCS34725 时钟线 (软件 I2C) |
| **PB5** | GPIOB | TCS_SDA | 双向 | Open-Drain | TCS34725 数据线 (软件 I2C) |
| **PB6** | GPIOB | USART1_TX | 输出 | AF_PP | 调试串口发送 |
| **PB7** | GPIOB | USART1_RX | 输入 | Input | 调试串口接收 |
| **PD0** | GPIOD | OSC_IN | 输入 | HSE | 外部 12MHz 晶振输入 |
| **PD1** | GPIOD | OSC_OUT | 输出 | HSE | 外部 12MHz 晶振输出 |
| **PA13** | GPIOA | SWDIO | 双向 | SWD | SWD 调试数据线 |
| **PA14** | GPIOA | SWCLK | 输入 | SWD | SWD 调试时钟线 |

### GPIO 配置详情

```c
// GPIO 端口时钟使能
__HAL_RCC_GPIOD_CLK_ENABLE();  // 外部晶振
__HAL_RCC_GPIOA_CLK_ENABLE();  // CAN, UART, PWM, SWD, MPU9250 I2C
__HAL_RCC_GPIOB_CLK_ENABLE();  // TCS34725 I2C, UART

// MPU9250 软件 I2C (PA12 SCL, PA11 SDA) - Open-Drain 模式
GPIO_InitStruct.Pin = GPIO_PIN_12 | GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

// TCS34725 软件 I2C (PB3 SCL, PB5 SDA) - Open-Drain 模式
GPIO_InitStruct.Pin = GPIO_PIN_3 | GPIO_PIN_5 | GPIO_PIN_4;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
```

---

## 外设配置

### CAN 总线配置

| 参数 | 值 | 说明 |
|------|-----|------|
| **实例** | CAN1 | STM32F103 内置 CAN 控制器 |
| **模式** | Normal | 正常收发模式 |
| **预分频** | 4 | APB1 时钟 36MHz / 4 = 9MHz |
| **TimeSeg1** | 5 TQ | 采样点位置 |
| **TimeSeg2** | 3 TQ | 采样点之后 |
| **SJW** | 1 TQ | 同步跳转宽度 |
| **波特率** | 500 kbps | 9MHz / (1+5+3) = 1Mbps... 实际配置 |
| **自动重发** | 禁用 | 需手动重发 |
| **自动唤醒** | 启用 | 低功耗唤醒 |
| **FIFO0 中断** | 启用 | 接收消息中断 |
| **过滤器** | 接收全部 | FilterMask = 0x00000000 |

```c
// CAN 初始化
hcan.Instance = CAN1;
hcan.Init.Prescaler = 4;
hcan.Init.Mode = CAN_MODE_NORMAL;
hcan.Init.SyncJumpWidth = CAN_SJW_1TQ;
hcan.Init.TimeSeg1 = CAN_BS1_5TQ;
hcan.Init.TimeSeg2 = CAN_BS2_3TQ;
hcan.Init.TimeTriggeredMode = DISABLE;
hcan.Init.AutoBusOff = DISABLE;
hcan.Init.AutoWakeUp = ENABLE;
hcan.Init.AutoRetransmission = DISABLE;
hcan.Init.ReceiveFifoLocked = DISABLE;
hcan.Init.TransmitFifoPriority = DISABLE;
```

### PWM 配置 (TIM2 CH1 - 舵机控制)

| 参数 | 值 | 说明 |
|------|-----|------|
| **定时器** | TIM2 | 通用定时器 |
| **通道** | CH1 (PA15) | PWM 输出 |
| **预分频** | 719 | 72MHz / 720 = 100kHz |
| **周期** | 1999 | 100kHz / 2000 = 50Hz (20ms) |
| **PWM 模式** | PWM1 | 向上计数 |
| **脉宽范围** | 50 ~ 250 | 0.5ms ~ 2.5ms |
| **角度映射** | 0° ~ 180° | 线性映射 |

**角度到脉宽转换公式**:
```
Pulse = ((Angle - 0) / ANGLE_MAX) * 200 + 50
```

- Angle = 0° → Pulse = 50 (0.5ms)
- Angle = 90° → Pulse = 150 (1.5ms)
- Angle = 180° → Pulse = 250 (2.5ms)

```c
// PWM 初始化
htim2.Instance = TIM2;
htim2.Init.Prescaler = 719;
htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
htim2.Init.Period = 1999;
htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;

// PWM 通道配置
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 100;  // 初始脉宽
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
```

### UART 配置 (USART1 - 调试串口)

| 参数 | 值 | 说明 |
|------|-----|------|
| **实例** | USART1 | APB2 时钟 72MHz |
| **波特率** | 115200 | 标准调试波特率 |
| **数据位** | 8 | 8-bit 数据 |
| **停止位** | 1 | 1 位停止位 |
| **校验** | 无 | 无奇偶校验 |
| **流控** | 无 | 无硬件流控 |
| **模式** | TX_RX | 收发双向 |

```c
// UART 初始化
huart1.Instance = USART1;
huart1.Init.BaudRate = 115200;
huart1.Init.WordLength = UART_WORDLENGTH_8B;
huart1.Init.StopBits = UART_STOPBITS_1;
huart1.Init.Parity = UART_PARITY_NONE;
huart1.Init.Mode = UART_MODE_TX_RX;
huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart1.Init.OverSampling = UART_OVERSAMPLING_16;
```

### 时钟配置

```
┌─────────────────────────────────────────────────────────────┐
│                     时钟树配置                               │
│                                                             │
│  HSE (12MHz) ──► PLL (x6) ──► SYSCLK (72MHz)               │
│                                   │                         │
│                                   ├──► AHB (72MHz)          │
│                                   │    ├──► APB2 (72MHz)    │
│                                   │    │    ├──► TIM1       │
│                                   │    │    ├──► USART1     │
│                                   │    │    └──► GPIO       │
│                                   │    └──► APB1 (36MHz)    │
│                                   │         ├──► TIM2       │
│                                   │         ├──► CAN        │
│                                   │         └──► USART2/3   │
│                                   └──► Flash (2WS)          │
└─────────────────────────────────────────────────────────────┘
```

---

## CAN 通信协议

### 协议概述

本模块使用 CAN 2.0B 扩展帧 (Extended ID, 29-bit) 进行通信。通信采用主从模式，主控制器发送命令，扩展模块响应数据。

### 帧格式定义

**扩展帧 ID 布局 (29-bit)**:

```
┌─────────────────────────────────────────────────────────────────┐
│  Bit 28 ~ 20    │  Bit 19 ~ 12    │  Bit 11 ~ 0                │
│  (未使用)        │  Node ID (7bit)  │  CMD (12bit)               │
│  固定为 0        │  bit[18:12]      │  bit[11:0]                 │
└─────────────────────────────────────────────────────────────────┘
```

**提取公式**:
```c
uint16_t id = (RxHeader.ExtId >> 12) & 0x7F;   // Node ID
uint16_t cmd = (RxHeader.ExtId >> 0) & 0xFFF;   // CMD
```

**构造公式**:
```c
txHeader.ExtId = (nodeId << 12) | cmd;
```

### 支持的命令

#### CMD 0x02 - Set Position (设置舵机角度)

| 方向 | 数据格式 | 说明 |
|------|----------|------|
| 主 → 从 | `float angle[0..3]` + `float power[4..7]` | 设置目标角度和功率限制 |

**数据字段**:
- `angle`: 目标角度 (float, 0.0 ~ 180.0)
- `power`: 功率限制 (float, 当前未使用)

**响应**:
```c
// 回传角度 + ACK
*(float*)&_data[0] = handAngle;  // 当前角度
_data[4] = 1;                     // ACK 标志
txHeader.ExtId = (nodeId << 12) | 0x23;
CAN_Send(&txHeader, _data);
```

#### CMD 0x23 - Get Position (获取位置和颜色)

| 方向 | 数据格式 | 说明 |
|------|----------|------|
| 主 → 从 | 无 | 请求当前位置和颜色数据 |

**响应 (两帧)**:

**第一帧 - 角度数据**:
```c
*(float*)&_data[0] = handAngle;  // 当前角度 (float)
_data[4] = 1;                     // ACK 标志
txHeader.ExtId = (nodeId << 12) | 0x23;
CAN_Send(&txHeader, _data);
```

**第二帧 - 颜色数据**:
```c
*(unsigned short*)&_data[0] = rgb.r;  // Red (0-65535)
*(unsigned short*)&_data[2] = rgb.g;  // Green (0-65535)
*(unsigned short*)&_data[4] = rgb.b;  // Blue (0-65535)
*(unsigned short*)&_data[6] = rgb.c;  // Clear (0-65535)
txHeader.ExtId = (nodeId << 12) | 0x7B;
CAN_Send(&txHeader, _data);
```

#### CMD 0x32 - Get Version (获取固件版本)

| 方向 | 数据格式 | 说明 |
|------|----------|------|
| 主 → 从 | 无 | 请求固件版本 |

**响应**:
```c
_data[0] = 0;              // 保留
_data[1] = 0;              // 保留
_data[2] = 0;              // 保留
_data[3] = SOFT_VER_A;    // 主版本号 (V1)
_data[4] = SOFT_VER_B;    // 次版本号 (V3)
txHeader.ExtId = (nodeId << 12) | 0x32;
CAN_Send(&txHeader, _data);
```

#### CMD 0x42 - Get IMU Data (获取姿态数据)

| 方向 | 数据格式 | 说明 |
|------|----------|------|
| 主 → 从 | 无 | 请求 IMU 姿态数据 |

**响应**:
```c
*(short*)&_data[0] = (short)(MpuData.axAngleMean * 100);  // X轴角度 ×100
*(short*)&_data[2] = (short)(MpuData.ayAngleMean * 100);  // Y轴角度 ×100
*(short*)&_data[4] = (short)(MpuData.azAngleMean * 100);  // Z轴角度 ×100
*(short*)&_data[6] = (short)(MpuData.MagDirMean * 100);   // 磁力计方向 ×100
txHeader.ExtId = (nodeId << 12) | 0x42;
CAN_Send(&txHeader, _data);
```

#### CMD 0x7F - Reboot (软件复位)

| 方向 | 数据格式 | 说明 |
|------|----------|------|
| 主 → 从 | 无 | 触发 MCU 软件复位 |

```c
case 0x7f:
    HAL_NVIC_SystemReset();  // 软件复位
    break;
```

### 通信时序

```
主控制器                              扩展模块
    │                                    │
    │──── CMD 0x02 (Set Position) ──────►│
    │                                    │  执行舵机控制
    │◄──── ACK (CMD 0x23) ─────────────│
    │                                    │
    │──── CMD 0x23 (Get Position) ──────►│
    │◄──── 角度数据 (CMD 0x23) ─────────│
    │◄──── 颜色数据 (CMD 0x7B) ─────────│
    │                                    │
    │──── CMD 0x42 (Get IMU Data) ──────►│
    │◄──── IMU 数据 (CMD 0x42) ─────────│
    │                                    │
    │──── CMD 0x32 (Get Version) ───────►│
    │◄──── 版本数据 (CMD 0x32) ─────────│
    │                                    │
```

### 节点 ID 配置

默认节点 ID 为 `7`，可通过修改 `main.c` 中的 `CAN_ID` 宏定义进行配置:

```c
#define CAN_ID  7  // 默认节点 ID
```

不同节点 ID 对应不同的初始角度:
- `CAN_ID = 7`: 初始角度 100°
- `CAN_ID = 27` 或 `37`: 初始角度 150°
- 其他: 初始角度 110° (ANGLE_MAX/2 + 20)

---

## 传感器模块

### MPU9250 (IMU 9轴传感器)

#### 基本参数

| 项目 | 参数 |
|------|------|
| **型号** | MPU9250 (MPU6500 + AK8963) |
| **I2C 地址** | 0x68 (MPU6500), 0x0C (AK8963) |
| **接口** | 软件 I2C (PA12 SCL, PA11 SDA) |
| **采样率** | 50 Hz |
| **陀螺仪量程** | ±2000 dps |
| **加速度计量程** | ±2g |
| **磁力计量程** | ±4900 μT |

#### 寄存器配置

```c
// MPU6500 关键寄存器
#define MPU_PWR_MGMT1_REG     0x6B  // 电源管理 1
#define MPU_PWR_MGMT2_REG     0x6C  // 电源管理 2
#define MPU_SAMPLE_RATE_REG   0x19  // 采样率分频
#define MPU_CFG_REG           0x1A  // 配置寄存器
#define MPU_GYRO_CFG_REG      0x1B  // 陀螺仪配置
#define MPU_ACCEL_CFG_REG     0x1C  // 加速度计配置
#define MPU_INTBP_CFG_REG     0x37  // 中断/旁路配置
#define MPU_ACCEL_XOUTH_REG   0x3B  // 加速度计 X 轴高字节
#define MPU_GYRO_XOUTH_REG    0x43  // 陀螺仪 X 轴高字节
#define MPU_DEVICE_ID_REG     0x75  // 设备 ID

// AK8963 关键寄存器
#define AK8963_ADDR           0x0C  // I2C 地址
#define MAG_CNTL1             0x0A  // 控制寄存器 1
#define MAG_XOUT_L            0x03  // 磁力计 X 轴低字节
```

#### 初始化流程

```c
uint8_t MPU9250_Init(void) {
    // 1. 复位 MPU9250
    MPU_Write_Byte(MPU9250_ADDR, MPU_PWR_MGMT1_REG, 0x80);
    HAL_Delay(100);
    
    // 2. 唤醒 MPU9250
    MPU_Write_Byte(MPU9250_ADDR, MPU_PWR_MGMT1_REG, 0x00);
    
    // 3. 配置陀螺仪量程 ±2000dps
    MPU_Set_Gyro_Fsr(3);
    
    // 4. 配置加速度计量程 ±2g
    MPU_Set_Accel_Fsr(0);
    
    // 5. 配置采样率 50Hz
    MPU_Set_Rate(50);
    
    // 6. 关闭中断
    MPU_Write_Byte(MPU9250_ADDR, MPU_INT_EN_REG, 0x00);
    
    // 7. 关闭 FIFO
    MPU_Write_Byte(MPU9250_ADDR, MPU_FIFO_EN_REG, 0x00);
    
    // 8. 使能旁路模式 (直接读取 AK8963)
    MPU_Write_Byte(MPU9250_ADDR, MPU_INTBP_CFG_REG, 0x82);
    
    // 9. 验证 MPU6500 ID
    res = MPU_Read_Byte(MPU9250_ADDR, MPU_DEVICE_ID_REG);
    if (res != MPU6500_ID1 && res != MPU6500_ID2 && res != MPU6500_ID3)
        return 1;
    
    // 10. 配置时钟源 PLL X
    MPU_Write_Byte(MPU9250_ADDR, MPU_PWR_MGMT1_REG, 0x01);
    
    // 11. 验证 AK8963 ID
    res = MPU_Read_Byte(AK8963_ADDR, MAG_WIA);
    if (res != AK8963_ID)
        return 1;
    
    // 12. 配置 AK8963 连续测量模式
    MPU_Write_Byte(AK8963_ADDR, MAG_CNTL1, 0x11);
    
    return 0;
}
```

#### 姿态角计算

```c
void AccUpdataAngle() {
    // 1. 读取加速度计原始数据
    AccUpdata();
    
    // 2. 转换为角度 (-90° ~ 90°)
    MpuData.axAngle = -90 + ((MpuData.ax + 1) / (1 - (-1))) * 180;
    MpuData.ayAngle = -90 + ((MpuData.ay + 1) / (1 - (-1))) * 180;
    MpuData.azAngle = -90 + ((MpuData.az + 1) / (1 - (-1))) * 180;
    
    // 3. 滑动均值滤波
    AccUpdataAngleMean();
}
```

#### 滑动均值滤波

使用 10 点滑动窗口，去掉最大最小各 1 个值，取中间 8 个值的平均值:

```c
#define ACC_DATA_MEAN_BUFF_SIZE 10
#define ACC_DATA_MEAN_SIZE 6  // 10 * 0.6

void AccUpdataAngleMean() {
    static float axBuff[ACC_DATA_MEAN_BUFF_SIZE];
    static uint16_t idx = 0;
    
    // 存入新数据
    axBuff[idx] = MpuData.axAngle;
    
    // 满 10 次后计算均值
    if (++AccUpdataTimes >= ACC_DATA_MEAN_BUFF_SIZE) {
        // 排序
        select_sort(axBuff, ACC_DATA_MEAN_BUFF_SIZE, 1, SMALL_TO_LARGE, LEFT_TO_RIGHT, NULL, 0);
        select_sort(axBuff, ACC_DATA_MEAN_BUFF_SIZE, 1, LARGE_TO_SMALL, RIGHT_TO_LEFT, NULL, 0);
        
        // 去掉最大最小各 1 个，取中间 8 个平均
        for (CalIdx = 1; CalIdx < 9; CalIdx++) {
            doubleBuff += axBuff[CalIdx];
        }
        MpuData.axAngleMean = doubleBuff / 8;
    }
}
```

### TCS34725 (RGB 颜色传感器)

#### 基本参数

| 项目 | 参数 |
|------|------|
| **型号** | TCS34725 |
| **I2C 地址** | 0x29 |
| **接口** | 软件 I2C (PB3 SCL, PB5 SDA) |
| **积分时间** | 50ms |
| **增益** | 1x |
| **数据格式** | RGBC (Red, Green, Blue, Clear) |
| **数据范围** | 0 ~ 65535 |

#### 寄存器配置

```c
#define TCS34725_ADDRESS      0x29
#define TCS34725_COMMAND_BIT  0x80

#define TCS34725_ENABLE       0x00  // 使能寄存器
#define TCS34725_ATIME        0x01  // 积分时间
#define TCS34725_CONTROL      0x0F  // 增益控制
#define TCS34725_ID           0x12  // 设备 ID
#define TCS34725_STATUS       0x13  // 状态寄存器
#define TCS34725_CDATAL       0x14  // Clear 数据低字节
#define TCS34725_RDATAL       0x16  // Red 数据低字节
#define TCS34725_GDATAL       0x18  // Green 数据低字节
#define TCS34725_BDATAL       0x1A  // Blue 数据低字节

// 积分时间定义
#define TCS34725_INTEGRATIONTIME_50MS   0xEB   // 50ms
#define TCS34725_INTEGRATIONTIME_700MS  0x00   // 700ms

// 增益定义
#define TCS34725_GAIN_1X   0x00   // 无增益
#define TCS34725_GAIN_4X   0x01   // 4x 增益
#define TCS34725_GAIN_16X  0x02   // 16x 增益
#define TCS34725_GAIN_60X  0x03   // 60x 增益
```

#### 初始化流程

```c
u8 TCS34725_Init(void) {
    u8 id = 0;
    
    // 1. 初始化 I2C
    TCS34725_I2C_Init();
    
    // 2. 读取设备 ID
    TCS34725_Read(TCS34725_ID, &id, 1);
    
    // 3. 验证 ID (0x44 = TCS34725, 0x4D = TCS34727)
    if (id == 0x4D || id == 0x44) {
        // 4. 设置积分时间 50ms
        TCS34725_SetIntegrationTime(TCS34725_INTEGRATIONTIME_50MS);
        
        // 5. 设置增益 1x
        TCS34725_SetGain(TCS34725_GAIN_1X);
        
        // 6. 使能传感器
        TCS34725_Enable();
        
        return 1;
    }
    return 0;
}
```

#### RGB 到 HSL 转换

```c
void RGBtoHSL(COLOR_RGBC *Rgb, COLOR_HSL *Hsl) {
    // 1. 归一化 RGB (0-100)
    u8 r = Rgb->r * 100 / Rgb->c;
    u8 g = Rgb->g * 100 / Rgb->c;
    u8 b = Rgb->b * 100 / Rgb->c;
    
    // 2. 计算最大值、最小值、差值
    maxVal = max3v(r, g, b);
    minVal = min3v(r, g, b);
    difVal = maxVal - minVal;
    
    // 3. 计算亮度 (L)
    Hsl->l = (maxVal + minVal) / 2;
    
    // 4. 计算饱和度 (S)
    if (Hsl->l <= 50)
        Hsl->s = difVal * 100 / (maxVal + minVal);
    else
        Hsl->s = difVal * 100 / (200 - (maxVal + minVal));
    
    // 5. 计算色相 (H)
    if (maxVal == r)
        Hsl->h = 60 * (g - b) / difVal;
    else if (maxVal == g)
        Hsl->h = 60 * (b - r) / difVal + 120;
    else
        Hsl->h = 60 * (r - g) / difVal + 240;
}
```

---

## 软件流程

### 主程序流程

```
┌─────────────────────────────────────────────────────────────┐
│                        系统启动                              │
└──────────────────────────┬──────────────────────────────────┘
                           │
                           ▼
┌─────────────────────────────────────────────────────────────┐
│                    HAL_Init()                                │
│  - 初始化 HAL 库                                             │
│  - 配置 SysTick 定时器 (1ms 中断)                           │
└──────────────────────────┬──────────────────────────────────┘
                           │
                           ▼
┌─────────────────────────────────────────────────────────────┐
│                SystemClock_Config()                          │
│  - 配置 HSE (12MHz)                                         │
│  - 配置 PLL (x6) → SYSCLK = 72MHz                          │
│  - 配置 AHB = 72MHz, APB1 = 36MHz, APB2 = 72MHz            │
└──────────────────────────┬──────────────────────────────────┘
                           │
                           ▼
┌─────────────────────────────────────────────────────────────┐
│                    外设初始化                                │
│  - MX_GPIO_Init(): GPIO 配置                                │
│  - MX_USART1_UART_Init(): 串口 115200                      │
│  - MX_CAN_Init(): CAN 总线配置                              │
│  - MX_TIM2_Init(): PWM 50Hz 配置                           │
└──────────────────────────┬──────────────────────────────────┘
                           │
                           ▼
┌─────────────────────────────────────────────────────────────┐
│                    启动 PWM                                  │
│  - HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1)                │
│  - 设置初始角度 (根据节点 ID)                                │
└──────────────────────────┬──────────────────────────────────┘
                           │
                           ▼
┌─────────────────────────────────────────────────────────────┐
│                    上电自检动作                               │
│  - 节点 7: 100° → 0° (张开到闭合)                           │
│  - 节点 27/37: 150° → 30°                                  │
│  - 其他: 110° → 100°                                        │
└──────────────────────────┬──────────────────────────────────┘
                           │
                           ▼
┌─────────────────────────────────────────────────────────────┐
│                    主循环 while(1)                            │
│  - 持续更新 PWM 占空比                                       │
│  - 延时 10ms                                                │
│  - 等待 CAN 中断接收命令                                     │
└──────────────────────────┬──────────────────────────────────┘
                           │
                           ▼
┌─────────────────────────────────────────────────────────────┐
│                    CAN 中断回调                               │
│  - HAL_CAN_RxFifo0MsgPendingCallback()                      │
│  - 解析 ExtId → 提取 Node ID 和 CMD                         │
│  - 调用 CanDataAnalysis() 处理命令                           │
│  - 执行相应操作并发送响应                                     │
└─────────────────────────────────────────────────────────────┘
```

### CAN 中断处理流程

```
CAN RX FIFO0 中断
    │
    ▼
获取接收到的消息
    │
    ▼
提取 Node ID 和 CMD
    │
    ▼
检查 ID 是否匹配 (id == 0 || id == canNodeId)
    │
    ├── 不匹配 → 忽略
    │
    └── 匹配 → 调用 CanDataAnalysis()
                    │
                    ├── CMD 0x02: 设置舵机角度
                    │   ├── 解析 angle 和 power
                    │   ├── 转换为 PWM 脉宽
                    │   ├── 更新 TIM2 比较值
                    │   └── 发送 ACK 响应
                    │
                    ├── CMD 0x23: 获取位置
                    │   ├── 发送角度数据 (CMD 0x23)
                    │   └── 发送颜色数据 (CMD 0x7B)
                    │
                    ├── CMD 0x32: 获取版本
                    │   └── 发送版本号
                    │
                    ├── CMD 0x42: 获取 IMU
                    │   └── 发送三轴角度 + 磁力计方向
                    │
                    └── CMD 0x7F: 软件复位
                        └── HAL_NVIC_SystemReset()
```

---

## 软件版本

| 版本 | 日期 | 说明 |
|------|------|------|
| V1.3 | 2022 | 当前版本 |

---

## 编译与烧录

### 编译环境

1. **IDE**: Keil MDK-ARM V5
2. **工程文件**: `MDK-ARM/RobotHand.uvprojx`
3. **目标芯片**: STM32F103CBT6
4. **编译器**: ARM Compiler V5 (armcc)

### 编译步骤

1. 打开 Keil MDK-ARM V5
2. 加载工程文件 `MDK-ARM/RobotHand.uvprojx`
3. 选择编译目标: `RobotHand`
4. 点击 "Build" 或按 F7 编译
5. 编译成功后生成 `MDK-ARM/RobotHand.hex`

### 烧录方式

#### 方式 1: ST-Link 调试器

1. 连接 ST-Link 到目标板 (SWD 接口)
   - SWDIO: PA13
   - SWCLK: PA14
   - GND: GND
   - 3.3V: 3.3V
2. 在 Keil 中选择 "Options for Target" → "Debug"
3. 选择 "ST-Link Debugger"
4. 点击 "Download" 烧录

#### 方式 2: J-Link 调试器

1. 连接 J-Link 到目标板 (SWD 接口)
2. 在 Keil 中选择 "Options for Target" → "Debug"
3. 选择 "J-Link/J-Trace Cortex"
4. 点击 "Download" 烧录

### CubeMX 配置

项目基于 STM32CubeMX 生成，配置文件为 `Pwmtest.ioc`。如需修改外设配置:

1. 打开 STM32CubeMX 6.6.1
2. 加载 `Pwmtest.ioc`
3. 修改所需配置
4. 生成代码 (Generate Code)

---

## 文件结构

```
RobotHandExt/
├── Core/
│   ├── Inc/
│   │   ├── main.h              # 主程序头文件
│   │   ├── mpu.h               # MPU9250 驱动头文件
│   │   ├── tcs34725.h          # TCS34725 驱动头文件
│   │   ├── bsp_i2c.h           # 软件 I2C 头文件
│   │   ├── stm32f1xx_hal_conf.h # HAL 配置
│   │   └── stm32f1xx_it.h      # 中断处理头文件
│   └── Src/
│       ├── main.c              # 主程序
│       ├── mpu.c               # MPU9250 驱动
│       ├── tcs34725.c          # TCS34725 驱动
│       ├── bsp_i2c.c           # 软件 I2C 实现
│       ├── stm32f1xx_hal_msp.c # HAL MSP 配置
│       ├── stm32f1xx_it.c      # 中断处理
│       └── system_stm32f1xx.c  # 系统初始化
├── Drivers/
│   ├── CMSIS/                  # ARM CMSIS 库
│   └── STM32F1xx_HAL_Driver/   # STM32 HAL 驱动
├── MDK-ARM/
│   └── RobotHand.uvprojx       # Keil 工程文件
├── IIC/                        # I2C 相关文件
├── MPU9250/                    # MPU9250 相关文件
├── Pwmtest.ioc                 # CubeMX 配置文件
└── README.md                   # 本文件
```

---

## 开发注意事项

### 1. CAN 总线注意事项

- CAN 总线波特率需与主控制器一致
- 使用扩展帧 (Extended ID) 格式
- 自动重发已禁用，需手动处理重发
- 过滤器配置为接收全部消息

### 2. PWM 舵机控制注意事项

- PWM 频率必须为 50Hz (周期 20ms)
- 脉宽范围 0.5ms ~ 2.5ms 对应 0° ~ 180°
- 上电时需设置初始角度，避免舵机抖动
- 建议添加角度限幅保护

### 3. I2C 通信注意事项

- 软件 I2C 需注意时序延迟
- MPU9250 和 TCS34725 使用不同的 I2C 总线
- 读取 AK8963 磁力计需先使能旁路模式
- I2C 地址为 7-bit，发送时需左移 1 位

### 4. 传感器数据处理注意事项

- 加速度计数据需进行滑动均值滤波
- 颜色传感器数据需进行归一化处理
- RGB 到 HSL 转换需注意饱和度计算
- IMU 数据采样率 50Hz，需在主循环中周期性读取

---

## 许可证

本项目为私有项目，版权所有。