├── python
    ├── robstride_dynamics
    │   ├── __init__.py
    │   ├── table.py
    │   └── protocol.py
    ├── requirements.txt
    ├── setup.py
    ├── src
    │   ├── ping.py
    │   └── speed_control.py
    ├── README.md
    └── examples
    │   ├── basic_usage.py
    │   └── advanced_control.py
├── arduino
    ├── mi_motor_control
    │   ├── library.properties
    │   ├── QUICK_START.md
    │   ├── README.md
    │   ├── TWAI_CAN_MI_Motor.h
    │   ├── mi_motor_control.ino
    │   └── TWAI_CAN_MI_Motor.cpp
    ├── joint_position_control
    │   ├── README.md
    │   └── TWAI_CAN_MI_Motor.h
    ├── sim_setup_motor
    │   ├── USAGE.md
    │   ├── README.md
    │   ├── TWAI_CAN_MI_Motor.h
    │   └── TWAI_CAN_MI_Motor.cpp
    ├── dual_motor_control
    │   ├── USAGE.md
    │   ├── README.md
    │   ├── TWAI_CAN_MI_Motor.h
    │   └── TWAI_CAN_MI_Motor.cpp
    ├── ANGLE_CONTROL_GUIDE.md
    ├── ANTI_VIBRATION_GUIDE.md
    ├── QUICK_ID_SETUP_GUIDE.md
    ├── CONFIG_GUIDE.md
    ├── simple_joint_control
    │   ├── README.md
    │   └── TWAI_CAN_MI_Motor.h
    ├── PROJECTS_OVERVIEW.md
    ├── advanced_motor_control
    │   ├── README.md
    │   ├── TWAI_CAN_MI_Motor.h
    │   └── TWAI_CAN_MI_Motor.cpp
    └── README.md
├── cpp
    ├── include
    │   ├── can_interface.h
    │   └── protocol.h
    ├── CMakeLists.txt
    ├── examples
    │   └── basic_control.cpp
    └── README.md
├── rust
    ├── Cargo.toml
    └── examples
    │   └── basic_control.rs
├── LICENSE
├── .gitignore
├── README.md
├── scripts
    └── setup.sh
└── README_zh.md


/python/robstride_dynamics/__init__.py:
--------------------------------------------------------------------------------
1 | from .bus import RobstrideBus, Motor  # noqa: F401
2 | from .protocol import CommunicationType, ParameterType  # noqa: F401
3 | 


--------------------------------------------------------------------------------
/arduino/mi_motor_control/library.properties:
--------------------------------------------------------------------------------
 1 | name=MI_Motor_Controller
 2 | version=1.0.0
 3 | author=tianrking
 4 | maintainer=tianrking
 5 | sentence=小米电机ESP32控制器
 6 | paragraph=基于ESP32的小米电机CAN总线控制库，支持速度、位置、电流和运控四种控制模式。
 7 | category=Device Control
 8 | url=
 9 | architectures=esp32
10 | depends=ESP32-TWAI-CAN


--------------------------------------------------------------------------------
/python/requirements.txt:
--------------------------------------------------------------------------------
 1 | # Core dependencies for RobStride motor control
 2 | python-can>=4.0.0
 3 | numpy>=1.21.0
 4 | 
 5 | # Optional dependencies for enhanced functionality
 6 | tqdm>=4.62.0          # Progress bars
 7 | matplotlib>=3.5.0     # Plotting and visualization (optional)
 8 | 
 9 | # Development dependencies (install with: pip install -r requirements-dev.txt)
10 | # pytest>=6.0.0       # Testing framework
11 | # black>=21.0.0       # Code formatter
12 | # flake8>=3.9.0       # Linter
13 | # mypy>=0.910         # Type checking


--------------------------------------------------------------------------------
/cpp/include/can_interface.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * CAN Interface for RobStride Motor Control
 3 |  * Low-level SocketCAN interface wrapper
 4 |  */
 5 | 
 6 | #ifndef CAN_INTERFACE_H
 7 | #define CAN_INTERFACE_H
 8 | 
 9 | #include <cstdint>
10 | #include <string>
11 | #include <linux/can.h>
12 | #include <linux/can/raw.h>
13 | 
14 | class CanInterface {
15 | public:
16 |     CanInterface();
17 |     ~CanInterface();
18 | 
19 |     // Initialize CAN interface
20 |     bool init(const std::string& interface = "can0");
21 | 
22 |     // Close CAN interface
23 |     void close();
24 | 
25 |     // Send CAN frame
26 |     bool send_frame(uint32_t can_id, const uint8_t* data, uint8_t dlc);
27 | 
28 |     // Receive CAN frame with timeout
29 |     bool read_frame(struct can_frame* frame, int timeout_ms = 100);
30 | 
31 |     // Check if interface is ready
32 |     bool is_ready() const { return socket_fd_ >= 0; }
33 | 
34 | private:
35 |     int socket_fd_;
36 |     std::string interface_name_;
37 | };
38 | 
39 | #endif // CAN_INTERFACE_H


--------------------------------------------------------------------------------
/rust/Cargo.toml:
--------------------------------------------------------------------------------
 1 | [package]
 2 | name = "robstride-control"
 3 | version = "1.0.0"
 4 | edition = "2021"
 5 | authors = ["tianrking"]
 6 | description = "Rust implementation of tianrking motor control"
 7 | license = "MIT OR Apache-2.0"
 8 | repository = "https://github.com/tianrking/robstride-control"
 9 | documentation = "https://docs.rs/robstride-control"
10 | readme = "README.md"
11 | keywords = ["motor", "control", "can", "robotics", "embedded"]
12 | categories = ["embedded", "hardware-support", "robotics"]
13 | 
14 | [[bin]]
15 | name = "robstride-mit-position"
16 | path = "src/main.rs"
17 | 
18 | [[bin]]
19 | name = "basic_control"
20 | path = "examples/basic_control.rs"
21 | 
22 | [dependencies]
23 | socketcan = "3.5.0"
24 | ctrlc = { version = "3.5.1", features = ["termination"] }
25 | clap = { version = "4.0", features = ["derive"] }
26 | anyhow = "1.0"
27 | thiserror = "1.0"
28 | log = "0.4"
29 | env_logger = "0.10"
30 | 
31 | [dev-dependencies]
32 | tempfile = "3.0"
33 | 
34 | [profile.release]
35 | lto = true
36 | codegen-units = 1
37 | panic = "abort"
38 | 
39 | [profile.dev]
40 | debug = true


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2024 tianrking
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.


--------------------------------------------------------------------------------
/arduino/mi_motor_control/QUICK_START.md:
--------------------------------------------------------------------------------
 1 | # ⚡ 快速开始指南
 2 | 
 3 | ## 🚀 5分钟快速上手
 4 | 
 5 | ### 1️⃣ 硬件连接
 6 | ```
 7 | ESP32      CAN模块    小米电机
 8 | GPIO5  ←→ TX   ←→ CAN_H
 9 | GPIO4  ←→ RX   ←→ CAN_L
10 | 3.3V   ←→ VCC  ←→ 12-24V
11 | GND    ←→ GND  ←→ GND
12 | ```
13 | 
14 | ### 2️⃣ Arduino IDE设置
15 | 1. 安装ESP32开发板支持
16 | 2. 选择开发板：ESP32 Dev Module
17 | 3. 安装库：ESP32-TWAI-CAN
18 | 
19 | ### 3️⃣ 上传程序
20 | 1. 打开 `mi_motor_control.ino`
21 | 2. 选择正确的串口
22 | 3. 点击上传
23 | 
24 | ### 4️⃣ 运行测试
25 | 1. 打开串口监视器（115200波特率）
26 | 2. 观察初始化过程
27 | 3. 电机将自动开始转动测试
28 | 
29 | ## 📱 串口命令测试
30 | 
31 | 输入以下命令测试电机：
32 | ```
33 | stop          # 停止
34 | enable        # 使能
35 | speed 2.0     # 设置速度2 rad/s
36 | pos 1.57      # 转到90度位置
37 | help          # 查看帮助
38 | ```
39 | 
40 | ## ✅ 预期输出
41 | ```
42 | === 小米电机控制程序 ESP32 ===
43 | 正在初始化系统...
44 | 1. 初始化CAN总线...
45 | 2. 初始化电机(ID=1)...
46 | 3. 设置电机机械零点...
47 | 4. 设置速度控制模式...
48 | 5. 使能电机...
49 | === 系统初始化完成 ===
50 | 电机已使能，开始运行...
51 | 
52 | M1: 0,1,0,0,0,0,0,0,2,angle:0.00,speed:0.00,torque:0.00,temp:25.0
53 | ```
54 | 
55 | ## 🔧 如果出现问题
56 | 
57 | 1. **无法上传**：检查ESP32驱动和串口连接
58 | 2. **电机无响应**：检查CAN连接和电源
59 | 3. **编译错误**：确认ESP32开发板和库已安装
60 | 
61 | 详细说明请参考 `README.md`


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | # Python
 2 | __pycache__/
 3 | *.py[cod]
 4 | *$py.class
 5 | *.so
 6 | .Python
 7 | build/
 8 | develop-eggs/
 9 | dist/
10 | downloads/
11 | eggs/
12 | .eggs/
13 | lib/
14 | lib64/
15 | parts/
16 | sdist/
17 | var/
18 | wheels/
19 | pip-wheel-metadata/
20 | share/python-wheels/
21 | *.egg-info/
22 | .installed.cfg
23 | *.egg
24 | MANIFEST
25 | 
26 | # Virtual environments
27 | .env
28 | .venv
29 | env/
30 | venv/
31 | ENV/
32 | env.bak/
33 | venv.bak/
34 | 
35 | # IDEs
36 | .vscode/
37 | .idea/
38 | *.swp
39 | *.swo
40 | *~
41 | 
42 | # OS
43 | .DS_Store
44 | .DS_Store?
45 | ._*
46 | .Spotlight-V100
47 | .Trashes
48 | ehthumbs.db
49 | Thumbs.db
50 | 
51 | # C/C++ build files
52 | **/CMakeFiles/
53 | **/CMakeCache.txt
54 | **/cmake_install.cmake
55 | **/Makefile
56 | *.o
57 | *.exe
58 | *.out
59 | *.app
60 | 
61 | # Rust build files
62 | **/target/
63 | **/Cargo.lock
64 | **/*.rs.bk
65 | 
66 | # Logs
67 | *.log
68 | logs/
69 | 
70 | # Temporary files
71 | *.tmp
72 | *.temp
73 | tmp/
74 | 
75 | # Build artifacts
76 | **/build/
77 | **/dist/
78 | 
79 | # Configuration files with sensitive data
80 | config.ini
81 | .env.local
82 | .env.*.local
83 | 
84 | # CAN interface logs
85 | can_logs/
86 | *.candump


--------------------------------------------------------------------------------
/arduino/joint_position_control/README.md:
--------------------------------------------------------------------------------
 1 | # 关节位置控制程序
 2 | 
 3 | **专门为关节电机优化的位置控制系统**
 4 | 
 5 | ## 🎯 项目特点
 6 | 
 7 | - ✅ 专业的关节位置控制
 8 | - ✅ 串口透传命令支持
 9 | - ✅ 平滑运动，减少振动
10 | - ✅ 实时状态监控
11 | - ✅ 完整错误检测
12 | 
13 | ## 📱 串口命令
14 | 
15 | ### 位置控制命令
16 | ```bash
17 | pos 1.57       # 转到1.57弧度位置
18 | angle 90       # 转到90度位置
19 | speed 2.0      # 设置最大速度2.0 rad/s
20 | ```
21 | 
22 | ### 控制命令
23 | ```bash
24 | stop           # 停止电机
25 | zero           # 设当前位置为零点
26 | enable         # 使能电机
27 | disable        # 禁用电机
28 | status         # 显示详细状态
29 | help           # 显示帮助
30 | ```
31 | 
32 | ## 🔧 硬件连接
33 | 
34 | ```
35 | ESP32      CAN模块    小米电机
36 | GPIO5  ←→ TX   ←→ CAN_H
37 | GPIO4  ←→ RX   ←→ CAN_L
38 | 3.3V   ←→ VCC  ←→ 12-24V
39 | GND    ←→ GND  ←→ GND
40 | ```
41 | 
42 | ## 🚀 使用步骤
43 | 
44 | 1. **Arduino IDE设置**
45 |    - 开发板：ESP32 Dev Module
46 |    - 安装库：ESP32-TWAI-CAN
47 | 
48 | 2. **上传程序**
49 |    - 打开 `joint_position_control.ino`
50 |    - 选择串口并上传
51 | 
52 | 3. **开始控制**
53 |    - 打开串口监视器（115200）
54 |    - 输入位置命令开始控制
55 | 
56 | ## ⚙️ 关键参数
57 | 
58 | - **最大速度**: 3.0 rad/s（可调）
59 | - **位置KP**: 30.0（减少振动）
60 | - **速度KP**: 10.0
61 | - **位置容差**: 0.05 rad
62 | 
63 | ## 📊 状态监控
64 | 
65 | 程序会实时显示：
66 | - 当前位置（弧度和度数）
67 | - 运动速度
68 | - 扭矩输出
69 | - 电机温度
70 | - 错误状态
71 | 
72 | ## 🛠️ 优势
73 | 
74 | 1. **专业关节控制**：专门为关节应用优化
75 | 2. **防振动设计**：参数经过优化
76 | 3. **简单命令**：支持弧度和角度输入
77 | 4. **安全保护**：自动错误检测和处理
78 | 5. **透传控制**：直接串口命令控制
79 | 
80 | ## 📋 适用场景
81 | 
82 | - 机器人关节控制
83 | - 机械臂位置控制
84 | - 舵机替代应用
85 | - 精密角度定位
86 | 
87 | 详细文档请参考父目录的 `ANTI_VIBRATION_GUIDE.md`


--------------------------------------------------------------------------------
/arduino/sim_setup_motor/USAGE.md:
--------------------------------------------------------------------------------
 1 | # 电机ID设置程序 - 使用说明
 2 | 
 3 | ## 🚀 快速开始
 4 | 
 5 | ### 1. 上传程序
 6 | - 打开 `sim_setup_motor.ino`
 7 | - 选择ESP32开发板
 8 | - 上传程序
 9 | 
10 | ### 2. 打开串口监视器
11 | - 波特率：115200
12 | - 会看到：`=== 小米电机ID设置程序 ===`
13 | 
14 | ### 3. 测试通信
15 | ```bash
16 | test
17 | ```
18 | 如果显示 `✅ CAN通信正常！` 则可以继续
19 | 
20 | ## 📋 主要命令
21 | 
22 | ### 🔍 查看和测试
23 | ```bash
24 | test              # 测试CAN通信
25 | scan              # 扫描1-127所有电机
26 | scan 1 20         # 扫描指定范围
27 | verify            # 验证已设置电机
28 | help              # 显示帮助
29 | ```
30 | 
31 | ### 🔧 设置电机ID
32 | ```bash
33 | set 1 2           # 将ID=1改为ID=2
34 | batch 5           # 批量设置：从ID=5开始
35 | reset 3           # 重置电机3
36 | ```
37 | 
38 | ## 🎯 推荐使用流程
39 | 
40 | ### 场景1：设置3个电机为ID=1,2,3
41 | ```bash
42 | test              # 1. 测试通信
43 | batch 3           # 2. 批量设置
44 | # 连接第1个电机，按回车 → ID=1
45 | # 连接第2个电机，按回车 → ID=2
46 | # 连接第3个电机，按回车 → ID=3
47 | verify            # 3. 验证结果
48 | ```
49 | 
50 | ### 场景2：精确设置特定ID
51 | ```bash
52 | test              # 1. 测试通信
53 | scan              # 2. 查看当前ID
54 | set 7 1           # 3. 将ID=7改为ID=1
55 | verify            # 4. 验证修改
56 | ```
57 | 
58 | ## ⚠️ 重要注意事项
59 | 
60 | 1. **修改ID时**：总线上只能连接**一个电机**！
61 | 2. **修改完成后**：断开电机，连接下一个，继续修改
62 | 3. **所有电机设置完成后**：可以全部连接到总线
63 | 4. **ID范围**：1-127
64 | 
65 | ## 🔧 故障排除
66 | 
67 | ### test命令失败？
68 | - 检查CAN连接线
69 | - 检查电机电源(12-24V)
70 | - 检查波特率(1Mbps)
71 | 
72 | ### scan扫描不到？
73 | - 先运行test命令
74 | - 只连接一个电机测试
75 | 
76 | ### set命令失败？
77 | - 确保总线上只有一个电机
78 | - 先运行test确认通信正常
79 | 
80 | ---
81 | 
82 | **简单使用流程**：test → batch/scan/set → verify ✅


--------------------------------------------------------------------------------
/python/robstride_dynamics/table.py:
--------------------------------------------------------------------------------
 1 | """
 2 | Communication constants definitions.
 3 | 电机常量表定义
 4 | 
 5 | This file contains the definitions of the constants used in communicating with the Robstride motors.
 6 | """
 7 | 
 8 | 
 9 | import numpy as np
10 | 
11 | 
12 | MODEL_MIT_POSITION_TABLE = {
13 |     "rs-00": 4 * np.pi,
14 |     "rs-01": 4 * np.pi,
15 |     "rs-02": 4 * np.pi,
16 |     "rs-03": 4 * np.pi,
17 |     "rs-04": 4 * np.pi,
18 |     "rs-05": 4 * np.pi,
19 |     "rs-06": 4 * np.pi,
20 | }
21 | """Position scaling range for MIT frame, in rad"""
22 | 
23 | 
24 | MODEL_MIT_VELOCITY_TABLE = {
25 |     "rs-00": 50,
26 |     "rs-01": 44,
27 |     "rs-02": 44,
28 |     "rs-03": 50,
29 |     "rs-04": 15,
30 |     "rs-05": 33,
31 |     "rs-06": 20,
32 | }
33 | """Velocity scaling range for MIT frame, in rad/s"""
34 | 
35 | 
36 | MODEL_MIT_TORQUE_TABLE = {
37 |     "rs-00": 17,
38 |     "rs-01": 17,
39 |     "rs-02": 17,
40 |     "rs-03": 60,
41 |     "rs-04": 120,
42 |     "rs-05": 17,
43 |     "rs-06": 60,
44 | }
45 | """Torque scaling range for MIT frame, in Nm"""
46 | 
47 | 
48 | MODEL_MIT_KP_TABLE = {
49 |     "rs-00": 500.0,
50 |     "rs-01": 500.0,
51 |     "rs-02": 500.0,
52 |     "rs-03": 5000.0,
53 |     "rs-04": 5000.0,
54 |     "rs-05": 500.0,
55 |     "rs-06": 5000.0,
56 | }
57 | """KP scaling range for MIT frame, in Nm/rad"""
58 | 
59 | 
60 | MODEL_MIT_KD_TABLE = {
61 |     "rs-00": 5.0,
62 |     "rs-01": 5.0,
63 |     "rs-02": 5.0,
64 |     "rs-03": 100.0,
65 |     "rs-04": 100.0,
66 |     "rs-05": 5.0,
67 |     "rs-06": 100.0,
68 | }
69 | """KD scaling range for MIT frame, in Nm/rad/s"""
70 | 


--------------------------------------------------------------------------------
/python/setup.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | # -*- coding: utf-8 -*-
 3 | """
 4 | tianrking Control Python Package Setup
 5 | """
 6 | 
 7 | from setuptools import setup, find_packages
 8 | 
 9 | with open("README.md", "r", encoding="utf-8") as fh:
10 |     long_description = fh.read()
11 | 
12 | with open("requirements.txt", "r", encoding="utf-8") as fh:
13 |     requirements = [line.strip() for line in fh if line.strip() and not line.startswith("#")]
14 | 
15 | setup(
16 |     name="robstride-control",
17 |     version="1.0.0",
18 |     author="tianrking",
19 |     author_email="",
20 |     description="Python implementation of tianrking motor control",
21 |     long_description=long_description,
22 |     long_description_content_type="text/markdown",
23 |     url="https://github.com/tianrking/robstride-control",
24 |     packages=find_packages(),
25 |     classifiers=[
26 |         "Development Status :: 4 - Beta",
27 |         "Intended Audience :: Developers",
28 |         "License :: OSI Approved :: MIT License",
29 |         "Operating System :: POSIX :: Linux",
30 |         "Programming Language :: Python :: 3",
31 |         "Programming Language :: Python :: 3.8",
32 |         "Programming Language :: Python :: 3.9",
33 |         "Programming Language :: Python :: 3.10",
34 |         "Programming Language :: Python :: 3.11",
35 |         "Topic :: Software Development :: Libraries :: Python Modules",
36 |         "Topic :: System :: Hardware :: Hardware Drivers",
37 |     ],
38 |     python_requires=">=3.8",
39 |     install_requires=requirements,
40 |     entry_points={
41 |         "console_scripts": [
42 |             "robstride-position=src.position_control:main",
43 |             "robstride-speed=src.speed_control:main",
44 |         ],
45 |     },
46 |     include_package_data=True,
47 |     zip_safe=False,
48 | )


--------------------------------------------------------------------------------
/cpp/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.12)
 2 | project(tianrkingControl VERSION 1.0.0 LANGUAGES CXX)
 3 | 
 4 | # Set C++ standard
 5 | set(CMAKE_CXX_STANDARD 17)
 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON)
 7 | set(CMAKE_CXX_EXTENSIONS OFF)
 8 | 
 9 | # Compiler flags
10 | if(CMAKE_BUILD_TYPE STREQUAL "Debug")
11 |     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -DDEBUG")
12 | else()
13 |     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2 -DNDEBUG")
14 | endif()
15 | 
16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -pthread")
17 | 
18 | # Include directories
19 | include_directories(include)
20 | 
21 | # Add executable
22 | add_executable(robstride-mit-position src/position_control.cpp)
23 | 
24 | # Link libraries
25 | target_link_libraries(robstride-mit-position pthread)
26 | 
27 | # Installation
28 | install(TARGETS robstride-mit-position
29 |     RUNTIME DESTINATION bin
30 | )
31 | 
32 | # Testing
33 | enable_testing()
34 | add_test(NAME motor_connection_test
35 |          COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/scripts/test_motor.sh
36 |          WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
37 | 
38 | # Custom targets
39 | add_custom_target(setup-can
40 |     COMMAND sudo ip link set can0 type can bitrate 1000000
41 |     COMMAND sudo ip link set up can0
42 |     COMMENT "Setting up CAN interface"
43 | )
44 | 
45 | add_custom_target(install-deps
46 |     COMMAND sudo apt-get update
47 |     COMMAND sudo apt-get install -y build-essential can-utils cmake
48 |     COMMENT "Installing dependencies"
49 | )
50 | 
51 | # Package configuration
52 | set(CPACK_PACKAGE_NAME "robstride-control")
53 | set(CPACK_PACKAGE_VERSION ${PROJECT_VERSION})
54 | set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "tianrking Motor Control Library")
55 | set(CPACK_PACKAGE_VENDOR "tianrking")
56 | set(CPACK_GENERATOR "DEB;RPM")
57 | set(CPACK_DEBIAN_PACKAGE_DEPENDS "libc6 (>= 2.17), libgcc1 (>= 1:4.2), libstdc++6 (>= 5.2)")
58 | 
59 | include(CPack)


--------------------------------------------------------------------------------
/rust/examples/basic_control.rs:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * RobStride Rust Basic Control Example
 3 |  * Demonstrates basic motor control operations
 4 |  */
 5 | 
 6 | use std::env;
 7 | use std::f64::consts::PI;
 8 | use std::sync::{Arc, Mutex};
 9 | use std::thread;
10 | use std::time::Duration;
11 | 
12 | // Import from main module
13 | use socketcan::{CanSocket, CanFrame, EmbeddedFrame, CanId};
14 | 
15 | // Include the control functions from main.rs
16 | include!("../main.rs");
17 | 
18 | fn main() -> Result<(), Box<dyn std::error::Error>> {
19 |     let motor_id: u8 = env::args()
20 |         .nth(1)
21 |         .unwrap_or("11".to_string())
22 |         .parse()
23 |         .expect("Invalid motor ID");
24 | 
25 |     println!("🎯 RobStride Rust Basic Control Example");
26 |     println!("Using Motor ID: {}", motor_id);
27 | 
28 |     // Create CAN socket
29 |     let socket = Arc::new(Mutex::new(
30 |         CanSocket::open("can0")?
31 |     ));
32 | 
33 |     println!("✅ CAN interface initialized");
34 | 
35 |     // Initialize motor
36 |     println!("⚡ Enabling motor...");
37 |     enable_motor(&socket, motor_id)?;
38 |     thread::sleep(Duration::from_millis(500));
39 | 
40 |     println!("⚙️ Setting MIT mode...");
41 |     set_mode_raw(&socket, motor_id, 0)?;
42 |     thread::sleep(Duration::from_millis(200));
43 | 
44 |     println!("📊 Setting limits...");
45 |     write_limit(&socket, motor_id, PARAM_VELOCITY_LIMIT, 20.0)?;
46 |     write_limit(&socket, motor_id, PARAM_TORQUE_LIMIT, 20.0)?;
47 | 
48 |     // Test movements
49 |     println!("🎮 Starting test movements...");
50 | 
51 |     let movements = vec![
52 |         (0.0, "Home"),
53 |         (PI/2.0, "90°"),
54 |         (-PI/2.0, "-90°"),
55 |         (0.0, "Home"),
56 |     ];
57 | 
58 |     for (position, description) in movements {
59 |         println!("📍 Moving to {}", description);
60 | 
61 |         let socket = socket.lock().unwrap();
62 |         write_operation_frame(&socket, motor_id, position, 30.0, 0.5)?;
63 |         drop(socket);
64 | 
65 |         thread::sleep(Duration::from_secs(2));
66 |     }
67 | 
68 |     println!("✅ Test completed successfully!");
69 |     Ok(())
70 | }


--------------------------------------------------------------------------------
/cpp/examples/basic_control.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * RobStride C++ Basic Control Example
 3 |  * Demonstrates basic motor control operations
 4 |  */
 5 | 
 6 | #include "../include/can_interface.h"
 7 | #include "../include/protocol.h"
 8 | #include <iostream>
 9 | #include <unistd.h>
10 | 
11 | int main(int argc, char* argv[]) {
12 |     int motor_id = 11;
13 |     if (argc > 1) {
14 |         motor_id = std::atoi(argv[1]);
15 |     }
16 | 
17 |     std::cout << "🎯 RobStride C++ Basic Control Example" << std::endl;
18 |     std::cout << "Using Motor ID: " << motor_id << std::endl;
19 | 
20 |     CanInterface can;
21 | 
22 |     // Initialize CAN
23 |     if (!can.init("can0")) {
24 |         std::cerr << "❌ Failed to initialize CAN interface" << std::endl;
25 |         return 1;
26 |     }
27 | 
28 |     std::cout << "✅ CAN interface initialized" << std::endl;
29 | 
30 |     try {
31 |         // Enable motor
32 |         std::cout << "⚡ Enabling motor..." << std::endl;
33 |         enable_motor(can.socket(), motor_id);
34 |         usleep(500000);
35 | 
36 |         // Set MIT mode
37 |         std::cout << "⚙️ Setting MIT mode..." << std::endl;
38 |         set_mode_raw(can.socket(), motor_id, ControlMode::MIT_MODE);
39 |         usleep(200000);
40 | 
41 |         // Set limits
42 |         std::cout << "📊 Setting limits..." << std::endl;
43 |         write_limit(can.socket(), motor_id, ParamID::VELOCITY_LIMIT, 20.0);
44 |         write_limit(can.socket(), motor_id, ParamID::TORQUE_LIMIT, 20.0);
45 | 
46 |         // Test movements
47 |         std::cout << "🎮 Starting test movements..." << std::endl;
48 | 
49 |         double movements[] = {0.0, M_PI/2, -M_PI/2, 0.0};  // 0°, 90°, -90°, 0°
50 |         const char* descriptions[] = {"Home", "90°", "-90°", "Home"};
51 | 
52 |         for (int i = 0; i < 4; i++) {
53 |             std::cout << "📍 Moving to " << descriptions[i] << std::endl;
54 |             write_operation_frame(can.socket(), motor_id, movements[i], 30.0, 0.5);
55 |             sleep(2);  // Wait for movement
56 |         }
57 | 
58 |         std::cout << "✅ Test completed successfully!" << std::endl;
59 | 
60 |     } catch (const std::exception& e) {
61 |         std::cerr << "❌ Error: " << e.what() << std::endl;
62 |         return 1;
63 |     }
64 | 
65 |     return 0;
66 | }


--------------------------------------------------------------------------------
/arduino/dual_motor_control/USAGE.md:
--------------------------------------------------------------------------------
  1 | # 双电机控制 - 快速使用指南
  2 | 
  3 | ## 🚀 快速开始
  4 | 
  5 | ### 1. 上传程序
  6 | - 打开 `dual_motor_control.ino`
  7 | - 选择ESP32开发板
  8 | - 上传程序
  9 | 
 10 | ### 2. 打开串口监视器
 11 | - 波特率：115200
 12 | - 会看到：`=== 双电机控制程序 ===`
 13 | 
 14 | ### 3. 基本测试
 15 | ```bash
 16 | 1-90              # 电机1转到90度
 17 | 2-180             # 电机2转到180度
 18 | sync 0 0          # 双电机回到0度
 19 | ```
 20 | 
 21 | ## 📋 主要命令
 22 | 
 23 | ### 单电机控制
 24 | ```bash
 25 | 1-90              # 电机1到90度
 26 | 2-180             # 电机2到180度
 27 | 1-0               # 电机1回到0度
 28 | 2-270             # 电机2到270度
 29 | ```
 30 | 
 31 | ### 同步控制
 32 | ```bash
 33 | sync 90 180       # 电机1到90度，电机2到180度
 34 | sync 0 0          # 双电机回到0度
 35 | sync 45 135       # 电机1到45度，电机2到135度
 36 | ```
 37 | 
 38 | ### 配置命令
 39 | ```bash
 40 | config            # 显示当前配置
 41 | id1 3             # 设置电机1ID为3
 42 | id2 4             # 设置电机2ID为4
 43 | restart           # 重启双电机
 44 | ```
 45 | 
 46 | ### 参数调节
 47 | ```bash
 48 | kp1 25            # 设置电机1位置KP
 49 | kp2 30            # 设置电机2位置KP
 50 | speed 3.0         # 设置双电机速度
 51 | ```
 52 | 
 53 | ### 状态控制
 54 | ```bash
 55 | status            # 显示双电机状态
 56 | stop              # 停止双电机
 57 | zero              # 设零点
 58 | help              # 显示帮助
 59 | ```
 60 | 
 61 | ## 🎯 推荐使用流程
 62 | 
 63 | ### 1. 首次使用
 64 | ```bash
 65 | config            # 查看当前配置
 66 | 1-90              # 测试电机1
 67 | 2-90              # 测试电机2
 68 | sync 45 135       # 测试同步控制
 69 | ```
 70 | 
 71 | ### 2. 如果电机ID不是1和2
 72 | ```bash
 73 | id1 11            # 设置电机1的实际ID
 74 | id2 12            # 设置电机2的实际ID
 75 | restart           # 重启应用
 76 | 1-90              # 测试电机1
 77 | 2-90              # 测试电机2
 78 | ```
 79 | 
 80 | ### 3. 机器人关节控制示例
 81 | ```bash
 82 | sync 0 0          # 双关节回到起始位置
 83 | sync 90 45        # 关节1到90度，关节2到45度
 84 | sync 180 90       # 关节1到180度，关节2到90度
 85 | sync 0 0          # 回到起始位置
 86 | ```
 87 | 
 88 | ## ⚠️ 重要注意事项
 89 | 
 90 | 1. **默认电机ID**：1和2
 91 | 2. **ID范围**：1-127
 92 | 3. **角度输入**：0-360度
 93 | 4. **弧度输入**：支持小弧度值
 94 | 5. **同步控制**：两个电机同时运动
 95 | 
 96 | ## 🔧 故障排除
 97 | 
 98 | ### 电机不响应？
 99 | - 检查CAN连接和电机电源
100 | - 查看config确认电机ID是否正确
101 | - 尝试单独控制每个电机
102 | 
103 | ### ID设置错误？
104 | - 使用config查看当前配置
105 | - 重新设置正确的ID：id1 X, id2 Y
106 | - restart重启应用
107 | 
108 | ---
109 | 
110 | **简单使用**：1-90(电机1), 2-180(电机2), sync 90 45(同步) ✅


--------------------------------------------------------------------------------
/python/src/ping.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | # -*- coding: utf-8 -*-
 3 | """
 4 | RobStride CAN 总线扫描工具 (Python 版)
 5 | 
 6 | 用法: python3 scan_bus.py [channel]
 7 | 示例: 
 8 |     sudo python3 scan_bus.py can0
 9 |     sudo python3 scan_bus.py can1
10 | 
11 | 此脚本将 ping 1 到 254 范围内的所有 ID，并报告响应的电机。
12 | **注意：** 访问 CAN 硬件通常需要 'sudo' 权限。
13 | """
14 | 
15 | import sys
16 | import os
17 | import time
18 | 
19 | # --- 导入 SDK ---
20 | # 假设此脚本与 position_control_mit.py 在同一目录
21 | # (即，上一级目录是 SDK 的根目录)
22 | try:
23 |     # 尝试将 SDK 根目录添加到路径
24 |     sdk_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
25 |     if sdk_path not in sys.path:
26 |         sys.path.insert(0, sdk_path)
27 |     
28 |     # 1. 尝试从已安装的包导入
29 |     from robstride_dynamics import RobstrideBus
30 | except ImportError:
31 |     # 2. 尝试从本地文件导入 (如果 SDK 未安装)
32 |     try:
33 |         print("未找到 'robstride_dynamics' 包, 尝试从本地文件导入...")
34 |         from bus import RobstrideBus
35 |     except ImportError as e:
36 |         print(f"❌ 无法导入 RobstrideBus SDK: {e}")
37 |         print("请确保此脚本的上一级目录是 SDK 的根目录,")
38 |         print("或者 SDK 已经通过 'pip install -e .' 安装。")
39 |         sys.exit(1)
40 | 
41 | def main():
42 |     # --- 1. 获取 CAN 通道 ---
43 |     if len(sys.argv) > 1:
44 |         channel = sys.argv[1]
45 |     else:
46 |         channel = "can0"
47 | 
48 |     print(f"🚀 RobStride 总线扫描工具")
49 |     print(f"📡 正在扫描通道: {channel}")
50 |     print(f"🔍 搜索范围: ID 1 到 254")
51 |     print("...")
52 |     time.sleep(1) # 暂停一下让用户阅读
53 | 
54 |     # --- 2. 运行扫描 ---
55 |     found_motors = None
56 |     try:
57 |         # RobstrideBus.scan_channel 已经为我们实现了所有逻辑
58 |         # 它内部使用了 tqdm 来显示进度条
59 |         found_motors = RobstrideBus.scan_channel(channel, start_id=1, end_id=255) # end_id=255 会扫描到 254
60 |     
61 |     except Exception as e:
62 |         print(f"\n❌ 扫描出错: {e}")
63 |         if "Operation not permitted" in str(e) or "Permission denied" in str(e):
64 |             print("🔑 权限错误：请使用 'sudo' 运行此脚本来访问 CAN 硬件。")
65 |             print(f"   示例: sudo python3 {sys.argv[0]} {channel}")
66 |         elif "No such device" in str(e):
67 |             print(f"🔌 设备错误：找不到 CAN 接口 '{channel}'。")
68 |         sys.exit(1)
69 | 
70 |     # --- 3. 打印结果 ---
71 |     if not found_motors:
72 |         print("\n🚫 未在总线上找到任何响应的电机。")
73 |     else:
74 |         print("\n✅ 扫描完成！发现以下电机：")
75 |         print("=" * 60)
76 |         print(f"{'电机 ID':<10} | {'MCU 唯一标识符 (UUID)':<45}")
77 |         print("-" * 60)
78 |         
79 |         # found_motors 是一个字典: {id: (id, uuid_bytearray)}
80 |         # 我们按 ID 排序
81 |         for motor_id in sorted(found_motors.keys()):
82 |             # value 是一个元组 (id, uuid)
83 |             _id, uuid = found_motors[motor_id]
84 |             
85 |             # 将 bytearray 转换为更易读的十六进制字符串
86 |             uuid_hex = uuid.hex() # 'hex()' 是 bytearray 的一个方法
87 |             
88 |             print(f"{motor_id:<10} | {uuid_hex}")
89 |             
90 |         print("=" * 60)
91 | 
92 | if __name__ == "__main__":
93 |     main()


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # RobStride Control
  2 | 
  3 | [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT)
  4 | [![C++](https://img.shields.io/badge/C++-17-blue.svg)](https://en.wikipedia.org/wiki/C%2B%2B17)
  5 | [![Python](https://img.shields.io/badge/Python-3.8%2B-green.svg)](https://www.python.org/downloads/)
  6 | [![Rust](https://img.shields.io/badge/Rust-1.70%2B-orange.svg)](https://www.rust-lang.org/)
  7 | [![Arduino](https://img.shields.io/badge/Arduino-ESP32-00979D.svg)](https://www.arduino.cc/)
  8 | 
  9 | High-performance motor control library for RobStride motors with implementations in Python, C++, Rust, and Arduino.
 10 | 
 11 | ## Quick Start
 12 | 
 13 | ```bash
 14 | # Environment setup
 15 | sudo apt-get install can-utils
 16 | sudo ip link set can0 type can bitrate 1000000
 17 | sudo ip link set up can0
 18 | 
 19 | # Python
 20 | cd python && pip install -r requirements.txt
 21 | python3 src/position_control.py 11
 22 | 
 23 | # C++
 24 | cd cpp && make && sudo ./build/robstride-mit-position 11
 25 | 
 26 | # Rust
 27 | cd rust && cargo run --release -- 11
 28 | 
 29 | # Arduino
 30 | # Open Arduino IDE, load arduino/simple_joint_control/simple_joint_control.ino
 31 | # Select ESP32 Dev Module and upload
 32 | ```
 33 | 
 34 | ## Languages
 35 | 
 36 | | Language | Control Frequency | Latency | Memory Usage | Platform |
 37 | |----------|------------------|--------|-------------|---------|
 38 | | Python   | 100 Hz  | 5ms  | 50MB     | Linux |
 39 | | C++      | 200 Hz  | 1ms  | 10MB     | Linux |
 40 | | Rust     | 150 Hz  | 2ms  | 15MB     | Linux |
 41 | | Arduino  | 50-200Hz| 2-20ms| 10-50KB  | ESP32/MCU |
 42 | 
 43 | ## Project Structure
 44 | 
 45 | ```
 46 | ├── python/          # Python implementation
 47 | ├── cpp/             # C++ implementation
 48 | ├── rust/            # Rust implementation
 49 | ├── arduino/         # Arduino implementation
 50 | └── scripts/         # Build and setup scripts
 51 | ```
 52 | 
 53 | ## Supported Motors
 54 | 
 55 | - **RS-00**: 17 Nm, 50 rad/s
 56 | - **RS-01**: 17 Nm, 44 rad/s
 57 | - **RS-02**: 17 Nm, 44 rad/s
 58 | - **RS-03**: 60 Nm, 50 rad/s
 59 | - **RS-04**: 120 Nm, 15 rad/s
 60 | - **RS-05**: 17 Nm, 33 rad/s
 61 | - **RS-06**: 60 Nm, 20 rad/s
 62 | 
 63 | ## Documentation
 64 | 
 65 | - [中文文档](README_zh.md)
 66 | - [Español](README_es.md)
 67 | - [Python](python/README.md)
 68 | - [C++](cpp/README.md)
 69 | - [Rust](rust/README.md)
 70 | - [Arduino](arduino/README.md)
 71 | 
 72 | ## Examples
 73 | 
 74 | ### Python
 75 | ```python
 76 | from src.position_control import PositionControllerMIT
 77 | controller = PositionControllerMIT(11)
 78 | controller.connect()
 79 | controller.set_angle(90.0)
 80 | ```
 81 | 
 82 | ### C++
 83 | ```cpp
 84 | CanInterface can;
 85 | can.init("can0");
 86 | enable_motor(can.socket(), 11);
 87 | write_operation_frame(can.socket(), 11, M_PI/2, 30.0, 0.5);
 88 | ```
 89 | 
 90 | ### Rust
 91 | ```rust
 92 | let socket = Arc::new(Mutex::new(CanSocket::open("can0")?));
 93 | enable_motor(&socket, 11)?;
 94 | write_operation_frame(&socket.lock()?, 11, std::f64::consts::PI/2.0, 30.0, 0.5)?;
 95 | ```
 96 | 
 97 | ### Arduino
 98 | ```cpp
 99 | TWAI_CAN_MI_Motor motor(11);
100 | motor.init(CAN_SPEED_1000KBPS);
101 | motor.enable_motor();
102 | motor.send_mit_command(PI/2, 30.0, 0.5);
103 | ```
104 | 
105 | ## License
106 | 
107 | MIT License - see [LICENSE](LICENSE) file
108 | 
109 | ## Support
110 | 
111 | - Issues: https://github.com/tianrking/robstride-control/issues


--------------------------------------------------------------------------------
/python/robstride_dynamics/protocol.py:
--------------------------------------------------------------------------------
 1 | """
 2 | Private protocol definitions.
 3 | 私有协议定义
 4 | 
 5 | This file contains the constants defined by the RobStride user manual.
 6 | """
 7 | 
 8 | import numpy as np
 9 | 
10 | 
11 | class CommunicationType:
12 |     """
13 |     Private communication type definitions
14 |     私有通信类型定义
15 |     """
16 | 
17 |     GET_DEVICE_ID       = 0
18 |     """Gets the device's ID and 64-bit MCU unique identifier."""
19 | 
20 |     OPERATION_CONTROL   = 1
21 |     """Sets target position, velocity, kp, and kd."""
22 | 
23 |     OPERATION_STATUS    = 2
24 |     """Motor report frame of position, velocity, torque, and temperature."""
25 | 
26 |     ENABLE              = 3
27 |     """Enables the motor."""
28 | 
29 |     DISABLE             = 4
30 |     """Disables the motor."""
31 | 
32 |     SET_ZERO_POSITION   = 6
33 |     """Set motor zero position."""
34 | 
35 |     SET_DEVICE_ID       = 7
36 |     """Set device ID."""
37 | 
38 |     READ_PARAMETER      = 17
39 |     """Read a parameter."""
40 | 
41 |     WRITE_PARAMETER     = 18
42 |     """Write a parameter."""
43 | 
44 |     FAULT_REPORT        = 21
45 |     """Fault report feedback frame."""
46 | 
47 |     SAVE_PARAMETERS     = 22
48 |     """Save all parameters."""
49 | 
50 |     SET_BAUDRATE        = 23
51 |     """Set baudrate."""
52 | 
53 |     ACTIVE_REPORT       = 24
54 |     """Motor active report frame."""
55 | 
56 |     SET_PROTOCOL        = 25
57 |     """Set protocol."""
58 | 
59 | 
60 | class ParameterType:
61 |     """
62 |     Parameter type definitions
63 |     参数 ID 定义
64 |     """
65 |     MECHANICAL_OFFSET       = (0x2005, np.float32,  "mechOffset")
66 |     MEASURED_POSITION       = (0x3016, np.float32,  "mechPos")
67 |     MEASURED_VELOCITY       = (0x3017, np.float32,  "mechVel")
68 |     MEASURED_TORQUE         = (0x302C, np.float32,  "torque_fdb")
69 |     MODE                    = (0x7005, np.int8,     "run_mode")
70 |     IQ_TARGET               = (0x7006, np.float32,  "iq_ref")
71 |     VELOCITY_TARGET         = (0x700A, np.float32,  "spd_ref")
72 |     TORQUE_LIMIT            = (0x700B, np.float32,  "limit_torque")
73 |     CURRENT_KP              = (0x7010, np.float32,  "cur_kp")
74 |     CURRENT_KI              = (0x7011, np.float32,  "cur_ki")
75 |     CURRENT_FILTER_GAIN     = (0x7014, np.float32,  "cur_filter_gain")
76 |     POSITION_TARGET         = (0x7016, np.float32,  "lof_ref")
77 |     VELOCITY_LIMIT          = (0x7017, np.float32,  "limit_spd")
78 |     CURRENT_LIMIT           = (0x7018, np.float32,  "limit_cur")
79 |     MECHANICAL_POSITION     = (0x7019, np.float32,  "mechPos")
80 |     IQ_FILTERED             = (0x701A, np.float32,  "iqf")
81 |     MECHANICAL_VELOCITY     = (0x701B, np.float32,  "mechVel")
82 |     VBUS                    = (0x701C, np.float32,  "VBUS")
83 |     # ROTATION                = (0x701D, np.float32,  "rot_cnt")
84 |     POSITION_KP             = (0x701E, np.float32,  "loc_kp")
85 |     VELOCITY_KP             = (0x701F, np.float32,  "spd_kp")
86 |     VELOCITY_KI             = (0x7020, np.float32,  "spd_ki")
87 |     VELOCITY_FILTER_GAIN    = (0x7021, np.float32,  "spd_filter_gain")
88 |     VEL_ACCELERATION_TARGET = (0x7022, np.float32,  "acc_rad")
89 |     PP_VELOCITY_MAX         = (0x7024, np.float32,  "vel_max")
90 |     PP_ACCELERATION_TARGET  = (0x7025, np.float32,  "acc_set")
91 |     EPSCAN_TIME             = (0x7026, np.uint16,   "EPScan_time")
92 |     CAN_TIMEOUT             = (0x7028, np.uint32,   "canTimeout")
93 |     ZERO_STATE              = (0x7029, np.uint8,    "zero_sta")
94 | 


--------------------------------------------------------------------------------
/arduino/ANGLE_CONTROL_GUIDE.md:
--------------------------------------------------------------------------------
  1 | # 🎯 0-360度角度控制快速指南
  2 | 
  3 | ## 🚀 立即开始使用角度控制
  4 | 
  5 | ### 📖 角度输入系统
  6 | 
  7 | 现在 `simple_joint_control` 项目支持 **0-360度角度输入**，让控制更加直观！
  8 | 
  9 | ### 🎪 角度对应关系
 10 | ```
 11 | 角度     位置说明
 12 | 0°     = 起始位置/正前方
 13 | 45°    = 东北方向
 14 | 90°    = 正右方
 15 | 135°   = 东南方向
 16 | 180°   = 正后方
 17 | 225°   = 西南方向
 18 | 270°   = 正左方
 19 | 315°   = 西北方向
 20 | 360°   = 回到起始位置(0°)
 21 | ```
 22 | 
 23 | ## 📱 快速命令测试
 24 | 
 25 | ### 基础角度测试
 26 | ```bash
 27 | # 打开串口监视器，依次输入：
 28 | 0        # 回到起始位置
 29 | 90       # 转到90度（正右方）
 30 | 180      # 转到180度（正后方）
 31 | 270      # 转到270度（正左方）
 32 | 0        # 回到0度
 33 | stop     # 停止
 34 | ```
 35 | 
 36 | ### 精确角度测试
 37 | ```bash
 38 | 22.5     # 22.5度
 39 | 67.5     # 67.5度
 40 | 112.5    # 112.5度
 41 | 157.5    # 157.5度
 42 | 0        # 回到起点
 43 | ```
 44 | 
 45 | ### 超大角度测试
 46 | ```bash
 47 | 450      # 450度 → 自动转换为90度
 48 | 720      # 720度 → 自动转换为0度
 49 | -90      # -90度 → 按弧度处理
 50 | ```
 51 | 
 52 | ## 🧠 智能识别规则
 53 | 
 54 | 程序会智能判断输入类型：
 55 | 
 56 | | 输入范围 | 识别结果 | 示例 |
 57 | |----------|----------|------|
 58 | | **0-360** | 角度 ✅ | `90` → 90度 |
 59 | | **小弧度值** | 弧度 | `1.57` → π/2弧度 |
 60 | | **>360度** | 归一化角度 | `450` → 90度 |
 61 | | **负数** | 弧度 | `-1.57` → -π/2弧度 |
 62 | 
 63 | ## 📊 实时反馈格式
 64 | 
 65 | 程序会显示两种角度格式：
 66 | ```
 67 | 位置: 1.571 rad (90.0° -> 0-360°显示: 90.0°), 速度: 0.15 rad/s
 68 |          │      │                 └─ 0-360度显示角度（直观）
 69 |          │      └─ 实际计算角度
 70 |          └─ 弧度值（内部计算）
 71 | ```
 72 | 
 73 | ## 🔧 使用步骤
 74 | 
 75 | ### 1. 上传程序
 76 | ```bash
 77 | 1. Arduino IDE打开: simple_joint_control.ino
 78 | 2. 选择开发板: ESP32 Dev Module
 79 | 3. 上传程序
 80 | ```
 81 | 
 82 | ### 2. 开始控制
 83 | ```bash
 84 | 1. 打开串口监视器（115200）
 85 | 2. 等待"就绪！支持0-360度角度输入"提示
 86 | 3. 直接输入角度数值开始控制
 87 | ```
 88 | 
 89 | ### 3. 角度控制示例
 90 | ```bash
 91 | # 测试四个基本方向：
 92 | 0      # 前方
 93 | 90     # 右方
 94 | 180    # 后方
 95 | 270    # 左方
 96 | 0      # 回到前方
 97 | ```
 98 | 
 99 | ## ⚡ 高级用法
100 | 
101 | ### 1. 精确定位
102 | ```bash
103 | # 使用小数获得更精确的角度：
104 | 33.75   # 33.75度
105 | 146.25  # 146.25度
106 | 292.5   # 292.5度
107 | ```
108 | 
109 | ### 2. 连续运动
110 | ```bash
111 | # 创建连续运动序列：
112 | 0 → 45 → 90 → 135 → 180 → 225 → 270 → 315 → 0
113 | # 每2秒输入一个角度
114 | ```
115 | 
116 | ### 3. 范围测试
117 | ```bash
118 | # 测试电机角度范围：
119 | 0 → 360    # 完整一圈
120 | 180 → 0    # 半圈返回
121 | 90 → 270   # 转半圈
122 | ```
123 | 
124 | ## 🛡️ 安全提示
125 | 
126 | ### 自动保护
127 | - ✅ 角度超出范围会自动限制
128 | - ✅ 大于360度自动归一化
129 | - ✅ 随时可以输入"stop"紧急停止
130 | 
131 | ### 建议做法
132 | - 🔸 从小角度开始测试
133 | - 🔸 观察电机运动是否平稳
134 | - 🔸 确认机械安装牢固
135 | 
136 | ## 🎪 实际应用场景
137 | 
138 | ### 1. 机器人关节
139 | ```bash
140 | # 模拟关节运动：
141 | 0      # 初始位置
142 | 45     # 抬起45度
143 | 90     # 抬起90度
144 | 45     # 放下45度
145 | 0      # 回到初始
146 | ```
147 | 
148 | ### 2. 摄像头云台
149 | ```bash
150 | # 控制摄像头方向：
151 | 0      # 正前方
152 | 45     # 右前方45度
153 | 90     # 正右方
154 | 315    # 左前方45度
155 | 0      # 回到正前方
156 | ```
157 | 
158 | ### 3. 指针指示
159 | ```bash
160 | # 模拟仪表指针：
161 | 0      # 指向0度
162 | 90     # 指向3点钟方向
163 | 180    # 指向6点钟方向
164 | 270    # 指向9点钟方向
165 | 0      # 回到12点钟方向
166 | ```
167 | 
168 | ## 🔍 故障排除
169 | 
170 | ### 问题：角度不准确
171 | ```bash
172 | 解决方案：
173 | 1. 重新设零：输入"zero"
174 | 2. 检查机械零点是否正确
175 | 3. 测试0度和180度是否准确
176 | ```
177 | 
178 | ### 问题：电机振动
179 | ```bash
180 | 这个程序已经优化防振动
181 | 如果仍有振动：
182 | 1. 检查机械安装
183 | 2. 确认电源稳定
184 | 3. 输入"stop"停止测试
185 | ```
186 | 
187 | ### 问题：角度识别错误
188 | ```bash
189 | 如果输入弧度被误识别为角度：
190 | 1. 使用小数弧度值：1.57, 3.14
191 | 2. 明确输入：1.5708 表示π/2
192 | 3. 查看串口输出的识别结果
193 | ```
194 | 
195 | ---
196 | 
197 | **总结**：现在可以使用0-360度角度直接控制电机，非常直观！推荐优先使用角度输入模式。
198 | 
199 | **文件位置**：`/mi_arduino/simple_joint_control/simple_joint_control.ino`


--------------------------------------------------------------------------------
/scripts/setup.sh:
--------------------------------------------------------------------------------
  1 | #!/bin/bash
  2 | # tianrking Control Environment Setup Script
  3 | # This script sets up the development environment for tianrking motor control
  4 | 
  5 | set -e
  6 | 
  7 | echo "🚀 Setting up tianrking Control development environment..."
  8 | 
  9 | # Colors for output
 10 | RED='\033[0;31m'
 11 | GREEN='\033[0;32m'
 12 | YELLOW='\033[1;33m'
 13 | BLUE='\033[0;34m'
 14 | NC='\033[0m' # No Color
 15 | 
 16 | # Function to print colored output
 17 | print_status() {
 18 |     echo -e "${GREEN}[✓]${NC} $1"
 19 | }
 20 | 
 21 | print_warning() {
 22 |     echo -e "${YELLOW}[⚠]${NC} $1"
 23 | }
 24 | 
 25 | print_error() {
 26 |     echo -e "${RED}[✗]${NC} $1"
 27 | }
 28 | 
 29 | print_info() {
 30 |     echo -e "${BLUE}[i]${NC} $1"
 31 | }
 32 | 
 33 | # Check if running as root
 34 | if [[ $EUID -eq 0 ]]; then
 35 |     print_warning "Running as root. Consider running as regular user with sudo."
 36 | fi
 37 | 
 38 | # Update system packages
 39 | print_info "Updating system packages..."
 40 | sudo apt-get update
 41 | 
 42 | # Install CAN utilities
 43 | print_info "Installing CAN utilities..."
 44 | sudo apt-get install -y can-utils
 45 | 
 46 | # Install Python development environment
 47 | print_info "Setting up Python environment..."
 48 | sudo apt-get install -y python3 python3-pip python3-venv
 49 | 
 50 | # Install C++ development environment
 51 | print_info "Setting up C++ environment..."
 52 | sudo apt-get install -y build-essential cmake pkg-config
 53 | 
 54 | # Install Rust
 55 | if ! command -v rustc &> /dev/null; then
 56 |     print_info "Installing Rust..."
 57 |     curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y
 58 |     source "$HOME/.cargo/env"
 59 | else
 60 |     print_status "Rust already installed"
 61 | fi
 62 | 
 63 | # Create Python virtual environment
 64 | if [ ! -d "venv" ]; then
 65 |     print_info "Creating Python virtual environment..."
 66 |     python3 -m venv venv
 67 | fi
 68 | 
 69 | # Activate virtual environment and install Python dependencies
 70 | print_info "Installing Python dependencies..."
 71 | source venv/bin/activate
 72 | pip install -r python/requirements.txt
 73 | 
 74 | # Build C++ components
 75 | print_info "Building C++ components..."
 76 | cd cpp
 77 | mkdir -p build
 78 | cd build
 79 | cmake ..
 80 | make
 81 | cd ../..
 82 | 
 83 | # Build Rust components
 84 | print_info "Building Rust components..."
 85 | cd rust
 86 | cargo build --release
 87 | cd ..
 88 | 
 89 | # Setup CAN interface
 90 | print_info "Setting up CAN interface..."
 91 | if ip link show can0 &> /dev/null; then
 92 |     print_status "CAN interface can0 already exists"
 93 | else
 94 |     print_warning "CAN interface can0 not found. You may need to configure your hardware."
 95 | fi
 96 | 
 97 | # Set permissions
 98 | print_info "Setting permissions..."
 99 | sudo usermod -a -G dialout $USER 2>/dev/null || true
100 | 
101 | # Create symbolic links for easy access
102 | print_info "Creating symbolic links..."
103 | sudo ln -sf $(pwd)/cpp/build/robstride-mit-position /usr/local/bin/robstride-cpp 2>/dev/null || true
104 | sudo ln -sf $(pwd)/rust/target/release/robstride-mit-position /usr/local/bin/robstride-rust 2>/dev/null || true
105 | 
106 | print_status "Setup completed successfully!"
107 | echo
108 | print_info "Next steps:"
109 | echo "1. Reboot your system or log out and log back in to apply group changes"
110 | echo "2. Connect your CAN hardware"
111 | echo "3. Run: sudo ip link set can0 type can bitrate 1000000"
112 | echo "4. Run: sudo ip link set up can0"
113 | echo "5. Test with: python python/src/position_control.py 11"
114 | echo
115 | print_warning "Remember to use sudo when accessing CAN devices!"


--------------------------------------------------------------------------------
/cpp/include/protocol.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * RobStride Motor Control Protocol Definitions
 3 |  * Communication protocol constants and data structures
 4 |  */
 5 | 
 6 | #ifndef PROTOCOL_H
 7 | #define PROTOCOL_H
 8 | 
 9 | #include <cstdint>
10 | #include <cstring>
11 | #include <cmath>
12 | 
13 | // --- Communication Types ---
14 | namespace CommType {
15 |     constexpr uint32_t GET_DEVICE_ID       = 0;
16 |     constexpr uint32_t OPERATION_CONTROL   = 1;
17 |     constexpr uint32_t OPERATION_STATUS    = 2;
18 |     constexpr uint32_t ENABLE              = 3;
19 |     constexpr uint32_t DISABLE             = 4;
20 |     constexpr uint32_t SET_ZERO_POSITION   = 6;
21 |     constexpr uint32_t SET_DEVICE_ID       = 7;
22 |     constexpr uint32_t READ_PARAMETER      = 17;
23 |     constexpr uint32_t WRITE_PARAMETER     = 18;
24 |     constexpr uint32_t FAULT_REPORT        = 21;
25 |     constexpr uint32_t SAVE_PARAMETERS     = 22;
26 |     constexpr uint32_t SET_BAUDRATE        = 23;
27 |     constexpr uint32_t ACTIVE_REPORT       = 24;
28 |     constexpr uint32_t SET_PROTOCOL        = 25;
29 | }
30 | 
31 | // --- Parameter IDs ---
32 | namespace ParamID {
33 |     constexpr uint16_t MECHANICAL_OFFSET       = 0x2005;
34 |     constexpr uint16_t MEASURED_POSITION       = 0x3016;
35 |     constexpr uint16_t MEASURED_VELOCITY       = 0x3017;
36 |     constexpr uint16_t MEASURED_TORQUE         = 0x302C;
37 |     constexpr uint16_t MODE                    = 0x7005;
38 |     constexpr uint16_t IQ_TARGET               = 0x7006;
39 |     constexpr uint16_t VELOCITY_TARGET         = 0x700A;
40 |     constexpr uint16_t TORQUE_LIMIT            = 0x700B;
41 |     constexpr uint16_t CURRENT_KP              = 0x7010;
42 |     constexpr uint16_t CURRENT_KI              = 0x7011;
43 |     constexpr uint16_t CURRENT_FILTER_GAIN     = 0x7014;
44 |     constexpr uint16_t POSITION_TARGET         = 0x7016;
45 |     constexpr uint16_t VELOCITY_LIMIT          = 0x7017;
46 |     constexpr uint16_t CURRENT_LIMIT           = 0x7018;
47 |     constexpr uint16_t MECHANICAL_POSITION     = 0x7019;
48 |     constexpr uint16_t IQ_FILTERED             = 0x701A;
49 |     constexpr uint16_t MECHANICAL_VELOCITY     = 0x701B;
50 |     constexpr uint16_t VBUS                    = 0x701C;
51 |     constexpr uint16_t POSITION_KP             = 0x701E;
52 |     constexpr uint16_t VELOCITY_KP             = 0x701F;
53 |     constexpr uint16_t VELOCITY_KI             = 0x7020;
54 |     constexpr uint16_t VELOCITY_FILTER_GAIN    = 0x7021;
55 |     constexpr uint16_t VEL_ACCELERATION_TARGET = 0x7022;
56 |     constexpr uint16_t PP_VELOCITY_MAX         = 0x7024;
57 |     constexpr uint16_t PP_ACCELERATION_TARGET  = 0x7025;
58 |     constexpr uint16_t EPSCAN_TIME             = 0x7026;
59 |     constexpr uint16_t CAN_TIMEOUT             = 0x7028;
60 |     constexpr uint16_t ZERO_STATE              = 0x7029;
61 | }
62 | 
63 | // --- Motor Model Scaling Tables (RS-03) ---
64 | namespace ModelScale {
65 |     // For MIT mode scaling
66 |     constexpr double POSITION = 4.0 * M_PI;    // ±4π radians
67 |     constexpr double VELOCITY = 50.0;          // ±50 rad/s
68 |     constexpr double TORQUE = 60.0;            // ±60 Nm
69 |     constexpr double KP = 5000.0;              // 0-5000 Nm/rad
70 |     constexpr double KD = 100.0;               // 0-100 Nm/rad/s
71 | }
72 | 
73 | // --- Control Modes ---
74 | namespace ControlMode {
75 |     constexpr int8_t MIT_MODE        = 0;      // MIT control mode
76 |     constexpr int8_t POSITION_MODE   = 1;      // Position control mode
77 |     constexpr int8_t SPEED_MODE      = 2;      // Speed control mode
78 |     constexpr int8_t TORQUE_MODE     = 3;      // Torque control mode
79 | }
80 | 
81 | // --- Utility Functions ---
82 | inline void pack_float_le(uint8_t* buf, float val) {
83 |     memcpy(buf, &val, sizeof(float));
84 | }
85 | 
86 | inline void pack_u16_le(uint8_t* buf, uint16_t val) {
87 |     buf[0] = val & 0xFF;
88 |     buf[1] = (val >> 8) & 0xFF;
89 | }
90 | 
91 | inline void pack_u16_be(uint8_t* buf, uint16_t val) {
92 |     buf[0] = (val >> 8) & 0xFF;
93 |     buf[1] = val & 0xFF;
94 | }
95 | 
96 | #endif // PROTOCOL_H


--------------------------------------------------------------------------------
/arduino/ANTI_VIBRATION_GUIDE.md:
--------------------------------------------------------------------------------
  1 | # 🔧 电机振动解决方案指南
  2 | 
  3 | ## 🌊 振动原因分析
  4 | 
  5 | ### 常见振动原因
  6 | 
  7 | 1. **控制参数过大**
  8 |    - KP（比例增益）过高 → 振荡
  9 |    - KD（微分增益）不合适 → 响应迟缓或振荡
 10 | 
 11 | 2. **速度变化过快**
 12 |    - 最大速度设置过高
 13 |    - 加速度无限制
 14 | 
 15 | 3. **控制模式切换不平滑**
 16 |    - 模式间直接切换
 17 |    - 参数重新设置时机不对
 18 | 
 19 | 4. **机械因素**
 20 |    - 负载不匹配
 21 |    - 安装不牢固
 22 | 
 23 | ## ✅ 解决方案
 24 | 
 25 | ### 方案1：使用简化关节控制程序（推荐）
 26 | 
 27 | **文件：`simple_joint_control.ino`**
 28 | 
 29 | 特点：
 30 | - ✅ 超级简单，专注位置控制
 31 | - ✅ 优化的控制参数，减少振动
 32 | - ✅ 支持弧度和角度输入
 33 | - ✅ 自动范围限制
 34 | 
 35 | **使用方法：**
 36 | ```cpp
 37 | // 直接输入位置值
 38 | 1.57          # 1.57弧度（90度）
 39 | 90            # 90度
 40 | -45           # -45度
 41 | stop          # 停止
 42 | zero          # 设零点
 43 | ```
 44 | 
 45 | ### 方案2：使用高级关节控制程序
 46 | 
 47 | **文件：`joint_position_control.ino`**
 48 | 
 49 | 特点：
 50 | - ✅ 完整的位置控制系统
 51 | - ✅ 详细的参数配置
 52 | - ✅ 实时状态监控
 53 | - ✅ 错误检测和处理
 54 | 
 55 | ## 🔧 关键参数优化
 56 | 
 57 | ### 减少振动的参数设置
 58 | 
 59 | ```cpp
 60 | // 1. 限制最大速度（重要！）
 61 | max_speed = 2.0;        // 从30降到2.0 rad/s
 62 | 
 63 | // 2. 降低位置控制增益
 64 | position_kp = 20.0;     // 从30降到20
 65 | 
 66 | // 3. 设置合适的速度控制参数
 67 | speed_kp = 10.0;        // 速度比例增益
 68 | speed_ki = 0.1;         // 速度积分增益
 69 | 
 70 | // 4. 限制电流
 71 | current_limit = 5.0;    // 限制最大电流
 72 | ```
 73 | 
 74 | ### 参数对振动的具体影响
 75 | 
 76 | | 参数 | 过高效果 | 过低效果 | 推荐值（关节控制） |
 77 | |------|----------|----------|-------------------|
 78 | | 最大速度 | 振动大 | 响应慢 | 2.0 rad/s |
 79 | | 位置KP | 振荡 | 精度差 | 15-25 |
 80 | | 速度KP | 超调 | 响应慢 | 5-15 |
 81 | | 速度KI | 振荡 | 稳态误差 | 0.05-0.2 |
 82 | 
 83 | ## 🎮 控制模式对比
 84 | 
 85 | ### 4种控制模式特点
 86 | 
 87 | | 模式 | 用途 | 振动情况 | 适用场景 |
 88 | |------|------|----------|----------|
 89 | | **位置模式** ⭐ | 精确定位 | 振动最小 | 关节控制 |
 90 | | **速度模式** | 连续旋转 | 中等 | 轮子控制 |
 91 | | **运控模式** | 复杂控制 | 振动较大 | 高级应用 |
 92 | | **电流模式** | 力控制 | 振动大 | 力控应用 |
 93 | 
 94 | ### 关节控制最佳实践：**位置模式 + 优化参数**
 95 | 
 96 | ## 🚀 快速测试步骤
 97 | 
 98 | ### 1. 使用简化程序测试
 99 | ```cpp
100 | // 上传 simple_joint_control.ino
101 | // 串口输入：
102 | 1.0    # 应该平稳转到1.0弧度
103 | -1.0   # 平稳转回来
104 | stop   # 立即停止
105 | ```
106 | 
107 | ### 2. 观察振动情况
108 | - ✅ 平滑无振动 → 参数合适
109 | - ⚠️ 轻微振动 → 降低速度
110 | - ❌ 明显振动 → 降低KP
111 | 
112 | ### 3. 进一步优化
113 | ```cpp
114 | // 如果还有振动，逐步降低：
115 | max_speed = 1.0;      // 降到1.0 rad/s
116 | position_kp = 15.0;   // 降到15
117 | ```
118 | 
119 | ## 📋 振动诊断流程
120 | 
121 | ### 步骤1：检查基础设置
122 | ```cpp
123 | 1. 确认使用位置模式
124 | 2. 检查最大速度设置
125 | 3. 验证机械安装
126 | ```
127 | 
128 | ### 步骤2：参数调试
129 | ```cpp
130 | // 从保守参数开始
131 | max_speed = 1.0;
132 | position_kp = 10.0;
133 | 
134 | // 逐步增加直到找到最佳点
135 | ```
136 | 
137 | ### 步骤3：实时监控
138 | ```cpp
139 | // 观察以下指标：
140 | - 超调量 < 5%
141 | - 稳定时间 < 2秒
142 | - 无持续振荡
143 | ```
144 | 
145 | ## 🛠️ 代码优化技巧
146 | 
147 | ### 1. 分步设置参数
148 | ```cpp
149 | // 错误做法：同时设置所有参数
150 | motor.Set_SingleParameter(LOC_REF, position);
151 | 
152 | // 正确做法：分步设置
153 | motor.Set_SingleParameter(LIMIT_SPD, max_speed);  // 先限速
154 | delay(20);
155 | motor.Set_SingleParameter(LOC_REF, position);      // 再设位置
156 | ```
157 | 
158 | ### 2. 添加延时缓冲
159 | ```cpp
160 | // 每个参数设置后加延时
161 | motor.Set_SingleParameter(LOC_KP, 20.0);
162 | delay(50);  // 重要：给电机处理时间
163 | ```
164 | 
165 | ### 3. 渐进式位置变化
166 | ```cpp
167 | // 大位置变化时分步进行
168 | float start_pos = current_position;
169 | float target_pos = desired_position;
170 | float steps = 10;
171 | 
172 | for (int i = 1; i <= steps; i++) {
173 |     float intermediate = start_pos + (target_pos - start_pos) * i / steps;
174 |     motor.Set_SingleParameter(LOC_REF, intermediate);
175 |     delay(100);  // 每步延时
176 | }
177 | ```
178 | 
179 | ## 📊 测试用例
180 | 
181 | ### 基础位置测试
182 | ```cpp
183 | // 测试序列
184 | 1.57 → 0 → -1.57 → 0
185 | // 期望：每个位置平稳到达，无振荡
186 | ```
187 | 
188 | ### 小步进测试
189 | ```cpp
190 | // 0.1弧度步进测试
191 | 0.0 → 0.1 → 0.2 → 0.3 → ... → 1.0
192 | // 期望：每步都平稳，无跳动
193 | ```
194 | 
195 | ### 响应时间测试
196 | ```cpp
197 | // 大位置变化
198 | 0.0 → 3.14
199 | // 期望：2-3秒内平稳到达，超调<5%
200 | ```
201 | 
202 | ## 🔍 故障排除
203 | 
204 | ### 振动仍然很大？
205 | 1. **进一步降低速度**：`max_speed = 0.5`
206 | 2. **降低控制增益**：`position_kp = 10`
207 | 3. **检查机械安装**：确保牢固无松动
208 | 4. **检查电源**：电压稳定，电流足够
209 | 
210 | ### 响应太慢？
211 | 1. **适当增加KP**：`position_kp = 25`
212 | 2. **增加最大速度**：`max_speed = 3.0`
213 | 3. **减少延时**：调整命令间延时
214 | 
215 | ### 精度不够？
216 | 1. **增加KP**：但要注意可能引起振动
217 | 2. **减少位置容差**：调整目标判断阈值
218 | 3. **检查编码器**：确认反馈准确
219 | 
220 | ---
221 | 
222 | **总结**：使用 `simple_joint_control.ino` 开始，这个程序已经针对振动问题进行了优化。如果仍有问题，逐步降低最大速度和KP参数。


--------------------------------------------------------------------------------
/arduino/QUICK_ID_SETUP_GUIDE.md:
--------------------------------------------------------------------------------
  1 | # ⚡ 电机ID快速设置指南
  2 | 
  3 | ## 🎯 5分钟快速设置多个电机ID
  4 | 
  5 | ### 准备工作
  6 | 1. **硬件连接**：
  7 |    - ESP32 + CAN模块
  8 |    - 小米电机 + 12-24V电源
  9 |    - CAN总线连接
 10 | 
 11 | 2. **软件准备**：
 12 |    - 上传 `sim_setup_motor.ino` 到ESP32
 13 |    - 打开串口监视器（115200波特率）
 14 | 
 15 | 3. **第一步测试**：
 16 |    ```bash
 17 |    test              # 测试CAN通信是否正常
 18 |    ```
 19 | 
 20 | ---
 21 | 
 22 | ## 🚀 方法一：批量设置（推荐）
 23 | 
 24 | ### 步骤1：开始批量设置
 25 | ```bash
 26 | batch 1
 27 | ```
 28 | 程序会提示：开始批量设置，从ID=1开始
 29 | 
 30 | ### 步骤2：依次连接电机
 31 | ```
 32 | === 设置电机 #1 ===
 33 | 请将电机连接到总线，然后输入任意键继续设置...
 34 | 或输入 'stop' 结束批量设置
 35 | ```
 36 | - **连接电机1** → 按任意键 → 自动设置为ID=1
 37 | - **连接电机2** → 按任意键 → 自动设置为ID=2
 38 | - **连接电机3** → 按任意键 → 自动设置为ID=3
 39 | - ... 继续直到所有电机设置完成
 40 | 
 41 | ### 步骤3：验证设置
 42 | ```bash
 43 | verify
 44 | ```
 45 | 输出示例：
 46 | ```
 47 | 验证结果：发现 3 个电机
 48 | 当前电机ID: 1, 2, 3
 49 | ✅ 电机数量充足，可以开始多电机控制
 50 | ```
 51 | 
 52 | ---
 53 | 
 54 | ## 🔧 方法二：精确控制设置
 55 | 
 56 | ### 设置单个电机
 57 | ```bash
 58 | # 1. 连接电机到总线
 59 | test              # 先测试通信
 60 | scan              # 查看当前ID，输出：发现电机ID: 7
 61 | 
 62 | # 2. 修改ID
 63 | set 7 1           # 将ID=7改为ID=1
 64 | 
 65 | # 3. 验证修改
 66 | verify            # 确认修改成功
 67 | ```
 68 | 
 69 | ### 批量修改
 70 | ```bash
 71 | # 设置机械臂6个关节
 72 | test              # 先测试通信
 73 | set 1 1           # 关节1 → ID=1 (换电机2)
 74 | set 1 2           # 关节2 → ID=2 (换电机3)
 75 | set 1 3           # 关节3 → ID=3 (换电机4)
 76 | set 1 4           # 关节4 → ID=4 (换电机5)
 77 | set 1 5           # 关节5 → ID=5 (换电机6)
 78 | set 1 6           # 关节6 → ID=6
 79 | 
 80 | # 最终验证
 81 | verify
 82 | ```
 83 | 
 84 | ---
 85 | 
 86 | ## 📊 典型应用场景
 87 | 
 88 | ### 场景1：3关节机械臂
 89 | ```bash
 90 | # 目标：3个关节，ID=1,2,3
 91 | test              # 测试通信
 92 | batch 3           # 批量设置
 93 | # 依次连接3个电机，程序自动设置为ID=1,2,3
 94 | verify            # 验证：发现3个电机，ID: 1,2,3
 95 | ```
 96 | 
 97 | ### 场景2：4轮驱动小车
 98 | ```bash
 99 | # 目标：4个轮子，ID=10,11,12,13
100 | test              # 测试通信
101 | set 1 10          # 前左轮
102 | set 1 11          # 前右轮
103 | set 1 12          # 后左轮
104 | set 1 13          # 后右轮
105 | verify            # 验证4个轮子电机
106 | ```
107 | 
108 | ### 场景3：云台系统
109 | ```bash
110 | # 目标：2轴云台，ID=20,21
111 | test              # 测试通信
112 | set 1 20          # 水平轴
113 | set 1 21          # 垂直轴
114 | scan 20 25        # 验证云台区域
115 | ```
116 | 
117 | ---
118 | 
119 | ## 🔍 故障排除
120 | 
121 | ### 问题1：test命令失败
122 | ```
123 | ❌ CAN通信失败！
124 | ```
125 | **解决方案**：
126 | 1. 检查CAN总线连接：CAN_H接CAN_H，CAN_L接CAN_L
127 | 2. 检查电机电源：12-24V，确保电源开启
128 | 3. 检查波特率：确保是1Mbps
129 | 4. 检查终端电阻：总线两端需要120Ω终端电阻
130 | 
131 | ### 问题2：scan扫描不到电机
132 | ```
133 | 未发现任何电机
134 | ```
135 | **解决方案**：
136 | 1. 先运行 `test` 命令检查基础通信
137 | 2. 只连接一个电机进行测试
138 | 3. 重新上电重启电机
139 | 
140 | ### 问题3：set命令修改失败
141 | ```
142 | ❌ 修改失败，请检查连接或重试
143 | ```
144 | **解决方案**：
145 | 1. 确保总线上只有一个电机
146 | 2. 重新运行 `test` 确认通信正常
147 | 3. 尝试 `reset 1` 重置电机后再设置
148 | 
149 | ### 问题4：verify验证结果不对
150 | ```
151 | 发现电机数量比预期少
152 | ```
153 | **解决方案**：
154 | 1. 可能存在ID冲突，重新设置ID
155 | 2. 逐一测试每个电机
156 | 3. 检查是否有电机未上电
157 | 
158 | ---
159 | 
160 | ## ⚠️ 重要提醒
161 | 
162 | ### ❌ 不能这样做
163 | ```bash
164 | # 错误：同时连接多个电机修改ID
165 | # 会导致多个电机同时响应，ID混乱
166 | ```
167 | 
168 | ### ✅ 正确做法
169 | ```bash
170 | # 正确：一次只连接一个电机
171 | # 每个电机单独设置ID
172 | # 设置完成后断开，连接下一个
173 | ```
174 | 
175 | ### 🔧 故障排除
176 | 
177 | #### 问题1：扫描不到电机
178 | ```bash
179 | # 检查清单：
180 | 1. CAN连接线是否正确
181 | 2. 电机电源是否开启(12-24V)
182 | 3. 终端电阻是否配置
183 | 4. 波特率是否为1Mbps
184 | ```
185 | 
186 | #### 问题2：ID修改失败
187 | ```bash
188 | reset 1               # 先重置电机
189 | set 1 2               # 重新尝试修改
190 | verify                 # 确认结果
191 | ```
192 | 
193 | #### 问题3：多个电机响应
194 | ```bash
195 | # 解决方案：
196 | 1. 断开所有电机连接
197 | 2. 只连接目标电机
198 | 3. 重新设置ID
199 | 4. 验证成功后再连接下一个
200 | ```
201 | 
202 | ---
203 | 
204 | ## 💡 实用技巧
205 | 
206 | ### 1. 快速测试
207 | ```bash
208 | scan 1 5              # 只扫描前5个ID，速度快
209 | ```
210 | 
211 | ### 2. ID规划
212 | ```bash
213 | # 推荐ID分配：
214 | 1-10   : 关节电机
215 | 11-20  : 轮子电机
216 | 21-30  : 执行器
217 | 31-50  : 备用
218 | ```
219 | 
220 | ### 3. 安全操作
221 | ```bash
222 | # 每次修改后立即验证
223 | set 1 5
224 | verify
225 | # 确认成功后再进行下一步
226 | ```
227 | 
228 | ### 4. 记录管理
229 | ```
230 | 电机分配表：
231 | ID=1 : 基座关节
232 | ID=2 : 肩部关节
233 | ID=3 : 肘部关节
234 | ID=4 : 腕部关节
235 | ```
236 | 
237 | ---
238 | 
239 | ## 🎯 成功标志
240 | 
241 | 设置成功的标志：
242 | ```
243 | ✓ 成功！电机ID已修改为: X
244 | 验证结果：发现 N 个电机
245 | 当前电机ID: 1, 2, 3, ...
246 | ✅ 电机数量充足，可以开始多电机控制
247 | ```
248 | 
249 | 看到这些输出说明电机ID设置成功，可以开始多电机控制系统开发了！
250 | 
251 | ---
252 | 
253 | **相关文件**：
254 | - `sim_setup_motor.ino` - ID设置主程序
255 | - `README.md` - 详细使用说明
256 | - `simple_joint_control.ino` - 多电机控制程序（设置完ID后使用）


--------------------------------------------------------------------------------
/python/README.md:
--------------------------------------------------------------------------------
  1 | # RobStride Control Python
  2 | 
  3 | Python 实现 RobStride 电机控制库，提供简单易用的 API 和丰富的功能。
  4 | 
  5 | ## 特性
  6 | 
  7 | - ✅ **MIT 模式位置控制**：高性能直接扭矩控制
  8 | - ✅ **速度控制**：精确的速度闭环控制
  9 | - ✅ **实时控制**：50-100Hz 控制频率
 10 | - ✅ **交互式界面**：友好的命令行界面
 11 | - ✅ **参数调整**：实时调整控制参数
 12 | - ✅ **状态监控**：实时显示电机状态
 13 | 
 14 | ## 安装
 15 | 
 16 | ### 环境要求
 17 | 
 18 | - Python 3.8+
 19 | - Linux 系统 (SocketCAN 支持)
 20 | - CAN 接口硬件
 21 | 
 22 | ### 安装依赖
 23 | 
 24 | ```bash
 25 | # 安装系统依赖
 26 | sudo apt-get install python3 python3-pip can-utils
 27 | 
 28 | # 克隆项目
 29 | git clone https://github.com/tianrking/robstride-control.git
 30 | cd robstride-control/python
 31 | 
 32 | # 安装 Python 依赖
 33 | pip install -r requirements.txt
 34 | 
 35 | # 或者使用虚拟环境
 36 | python3 -m venv venv
 37 | source venv/bin/activate
 38 | pip install -r requirements.txt
 39 | ```
 40 | 
 41 | ## 快速开始
 42 | 
 43 | ### 位置控制
 44 | 
 45 | ```bash
 46 | # 运行 MIT 位置控制
 47 | python3 src/position_control.py 11
 48 | ```
 49 | 
 50 | ### 速度控制
 51 | 
 52 | ```bash
 53 | # 运行速度控制
 54 | python3 src/speed_control.py 11
 55 | ```
 56 | 
 57 | ## API 使用
 58 | 
 59 | ### MIT 位置控制
 60 | 
 61 | ```python
 62 | from src.position_control import PositionControllerMIT
 63 | 
 64 | # 创建控制器
 65 | controller = PositionControllerMIT(motor_id=11)
 66 | 
 67 | # 连接电机
 68 | controller.connect()
 69 | 
 70 | # 设置位置 (角度)
 71 | controller.set_angle(90.0)
 72 | 
 73 | # 调整控制参数
 74 | controller.set_kp(30.0)  # 位置增益
 75 | controller.set_kd(0.5)   # 阻尼增益
 76 | 
 77 | # 交互式控制
 78 | controller.run_interactive()
 79 | ```
 80 | 
 81 | ### 速度控制
 82 | 
 83 | ```python
 84 | from src.speed_control import SpeedController
 85 | 
 86 | # 创建控制器
 87 | controller = SpeedController(motor_id=11)
 88 | 
 89 | # 连接电机
 90 | controller.connect()
 91 | 
 92 | # 设置速度 (rad/s)
 93 | controller.set_velocity(5.0)
 94 | 
 95 | # 交互式控制
 96 | controller.run_interactive()
 97 | ```
 98 | 
 99 | ## 交互式命令
100 | 
101 | ### 位置控制命令
102 | 
103 | - **数字输入**：设置目标角度（度）
104 |   ```
105 |   90     # 顺时针 90 度
106 |   -45    # 逆时针 45 度
107 |   0      # 零点位置
108 |   ```
109 | 
110 | - **参数调整**：
111 |   ```
112 |   kp 30  # 设置位置增益为 30
113 |   kd 0.8 # 设置阻尼增益为 0.8
114 |   ```
115 | 
116 | - **特殊命令**：
117 |   ```
118 |   home   # 回零位
119 |   status # 显示状态
120 |   params # 显示参数
121 |   quit   # 退出
122 |   ```
123 | 
124 | ### 速度控制命令
125 | 
126 | - **数字输入**：设置目标速度 (rad/s)
127 |   ```
128 |   5.0    # 正向 5 rad/s
129 |   -3.0   # 反向 3 rad/s
130 |   0      # 停止
131 |   ```
132 | 
133 | - **特殊命令**：
134 |   ```
135 |   stop   # 停止电机
136 |   status # 显示状态
137 |   quit   # 退出
138 |   ```
139 | 
140 | ## 控制模式
141 | 
142 | ### MIT 模式 (Mode 0)
143 | - 直接发送位置、速度、扭矩目标
144 | - 50Hz 控制频率
145 | - 适用于高性能机器人应用
146 | 
147 | ### 速度模式 (Mode 2)
148 | - 基于 VELOCITY_TARGET 参数
149 | - 20Hz 控制频率
150 | - 适用于需要精确速度控制的应用
151 | 
152 | ## 参数说明
153 | 
154 | ### 位置控制参数
155 | 
156 | | 参数 | 范围 | 说明 |
157 | |------|------|------|
158 | | Kp   | 0-500 | 位置增益，控制响应速度 |
159 | | Kd   | 0-5   | 阻尼增益，抑制震荡 |
160 | | 速度限制 | 0-50 rad/s | 最大速度限制 |
161 | | 扭矩限制 | 0-60 Nm | 最大扭矩限制 |
162 | 
163 | ### 速度控制参数
164 | 
165 | | 参数 | 范围 | 说明 |
166 | |------|------|------|
167 | | Kp   | 0-500 | 速度比例增益 |
168 | | Ki   | 0-100 | 速度积分增益 |
169 | | 速度限制 | 0-50 rad/s | 最大速度限制 |
170 | 
171 | ## 示例程序
172 | 
173 | ```bash
174 | # 基础使用示例
175 | python3 examples/basic_usage.py 11
176 | 
177 | # 高级控制示例
178 | python3 examples/advanced_control.py 11
179 | ```
180 | 
181 | ## 故障排除
182 | 
183 | ### 常见问题
184 | 
185 | 1. **找不到电机**
186 |    ```bash
187 |    # 检查 CAN 连接
188 |    sudo ip link show can0
189 | 
190 |    # 扫描电机
191 |    python3 -c "from robstride_dynamics import RobstrideBus; print(RobstrideBus.scan_channel('can0'))"
192 |    ```
193 | 
194 | 2. **权限错误**
195 |    ```bash
196 |    # 添加用户到 dialout 组
197 |    sudo usermod -a -G dialout $USER
198 |    # 重启或重新登录
199 |    ```
200 | 
201 | 3. **CAN 接口未设置**
202 |    ```bash
203 |    sudo ip link set can0 type can bitrate 1000000
204 |    sudo ip link set up can0
205 |    ```
206 | 
207 | ## 性能优化
208 | 
209 | ### 控制频率调整
210 | 
211 | ```python
212 | # 在控制循环中调整 sleep 时间
213 | time.sleep(0.02)  # 50Hz (默认)
214 | time.sleep(0.01)  # 100Hz (高性能)
215 | time.sleep(0.05)  # 20Hz (低负载)
216 | ```
217 | 
218 | ### 参数调优
219 | 
220 | 1. **从小参数开始**：Kp=10, Kd=0.1
221 | 2. **逐步增加 Kp**：直到响应快速但不震荡
222 | 3. **调整 Kd**：消除过冲和震荡
223 | 4. **检查温度**：确保电机不过热
224 | 
225 | ## 开发
226 | 
227 | ### 运行测试
228 | 
229 | ```bash
230 | # 安装开发依赖
231 | pip install pytest
232 | 
233 | # 运行测试
234 | pytest tests/
235 | ```
236 | 
237 | ### 代码格式化
238 | 
239 | ```bash
240 | # 安装格式化工具
241 | pip install black flake8
242 | 
243 | # 格式化代码
244 | black src/
245 | flake8 src/
246 | ```
247 | 
248 | ## 许可证
249 | 
250 | MIT License - 详见 [LICENSE](../LICENSE) 文件
251 | 
252 | ## 贡献
253 | 
254 | 欢迎提交 Issue 和 Pull Request！
255 | 
256 | ## 支持
257 | 
258 | - 📖 [完整文档](../docs/)
259 | - 🐛 [问题反馈](https://github.com/tianrking/robstride-control/issues)


--------------------------------------------------------------------------------
/arduino/CONFIG_GUIDE.md:
--------------------------------------------------------------------------------
  1 | # 🔧 电机ID配置功能使用指南
  2 | 
  3 | ## 🎯 功能概述
  4 | 
  5 | `simple_joint_control` 现在支持**动态电机ID配置**，无需重新编译程序即可控制不同ID的电机！
  6 | 
  7 | ## 🚀 快速使用
  8 | 
  9 | ### 1. 查看当前配置
 10 | ```bash
 11 | config
 12 | ```
 13 | **输出示例：**
 14 | ```
 15 | === 当前配置 ===
 16 | 电机ID: 1
 17 | 主机ID: 0
 18 | 最大速度: 2.0 rad/s
 19 | 位置控制KP: 20.0
 20 | ==================
 21 | ```
 22 | 
 23 | ### 2. 更改电机ID
 24 | ```bash
 25 | id 2
 26 | ```
 27 | **输出示例：**
 28 | ```
 29 | 电机ID已设置为: 2
 30 | 请输入 'restart' 重启电机以应用新ID
 31 | ```
 32 | 
 33 | ### 3. 重启电机应用新ID
 34 | ```bash
 35 | restart
 36 | ```
 37 | 
 38 | ### 4. 测试新ID电机
 39 | ```bash
 40 | 90      # 测试电机2是否响应
 41 | ```
 42 | 
 43 | ## 📋 完整配置流程
 44 | 
 45 | ### 场景1：首次使用电机ID=2
 46 | ```bash
 47 | # 1. 查看当前配置
 48 | config          # 确认当前是ID=1
 49 | 
 50 | # 2. 设置新ID
 51 | id 2            # 设置为电机ID=2
 52 | 
 53 | # 3. 确认设置
 54 | config          # 确认ID已改为2
 55 | 
 56 | # 4. 重启电机
 57 | restart         # 应用新配置
 58 | 
 59 | # 5. 测试控制
 60 | 90              # 电机2应该转到90度
 61 | 0               # 电机2应该回到0度
 62 | ```
 63 | 
 64 | ### 场景2：切换不同电机
 65 | ```bash
 66 | # 当前控制电机3，想切换到电机5
 67 | config          # 查看：当前ID=3
 68 | id 5            # 设置为ID=5
 69 | restart         # 重启电机
 70 | 180             # 测试电机5
 71 | ```
 72 | 
 73 | ### 场景3：恢复默认设置
 74 | ```bash
 75 | # 恢复到默认电机ID=1
 76 | id 1            # 设置为ID=1
 77 | restart         # 重启电机
 78 | ```
 79 | 
 80 | ## 💾 配置保存机制
 81 | 
 82 | ### 自动保存
 83 | - ✅ 配置更改后**自动保存**到ESP32的EEPROM
 84 | - ✅ **断电不丢失**，重启后自动加载保存的配置
 85 | - ✅ 首次运行自动使用默认配置（ID=1）
 86 | 
 87 | ### 配置内容
 88 | ```cpp
 89 | struct MotorConfig {
 90 |   uint8_t motor_id;        // 电机ID (1-127)
 91 |   uint8_t master_id;       // 主机ID (固定为0)
 92 |   float max_speed;         // 最大速度 (2.0 rad/s)
 93 |   float position_kp;       // 位置控制KP (20.0)
 94 |   bool initialized;        // 初始化标志
 95 | };
 96 | ```
 97 | 
 98 | ## 🔧 技术细节
 99 | 
100 | ### ID范围限制
101 | - **电机ID**: 1-127（小米电机标准范围）
102 | - **主机ID**: 0（固定，小米协议规定）
103 | - **自动验证**: 输入超出范围会拒绝设置
104 | 
105 | ### 配置验证
106 | ```bash
107 | # 测试无效ID
108 | id 0            # ❌ 错误：电机ID范围应为1-127
109 | id 128          # ❌ 错误：电机ID范围应为1-127
110 | id 5            # ✅ 成功：在有效范围内
111 | ```
112 | 
113 | ### 实时反馈
114 | - 状态显示中会显示当前控制的电机ID
115 | ```bash
116 | 电机2: 位置 1.571 rad (90.0°), 速度: 0.15 rad/s
117 |       ↑
118 |    当前电机ID
119 | ```
120 | 
121 | ## 🎪 实际应用场景
122 | 
123 | ### 1. 机器人多关节控制
124 | ```bash
125 | # 控制关节1（电机ID=1）
126 | id 1
127 | restart
128 | 0     # 回到起始位置
129 | 90    # 转到90度
130 | 
131 | # 控制关节2（电机ID=2）
132 | id 2
133 | restart
134 | 0     # 回到起始位置
135 | 45    # 转到45度
136 | 
137 | # 控制关节3（电机ID=3）
138 | id 3
139 | restart
140 | 0     # 回到起始位置
141 | 180   # 转到180度
142 | ```
143 | 
144 | ### 2. 多设备测试
145 | ```bash
146 | # 测试电机1
147 | id 1 && restart && 90
148 | 
149 | # 测试电机2
150 | id 2 && restart && 90
151 | 
152 | # 测试电机3
153 | id 3 && restart && 90
154 | ```
155 | 
156 | ### 3. 设备维护
157 | ```bash
158 | # 维修时快速切换测试电机
159 | config          # 查看当前配置
160 | id 10           # 切换到测试电机
161 | restart         # 应用配置
162 | 90              # 测试功能
163 | ```
164 | 
165 | ## 🛡️ 安全特性
166 | 
167 | ### 配置保护
168 | - ✅ **自动验证**：检查配置有效性
169 | - ✅ **备份机制**：首次运行自动创建默认配置
170 | - ✅ **错误恢复**：配置损坏时自动使用默认值
171 | 
172 | ### 使用安全
173 | - ✅ **即时反馈**：所有操作都有明确的成功/失败反馈
174 | - ✅ **确认机制**：更改ID后需要重启才生效，避免误操作
175 | - ✅ **状态显示**：实时显示当前控制的电机ID
176 | 
177 | ## 🔍 故障排除
178 | 
179 | ### 问题1：配置更改无效
180 | ```bash
181 | # 症状：id 2 后电机还是按ID=1响应
182 | # 解决：必须输入restart重启电机
183 | id 2
184 | restart
185 | ```
186 | 
187 | ### 问题2：配置丢失
188 | ```bash
189 | # 症状：重启后配置回到默认
190 | # 解决：重新设置，EEPROM会自动保存
191 | id 2
192 | restart
193 | ```
194 | 
195 | ### 问题3：无效ID被接受
196 | ```bash
197 | # 痋状：id 0 或 id 128 被接受
198 | # 解决：程序会自动拒绝，显示错误信息
199 | ❌ 电机ID范围应为1-127
200 | ```
201 | 
202 | ### 问题4：EEPROM错误
203 | ```bash
204 | # 症状：首次运行显示配置错误
205 | # 解决：程序会自动使用默认配置并保存
206 | 首次运行或配置无效，使用默认配置
207 | ```
208 | 
209 | ## 💡 最佳实践
210 | 
211 | ### 1. 定期检查配置
212 | ```bash
213 | # 在重要操作前检查配置
214 | config
215 | ```
216 | 
217 | ### 2. 谨试更改ID
218 | ```bash
219 | # 更改ID后立即重启测试
220 | id X
221 | restart
222 | 90      # 测试新ID电机是否响应
223 | ```
224 | 
225 | ### 3. 记录电机分配
226 | ```
227 | 电机ID分配表：
228 | ID=1 : 左臂关节
229 | ID=2 : 右臂关节
230 | ID=3 : 头部关节
231 | ...
232 | ```
233 | 
234 | ### 4. 备份重要配置
235 | ```bash
236 | # 记录当前配置信息
237 | 电机ID: 1
238 | 最大速度: 2.0
239 | 位置KP: 20.0
240 | ```
241 | 
242 | ## 🎯 高级用法
243 | 
244 | ### 批量控制脚本
245 | ```bash
246 | # 可以编写脚本顺序控制多个电机
247 | # pseudo code
248 | for motor_id in [1, 2, 3]:
249 |   send "id " + motor_id
250 |   send "restart"
251 |   send "90"
252 |   wait 3000
253 |   send "0"
254 |   wait 2000
255 | ```
256 | 
257 | ### 配置预设
258 | ```bash
259 | # 高速配置（用于测试）
260 | id 1 && restart && speed 5.0
261 | 
262 | # 精密配置（用于精密控制）
263 | id 1 && restart && speed 1.0
264 | ```
265 | 
266 | ---
267 | 
268 | **总结**：现在您可以轻松控制不同ID的电机，无需重新编译程序！配置会自动保存，使用非常方便。
269 | 
270 | **文件位置**：`/mi_arduino/simple_joint_control/simple_joint_control.ino`


--------------------------------------------------------------------------------
/arduino/simple_joint_control/README.md:
--------------------------------------------------------------------------------
  1 | # 简化关节控制程序
  2 | 
  3 | **最简单的关节位置控制，支持0-360度角度输入，专治振动**
  4 | 
  5 | ## 🎯 项目特点
  6 | 
  7 | - ✅ **0-360度角度输入**（推荐！），超级直观
  8 | - ✅ **默认控制电机ID=11**，可动态修改
  9 | - ✅ **动态电机ID配置**，无需重新编译
 10 | - ✅ 配置自动保存，断电不丢失
 11 | - ✅ 超级简单，专注核心功能
 12 | - ✅ 最小振动，最优参数
 13 | - ✅ 智能识别角度/弧度输入
 14 | - ✅ 即插即用
 15 | 
 16 | ## ⚙️ 默认配置
 17 | 
 18 | ```cpp
 19 | 电机ID: 11          # 默认控制电机ID=11
 20 | 主机ID: 0
 21 | 最大速度: 2.0 rad/s
 22 | 位置KP: 30.0
 23 | 转矩限制: 12.0 Nm
 24 | ```
 25 | 
 26 | ## 📱 控制命令
 27 | 
 28 | ### 🎯 角度输入（推荐使用0-360度）
 29 | ```bash
 30 | 0              # 0度（正前方/起始位置）
 31 | 90             # 顺时针90度
 32 | 180            # 顺时针180度
 33 | 270            # 顺时针270度
 34 | 360            # 回到0度
 35 | 45             # 顺时针45度
 36 | 135            # 顺时针135度
 37 | ```
 38 | 
 39 | ### 📐 弧度输入（支持小弧度值）
 40 | ```bash
 41 | 1.57           # π/2弧度（90度）
 42 | 3.14           # π弧度（180度）
 43 | -1.57          # -π/2弧度（-90度）
 44 | 0              # 零点位置
 45 | ```
 46 | 
 47 | ### 🔧 控制命令
 48 | ```bash
 49 | stop           # 立即停止
 50 | zero           # 设当前位置为零点
 51 | id X           # 设置电机ID为X (1-127)，默认为11
 52 | config         # 显示当前配置
 53 | restart        # 重启电机（更改ID后使用）
 54 | ```
 55 | 
 56 | **注意**：程序默认控制电机ID=11，如果您的电机ID不是11，请使用 `id X` 命令修改
 57 | 
 58 | ## 🎪 角度系统说明
 59 | 
 60 | ### 角度对应关系
 61 | ```
 62 | 0°   = 起始位置/正前方
 63 | 90°  = 顺时针转90度
 64 | 180° = 顺时针转180度
 65 | 270° = 顺时针转270度
 66 | 360° = 回到起始位置（0°）
 67 | ```
 68 | 
 69 | ### 智能识别规则
 70 | - **0-360范围**：自动识别为角度 ✅
 71 | - **小弧度值**：自动识别为弧度
 72 | - **大于360度**：归一化到0-360度范围
 73 | 
 74 | ## 🔧 优化参数
 75 | 
 76 | ```cpp
 77 | 最大速度: 2.0 rad/s        // 减少振动
 78 | 位置KP:   20.0             // 保守增益
 79 | 速度KP:   10.0             // 稳定响应
 80 | ```
 81 | 
 82 | ## 🚀 快速开始
 83 | 
 84 | 1. **上传程序**
 85 |    - 打开 `simple_joint_control.ino`
 86 |    - 选择ESP32开发板
 87 |    - 上传程序
 88 | 
 89 | 2. **开始控制**
 90 |    - 打开串口监视器（115200）
 91 |    - 直接输入位置数值
 92 |    - 观察电机平稳运动
 93 | 
 94 | ## 📊 实时反馈
 95 | 
 96 | ```
 97 | 电机11: 位置 1.570 rad (90.0° -> 0-360°显示: 90.0°), 速度: 0.15 rad/s
 98 | 电机11: 位置 1.571 rad (90.1° -> 0-360°显示: 90.1°), 速度: 0.05 rad/s
 99 | 电机11: 位置 1.571 rad (90.1° -> 0-360°显示: 90.1°), 速度: 0.00 rad/s ✓ 到达
100 | ```
101 | 
102 | ## 🎪 使用示例
103 | 
104 | ### 角度控制测试（推荐）
105 | ```bash
106 | 0       # 0度（起始位置）
107 | 90      # 转到90度
108 | 180     # 转到180度
109 | 270     # 转到270度
110 | 360     # 回到0度
111 | 45      # 转到45度
112 | 135     # 转到135度
113 | stop    # 停止
114 | ```
115 | 
116 | ### 精确角度测试
117 | ```bash
118 | 22.5    # 22.5度
119 | 67.5    # 67.5度
120 | 112.5   # 112.5度
121 | 202.5   # 202.5度
122 | 0       # 回到0度
123 | ```
124 | 
125 | ### 弧度控制（兼容模式）
126 | ```bash
127 | 1.57    # π/2弧度（自动识别为90度）
128 | 3.14    # π弧度（自动识别为180度）
129 | -1.57   # -π/2弧度（保持负值）
130 | 0       # 零点
131 | ```
132 | 
133 | ### 电机ID配置（新增功能）
134 | ```bash
135 | config          # 查看当前配置（默认ID=11）
136 | id 2            # 设置电机ID为2
137 | config          # 再次查看，确认ID已更改
138 | restart         # 重启电机应用新ID
139 | 90              # 测试：新ID电机转到90度
140 | ```
141 | 
142 | ### 配置保存说明
143 | ```bash
144 | - 配置自动保存到EEPROM
145 | - 断电后配置不会丢失
146 | - 首次运行使用默认配置（ID=11）
147 | - 可随时更改电机ID（1-127）
148 | ```
149 | 
150 | ### 如果您的电机ID不是11
151 | ```bash
152 | # 假设您的电机实际ID是7
153 | config          # 查看当前配置：ID=11
154 | id 7            # 修改为ID=7
155 | restart         # 重启应用新ID
156 | 90              # 测试控制电机ID=7
157 | ```
158 | 
159 | ## ⚡ 优势
160 | 
161 | 1. **🎯 角度输入**：0-360度，超级直观
162 | 2. **🔧 动态配置**：运行时更改电机ID，无需重新编译
163 | 3. **💾 配置保存**：自动保存到EEPROM，断电不丢失
164 | 4. **🧠 智能识别**：自动区分弧度/角度
165 | 5. **✨ 无振动**：经过优化的控制参数
166 | 6. **⚡ 响应快**：直接控制，无延迟
167 | 7. **🔒 稳定可靠**：简单就是稳定
168 | 
169 | ## 🛡️ 安全特性
170 | 
171 | - **自动范围限制**：-12.5 到 12.5 弧度
172 | - **角度归一化**：大于360度自动转换
173 | - **智能识别**：0-360度优先识别为角度
174 | - **动态ID设置**：1-127范围内任意设置
175 | - **配置验证**：自动检查配置有效性
176 | - **紧急停止**：随时输入"stop"立即停止
177 | - **实时监控**：显示0-360度角度反馈和当前电机ID
178 | 
179 | ## 📋 适用场景
180 | 
181 | - 🤖 机器人关节控制
182 | - 🎯 精确角度定位
183 | - ⚡ 快速原型开发
184 | - 👨‍🏫 教学演示
185 | - 🔬 测试验证
186 | 
187 | ## 💡 使用技巧
188 | 
189 | 1. **🎯 角度优先**：输入0-360范围内的数字会自动识别为角度
190 | 2. **🔧 ID配置**：使用"config"查看配置，"id X"更改电机ID
191 | 3. **📍 精确定位**：使用小数点位数获得更高精度，如22.5度
192 | 4. **⚡ 快速停止**：随时输入"stop"立即停止
193 | 5. **🔄 重新设零**：在任意位置输入"zero"设为新零点
194 | 6. **🔢 大角度处理**：输入450度会自动转换为90度
195 | 7. **💾 配置保存**：更改的电机ID会自动保存，断电不丢失
196 | 
197 | ## 🔧 故障排除
198 | 
199 | ### 电机不动？
200 | - 检查CAN连接
201 | - 确认电机电源
202 | - 查看串口初始化信息
203 | - 检查电机ID是否正确（使用"config"查看）
204 | - **确认电机实际ID是否为11**，如果不是请使用 `id X` 命令修改
205 | 
206 | ### 更改电机ID后没反应？
207 | - 更改ID后必须输入"restart"重启电机
208 | - 确认电机ID在1-127范围内
209 | - 使用"config"确认设置是否生效
210 | 
211 | ### 配置丢失？
212 | - 首次运行会自动使用默认配置（ID=11）
213 | - 检查EEPROM是否正常工作
214 | - 重新设置后会自动保存
215 | 
216 | ### 默认控制ID=11但电机不是11？
217 | ```bash
218 | config          # 查看当前配置
219 | id X            # X是您的电机实际ID
220 | restart         # 重启应用
221 | ```
222 | 
223 | ### 振动明显？
224 | - 这是经过优化的程序，正常情况下振动很小
225 | - 如果仍有振动，检查机械安装是否牢固
226 | 
227 | ### 位置不准确？
228 | - 重新设零：输入"zero"
229 | - 检查是否有机械间隙
230 | - 确认电机ID对应的电机是否正确
231 | 
232 | ---
233 | 
234 | **提示**：如果需要更复杂的功能，请使用 `joint_position_control` 项目


--------------------------------------------------------------------------------
/arduino/PROJECTS_OVERVIEW.md:
--------------------------------------------------------------------------------
  1 | # 📚 ESP32小米电机控制项目集合
  2 | 
  3 | ## 🎯 项目总览
  4 | 
  5 | 本项目包含3个独立的Arduino项目，专为ESP32控制小米电机设计，涵盖从基础到高级的各种应用场景。
  6 | 
  7 | ---
  8 | 
  9 | ## 📁 项目结构
 10 | 
 11 | ```
 12 | mi_arduino/
 13 | ├── mi_motor_control/          # 🔧 基础电机控制
 14 | ├── joint_position_control/    # 🎯 关节位置控制
 15 | ├── simple_joint_control/      # ⚡ 简化关节控制
 16 | ├── ANTI_VIBRATION_GUIDE.md    # 📖 振动解决方案
 17 | └── PROJECTS_OVERVIEW.md       # 📋 本文件
 18 | ```
 19 | 
 20 | ---
 21 | 
 22 | ## 🚀 三大项目对比
 23 | 
 24 | | 项目 | 复杂度 | 振动控制 | 适用场景 | 特点 |
 25 | |------|--------|----------|----------|------|
 26 | | **mi_motor_control** | ⭐⭐⭐ | 基础 | 多模式演示 | 完整功能展示 |
 27 | | **joint_position_control** | ⭐⭐⭐⭐ | 优秀 | 专业关节控制 | 串口透传，状态监控 |
 28 | | **simple_joint_control** | ⭐ | **最佳** | 快速原型 | 超简单，无振动 |
 29 | 
 30 | ---
 31 | 
 32 | ## 📋 选择指南
 33 | 
 34 | ### 🏃‍♂️ 快速开始 → **simple_joint_control**
 35 | ```
 36 | 优势：最简单，直接输入数字即可控制
 37 | 使用：输入 1.57 或 90 就能转到对应位置
 38 | ```
 39 | 
 40 | ### 🎯 专业应用 → **joint_position_control**
 41 | ```
 42 | 优势：功能完整，专业关节控制
 43 | 使用：支持 pos 1.57, angle 90, status 等命令
 44 | ```
 45 | 
 46 | ### 🔧 功能演示 → **mi_motor_control**
 47 | ```
 48 | 优势：展示所有控制模式
 49 | 使用：自动演示速度、位置、运控模式
 50 | ```
 51 | 
 52 | ---
 53 | 
 54 | ## 📱 各项目详细说明
 55 | 
 56 | ### 1. 📊 mi_motor_control - 基础电机控制
 57 | 
 58 | **用途**：学习小米电机的基本控制方法
 59 | 
 60 | **功能**：
 61 | - ✅ 4种控制模式演示
 62 | - ✅ 自动测试序列
 63 | - ✅ 串口命令控制
 64 | - ✅ 实时数据监控
 65 | 
 66 | **命令示例**：
 67 | ```bash
 68 | speed 2.0     # 设置速度
 69 | pos 1.57      # 设置位置
 70 | enable        # 使能电机
 71 | help          # 查看帮助
 72 | ```
 73 | 
 74 | **适用**：学习、测试、演示
 75 | 
 76 | ---
 77 | 
 78 | ### 2. 🎯 joint_position_control - 关节位置控制
 79 | 
 80 | **用途**：专业的关节电机控制应用
 81 | 
 82 | **功能**：
 83 | - ✅ 专用的位置控制模式
 84 | - ✅ 串口透传命令
 85 | - ✅ 平滑运动优化
 86 | - ✅ 详细状态监控
 87 | - ✅ 错误检测保护
 88 | 
 89 | **命令示例**：
 90 | ```bash
 91 | pos 1.57       # 转到1.57弧度
 92 | angle 90       # 转到90度
 93 | speed 2.0      # 设置最大速度
 94 | status         # 显示状态
 95 | ```
 96 | 
 97 | **适用**：机器人关节、机械臂、精密定位
 98 | 
 99 | ---
100 | 
101 | ### 3. ⚡ simple_joint_control - 简化关节控制 **(强烈推荐)**
102 | 
103 | **用途**：最简单的关节控制，专治振动
104 | 
105 | **功能**：
106 | - ✅ 超级简单的操作
107 | - ✅ 最优防振动参数
108 | - ✅ 直接数字输入
109 | - ✅ 自动弧度/角度识别
110 | 
111 | **命令示例**：
112 | ```bash
113 | 1.57           # 1.57弧度（90度）
114 | 90             # 90度
115 | -45            # -45度
116 | stop           # 停止
117 | ```
118 | 
119 | **适用**：快速原型、教学、简单应用
120 | 
121 | ---
122 | 
123 | ## 🔧 硬件要求
124 | 
125 | ### 通用硬件
126 | - ESP32开发板
127 | - CAN模块（支持1Mbps）
128 | - 小米电机（CAN ID=1）
129 | - 12-24V电源
130 | 
131 | ### 接线图
132 | ```
133 | ESP32      CAN模块    小米电机
134 | GPIO5  ←→ TX   ←→ CAN_H
135 | GPIO4  ←→ RX   ←→ CAN_L
136 | 3.3V   ←→ VCC  ←→ 12-24V
137 | GND    ←→ GND  ←→ GND
138 | ```
139 | 
140 | ---
141 | 
142 | ## 🚀 快速上手步骤
143 | 
144 | ### 1. 环境准备
145 | ```bash
146 | 1. Arduino IDE安装ESP32开发板支持
147 | 2. 安装库：ESP32-TWAI-CAN
148 | 3. 选择开发板：ESP32 Dev Module
149 | ```
150 | 
151 | ### 2. 选择项目
152 | ```bash
153 | 初学者：simple_joint_control
154 | 专业应用：joint_position_control
155 | 学习测试：mi_motor_control
156 | ```
157 | 
158 | ### 3. 上传运行
159 | ```bash
160 | 1. 打开对应目录的.ino文件
161 | 2. 连接ESP32，选择串口
162 | 3. 上传程序
163 | 4. 打开串口监视器（115200）
164 | 5. 开始输入命令控制电机
165 | ```
166 | 
167 | ---
168 | 
169 | ## 🎪 控制模式说明
170 | 
171 | ### 4种控制模式特点
172 | 
173 | | 模式 | 代码 | 用途 | 振动 | 项目支持 |
174 | |------|------|------|------|----------|
175 | | **位置模式** | POS_MODE | 精确定位 | 最小 | 所有项目 |
176 | | **速度模式** | SPEED_MODE | 连续旋转 | 中等 | mi_motor_control |
177 | | **运控模式** | CTRL_MODE | 复杂控制 | 较大 | mi_motor_control |
178 | | **电流模式** | CUR_MODE | 力控制 | 较大 | mi_motor_control |
179 | 
180 | ### 关节控制最佳实践
181 | **推荐使用位置模式 + 优化参数**
182 | 
183 | ---
184 | 
185 | ## 🛠️ 振动解决方案
186 | 
187 | ### 根本原因
188 | 1. 控制参数过大
189 | 2. 速度变化过快
190 | 3. 模式切换不平滑
191 | 
192 | ### 解决方法
193 | 1. **使用优化程序**：`simple_joint_control`
194 | 2. **降低最大速度**：2.0 rad/s以下
195 | 3. **减小控制增益**：KP=15-25
196 | 4. **增加缓冲延时**：每个命令间20ms
197 | 
198 | ### 详细指南
199 | 📖 参考：`ANTI_VIBRATION_GUIDE.md`
200 | 
201 | ---
202 | 
203 | ## 📊 性能对比
204 | 
205 | | 项目 | 响应速度 | 平稳性 | 易用性 | 功能性 |
206 | |------|----------|--------|--------|--------|
207 | | mi_motor_control | 快 | 中等 | 中等 | ⭐⭐⭐⭐⭐ |
208 | | joint_position_control | 中等 | 优秀 | 良好 | ⭐⭐⭐⭐ |
209 | | simple_joint_control | 中等 | **最佳** | **最佳** | ⭐⭐⭐ |
210 | 
211 | ---
212 | 
213 | ## 🔍 故障排除
214 | 
215 | ### 常见问题
216 | 1. **编译错误**：检查ESP32开发板和库安装
217 | 2. **通信失败**：检查CAN连接和波特率
218 | 3. **电机不响应**：检查电源和使能状态
219 | 4. **振动问题**：使用`simple_joint_control`
220 | 
221 | ### 调试步骤
222 | 1. 先用`mi_motor_control`测试基础功能
223 | 2. 再用`simple_joint_control`验证防振动
224 | 3. 最后用`joint_position_control`开发应用
225 | 
226 | ---
227 | 
228 | ## 💡 使用建议
229 | 
230 | ### 🎯 开发流程
231 | ```bash
232 | 1. 测试阶段：mi_motor_control
233 | 2. 验证阶段：simple_joint_control
234 | 3. 应用阶段：joint_position_control
235 | ```
236 | 
237 | ### 🎪 学习路径
238 | ```bash
239 | 初学者：simple_joint_control → joint_position_control
240 | 开发者：mi_motor_control → simple_joint_control → joint_position_control
241 | ```
242 | 
243 | ### 🚀 项目选择
244 | ```bash
245 | 快速验证 → simple_joint_control
246 | 关节应用 → joint_position_control
247 | 功能学习 → mi_motor_control
248 | ```
249 | 
250 | ---
251 | 
252 | ## 📞 技术支持
253 | 
254 | - 📖 振动问题：`ANTI_VIBRATION_GUIDE.md`
255 | - 🔧 硬件连接：各项目README
256 | - 💻 软件问题：检查各项目详细文档
257 | 
258 | ---
259 | 
260 | **最后更新：2024年**
261 | **版本：v2.0 ESP32优化版**


--------------------------------------------------------------------------------
/arduino/sim_setup_motor/README.md:
--------------------------------------------------------------------------------
  1 | # 电机ID设置程序
  2 | 
  3 | **通过CAN总线动态修改小米电机ID，实现多电机串联控制**
  4 | 
  5 | ## 🎯 功能特点
  6 | 
  7 | - ✅ **扫描电机**：自动发现总线上所有电机
  8 | - ✅ **ID修改**：通过CAN总线修改电机ID
  9 | - ✅ **批量设置**：快速设置连续电机ID
 10 | - ✅ **验证功能**：确认ID修改结果
 11 | - ✅ **安全检查**：防止ID冲突和错误
 12 | 
 13 | ## 📱 串口命令
 14 | 
 15 | ### 基础命令
 16 | ```bash
 17 | help                   # 显示帮助信息
 18 | scan                   # 扫描总线上的电机(1-127)
 19 | scan 1 20              # 扫描指定范围的电机ID
 20 | verify                 # 验证发现的电机ID
 21 | ```
 22 | 
 23 | ### ID设置命令 ⚠️ **（总线上只能连接一个电机）**
 24 | ```bash
 25 | set 1 2               # 将ID=1的电机改为ID=2
 26 | set 5 10              # 将ID=5的电机改为ID=10
 27 | reset 3               # 重置指定电机
 28 | ```
 29 | 
 30 | ### 批量设置命令
 31 | ```bash
 32 | batch 5               # 批量设置：1→5, 2→6, 3→7, 4→8, 5→9...
 33 | ```
 34 | 
 35 | ## 🚀 使用流程
 36 | 
 37 | ### 方案一：单个电机逐步设置
 38 | ```bash
 39 | # 1. 连接第一个电机到总线
 40 | scan                    # 查看当前电机ID
 41 | set 1 2                 # 将ID=1改为ID=2
 42 | verify                  # 确认修改成功
 43 | 
 44 | # 2. 断开第一个电机，连接第二个电机
 45 | scan                    # 查看第二个电机ID
 46 | set 1 3                 # 将ID=1改为ID=3
 47 | verify                  # 确认修改成功
 48 | 
 49 | # 3. 重复步骤，直到所有电机都设置了唯一ID
 50 | ```
 51 | 
 52 | ### 方案二：批量快速设置（推荐）
 53 | ```bash
 54 | batch 5                 # 开始批量设置，从ID=5开始
 55 | # 程序会提示：
 56 | # - 连接电机1，按任意键继续
 57 | # - 设置电机1为ID=5
 58 | # - 连接电机2，按任意键继续
 59 | # - 设置电机2为ID=6
 60 | # ... 继续直到所有电机设置完成
 61 | 
 62 | verify                  # 验证所有电机ID设置
 63 | ```
 64 | 
 65 | ### 方案三：精确控制设置
 66 | ```bash
 67 | # 预先规划电机ID分配：
 68 | # 电机1 → ID=1 (关节1)
 69 | # 电机2 → ID=2 (关节2)
 70 | # 电机3 → ID=3 (关节3)
 71 | 
 72 | # 依次设置：
 73 | scan 1 10              # 扫描1-10范围内的电机
 74 | set 7 1                # 将发现的电机改为ID=1
 75 | # (断开电机1，连接电机2)
 76 | set 7 2                # 将发现的电机改为ID=2
 77 | # ... 继续设置所有电机
 78 | 
 79 | verify                  # 最终验证
 80 | ```
 81 | 
 82 | ## 🔧 工作原理
 83 | 
 84 | ### SET_MOTOR_CAN_ID协议
 85 | - **命令模式**: 7 (SET_MOTOR_CAN_ID)
 86 | - **数据格式**: 第一个字节为新的电机ID
 87 | - **仲裁ID**: (7<<24) | (MASTER_ID<<8) | OLD_MOTOR_ID
 88 | - **响应验证**: 修改后用新ID ping 电机
 89 | 
 90 | ### ID修改过程
 91 | 1. 发送SET_MOTOR_CAN_ID命令
 92 | 2. 电机内部修改保存新的CAN ID
 93 | 3. 电机重启或重新初始化
 94 | 4. 使用新ID验证修改成功
 95 | 
 96 | ## 📊 实际应用场景
 97 | 
 98 | ### 1. 机器人多关节控制
 99 | ```bash
100 | # 6自由度机械臂
101 | batch 6                 # 设置6个电机：ID=1,2,3,4,5,6
102 | # 对应关系：
103 | # ID=1 → 基座关节
104 | # ID=2 → 肩部关节
105 | # ID=3 → 肘部关节
106 | # ID=4 → 腕部关节
107 | # ID=5 → 手腕关节
108 | # ID=6 → 手爪关节
109 | ```
110 | 
111 | ### 2. 多轮驱动控制
112 | ```bash
113 | # 4轮驱动小车
114 | set 1 1                # 前左轮 (保持ID=1)
115 | set 1 2                # 前右轮 (改为ID=2)
116 | set 1 3                # 后左轮 (改为ID=3)
117 | set 1 4                # 后右轮 (改为ID=4)
118 | ```
119 | 
120 | ### 3. 云台控制系统
121 | ```bash
122 | # 2轴云台
123 | set 1 10               # 水平轴改为ID=10
124 | set 1 11               # 垂直轴改为ID=11
125 | # 避免与底盘电机ID冲突
126 | ```
127 | 
128 | ## ⚠️ 重要注意事项
129 | 
130 | ### 安全操作
131 | 1. **修改ID时只连接一个电机**：避免多个电机同时响应造成混乱
132 | 2. **记录电机对应关系**：建议记录哪个物理电机对应哪个ID
133 | 3. **备份原始ID**：某些电机的原始ID可能有特殊用途
134 | 4. **电源稳定**：ID修改过程中不要断电
135 | 
136 | ### ID分配原则
137 | 1. **连续分配**：1,2,3,4... 便于管理
138 | 2. **功能分组**：关节电机用连续ID，轮子电机用另一组连续ID
139 | 3. **预留空间**：为后续扩展预留ID空间
140 | 
141 | ### 故障排除
142 | ```bash
143 | # 修改失败？
144 | reset <id>              # 先重置电机
145 | set <old> <new>          # 重新尝试修改
146 | verify                  # 确认结果
147 | 
148 | # 扫描不到电机？
149 | - 检查CAN连接
150 | - 检查电机电源
151 | - 确认波特率1Mbps
152 | - 检查终端电阻
153 | 
154 | # ID冲突？
155 | scan 1 127              # 扫描所有ID
156 | - 找出冲突的ID
157 | - 重新分配唯一ID
158 | ```
159 | 
160 | ## 🎪 完整示例
161 | 
162 | ### 示例1：3关节机械臂设置
163 | ```bash
164 | # 步骤1：连接第一个电机
165 | scan
166 | # 输出：发现电机ID: 1
167 | set 1 1
168 | # 输出：成功！电机ID已修改为: 1
169 | 
170 | # 步骤2：连接第二个电机
171 | scan
172 | # 输出：发现电机ID: 1
173 | set 1 2
174 | # 输出：成功！电机ID已修改为: 2
175 | 
176 | # 步骤3：连接第三个电机
177 | scan
178 | # 输出：发现电机ID: 1
179 | set 1 3
180 | # 输出：成功！电机ID已修改为: 3
181 | 
182 | # 步骤4：验证所有电机
183 | verify
184 | # 输出：发现 3 个电机，当前电机ID: 1, 2, 3
185 | ```
186 | 
187 | ### 示例2：快速批量设置5个电机
188 | ```bash
189 | batch 1
190 | # 程序提示：连接第1个电机，按任意键...
191 | # 用户按键，程序自动设置为ID=1
192 | 
193 | # 程序提示：连接第2个电机，按任意键...
194 | # 用户按键，程序自动设置为ID=2
195 | 
196 | # ... 继续设置第3、4、5个电机
197 | 
198 | # 最终输出：批量设置完成！5个电机ID为: 1,2,3,4,5
199 | ```
200 | 
201 | ## 🔧 技术细节
202 | 
203 | ### CAN帧格式
204 | ```
205 | 仲裁ID: (7<<24) | (0<<8) | old_motor_id
206 | 数据: [new_motor_id, 0, 0, 0, 0, 0, 0, 0]
207 | ```
208 | 
209 | ### 支持的ID范围
210 | - **电机ID**: 1-127 (小米电机标准)
211 | - **主机ID**: 0 (固定，小米协议)
212 | - **最大电机数**: 127个
213 | 
214 | ### 通信参数
215 | - **波特率**: 1Mbps
216 | - **帧格式**: 扩展帧
217 | - **数据长度**: 8字节
218 | 
219 | ## 💡 高级技巧
220 | 
221 | ### 1. ID规划模板
222 | ```bash
223 | # 机器人系统规划：
224 | # ID 1-10:   关节电机
225 | # ID 11-20:  轮子电机
226 | # ID 21-30:  执行器电机
227 | # ID 31-50:  传感器预留
228 | # ID 51-127: 备用
229 | ```
230 | 
231 | ### 2. 快速验证脚本
232 | ```bash
233 | # 验证前10个电机
234 | verify
235 | 
236 | # 验证特定电机
237 | scan 10 15
238 | # 只扫描ID 10-15
239 | ```
240 | 
241 | ### 3. 故障恢复
242 | ```bash
243 | # 如果某个电机无响应
244 | reset <problem_id>
245 | # 重置电机
246 | 
247 | # 如果ID冲突
248 | scan 1 127
249 | # 全面扫描找出所有电机
250 | ```
251 | 
252 | ## 📋 适用场景
253 | 
254 | - 🤖 **多关节机器人控制**：每个关节独立ID
255 | - 🚗 **多轮驱动系统**：左右轮独立控制
256 | - 📷 **云台系统**：水平和垂直轴独立ID
257 | - 🔧 **生产线设备**：多个执行器协调控制
258 | - 🎮 **游戏设备**：多个电机同时控制
259 | 
260 | ---
261 | 
262 | **总结**：通过这个程序，您可以轻松管理多个小米电机的ID，实现复杂的电机控制系统。建议使用批量设置功能来快速配置多个电机。


--------------------------------------------------------------------------------
/arduino/dual_motor_control/README.md:
--------------------------------------------------------------------------------
  1 | # 双电机控制程序
  2 | 
  3 | **同时控制两个小米电机，支持独立控制和同步控制**
  4 | 
  5 | ## 🎯 项目特点
  6 | 
  7 | - ✅ **双电机独立控制**：分别控制电机1和电机2
  8 | - ✅ **同步控制模式**：两个电机同时运动到指定位置
  9 | - ✅ **0-360度角度输入**：直观的角度控制
 10 | - ✅ **动态ID配置**：运行时更改电机ID，无需重新编译
 11 | - ✅ **独立PID参数**：每个电机可单独调节响应特性
 12 | - ✅ **配置自动保存**：断电不丢失
 13 | - ✅ **实时状态监控**：显示两个电机的位置和速度
 14 | 
 15 | ## 📱 控制命令
 16 | 
 17 | ### 🎯 双电机控制（推荐）
 18 | ```bash
 19 | 90,180           # 电机1到90度，电机2到180度
 20 | 0,0              # 双电机都回到0度
 21 | 90,360           # 电机1到90度，电机2到360度(回零)
 22 | 45,135           # 电机1到45度，电机2到135度
 23 | 1.57,3.14        # 弧度输入：电机1到π/2，电机2到π
 24 | ```
 25 | 
 26 | ### 🎯 单电机控制
 27 | ```bash
 28 | 1-90             # 电机1转到90度
 29 | 2-180            # 电机2转到180度
 30 | 1-0              # 电机1回到0度
 31 | 2-270            # 电机2转到270度
 32 | 1-45             # 电机1转到45度
 33 | 2-22.5           # 电机2转到22.5度
 34 | ```
 35 | 
 36 | ### 🔄 旧格式同步控制（兼容）
 37 | ```bash
 38 | sync 90 180      # 电机1到90度，电机2到180度
 39 | sync 0 0         # 双电机都回到0度
 40 | sync 45 135      # 电机1到45度，电机2到135度
 41 | sync 180 90      # 电机1到180度，电机2到90度
 42 | ```
 43 | 
 44 | ### 📐 弧度输入支持
 45 | ```bash
 46 | 1-1.57           # 电机1转到π/2弧度(90度)
 47 | 2-3.14           # 电机2转到π弧度(180度)
 48 | sync -1.57 1.57  # 电机1到-90度，电机2到90度
 49 | ```
 50 | 
 51 | ### ⚙️ 配置命令
 52 | ```bash
 53 | id1 3            # 设置电机1ID为3
 54 | id2 4            # 设置电机2ID为4
 55 | config           # 显示当前配置
 56 | restart          # 重启双电机
 57 | ```
 58 | 
 59 | ### 🔧 参数调节
 60 | ```bash
 61 | kp1 25           # 设置电机1位置KP
 62 | kp2 30           # 设置电机2位置KP
 63 | speed 3.0        # 设置双电机最大速度
 64 | ```
 65 | 
 66 | ### 📊 状态命令
 67 | ```bash
 68 | status           # 显示双电机详细状态
 69 | stop             # 停止双电机
 70 | zero             # 设双电机零点
 71 | restart          # 重启双电机
 72 | help             # 显示帮助信息
 73 | ```
 74 | 
 75 | ## 🎪 应用场景
 76 | 
 77 | ### 1. 机器人双关节控制
 78 | ```bash
 79 | # 关节1和关节2协调运动
 80 | sync 0 0         # 双关节回到起始位置
 81 | sync 90 45       # 关节1到90度，关节2到45度
 82 | sync 180 90      # 关节1到180度，关节2到90度
 83 | sync 0 0         # 回到起始位置
 84 | ```
 85 | 
 86 | ### 2. 云台控制系统
 87 | ```bash
 88 | # 水平和垂直轴控制
 89 | 1-90             # 水平轴转到90度
 90 | 2-45             # 垂直轴转到45度
 91 | sync 180 90      # 同时转到水平和垂直位置
 92 | sync 0 0         # 回到中心位置
 93 | ```
 94 | 
 95 | ### 3. 对称机构控制
 96 | ```bash
 97 | # 对称运动，如夹爪开合
 98 | sync 30 -30      # 夹爪打开30度
 99 | sync 60 -60      # 夹爪打开60度
100 | sync 0 0         # 夹爪关闭
101 | ```
102 | 
103 | ### 4. 独立轮子控制
104 | ```bash
105 | # 独立控制两个轮子
106 | 1-90             # 左轮前进
107 | 2-90             # 右轮前进
108 | 1-0              # 左轮停止
109 | 2-0              # 右轮停止
110 | ```
111 | 
112 | ## 🔧 配置管理
113 | 
114 | ### 默认配置
115 | ```cpp
116 | 电机1 ID: 1
117 | 电机2 ID: 2
118 | 主机ID: 0
119 | 最大速度: 2.0 rad/s
120 | 电机1位置KP: 30.0
121 | 电机2位置KP: 30.0
122 | 转矩限制: 12.0 Nm
123 | ```
124 | 
125 | ### 更改电机ID
126 | ```bash
127 | # 如果您的电机ID不是1和2
128 | id1 11           # 电机1实际ID是11
129 | id2 12           # 电机2实际ID是12
130 | restart          # 重启应用新ID
131 | ```
132 | 
133 | ### PID参数调节
134 | ```bash
135 | # 如果电机响应太快或有振动
136 | kp1 20           # 降低电机1的响应速度
137 | kp2 25           # 降低电机2的响应速度
138 | 
139 | # 如果电机响应太慢
140 | kp1 40           # 提高电机1的响应速度
141 | kp2 45           # 提高电机2的响应速度
142 | ```
143 | 
144 | ## 📊 实时状态监控
145 | 
146 | 程序每2秒显示一次双电机状态：
147 | ```
148 | 电机1(ID=1): 位置 1.571 rad (90.0°), 速度: 0.15 rad/s
149 | 电机2(ID=2): 位置 0.785 rad (45.0°), 速度: 0.08 rad/s
150 | ```
151 | 
152 | ## ⚡ 使用流程
153 | 
154 | ### 1. 硬件连接
155 | - ESP32 + CAN模块
156 | - 两个小米电机 + 电源(12-24V)
157 | - CAN总线连接
158 | 
159 | ### 2. 上传程序
160 | - 上传 `dual_motor_control.ino` 到ESP32
161 | - 打开串口监视器（115200波特率）
162 | 
163 | ### 3. 电机ID设置（如需要）
164 | ```bash
165 | # 如果电机ID不是默认的1和2
166 | id1 11           # 设置电机1 ID
167 | id2 12           # 设置电机2 ID
168 | restart          # 重启应用
169 | ```
170 | 
171 | ### 4. 开始控制
172 | ```bash
173 | # 测试单电机
174 | 1-90             # 电机1到90度
175 | 2-180            # 电机2到180度
176 | 
177 | # 测试同步控制
178 | sync 45 135      # 双电机同步运动
179 | 
180 | # 查看状态
181 | status           # 显示详细状态
182 | ```
183 | 
184 | ## 🛡️ 安全特性
185 | 
186 | - **独立控制**：每个电机独立控制，互不干扰
187 | - **范围限制**：自动限制到电机安全范围(-12到12弧度)
188 | - **智能识别**：自动识别角度/弧度输入
189 | - **参数验证**：自动检查参数有效性
190 | - **紧急停止**：随时输入"stop"立即停止双电机
191 | - **故障隔离**：一个电机故障不影响另一个
192 | 
193 | ## 💡 使用技巧
194 | 
195 | ### 1. 同步控制技巧
196 | ```bash
197 | # 先小幅度测试
198 | sync 10 10        # 小幅度同步运动
199 | sync 45 45        # 中等幅度
200 | sync 90 90        # 大幅度同步
201 | ```
202 | 
203 | ### 2. 精确定位
204 | ```bash
205 | # 使用小数点位数获得更高精度
206 | 1-22.5           # 精确22.5度
207 | 2-67.75          # 精确67.75度
208 | sync 112.5 67.5   # 精确同步定位
209 | ```
210 | 
211 | ### 3. 速度调节
212 | ```bash
213 | # 需要快速响应
214 | speed 5.0        # 提高速度
215 | 
216 | # 需要平滑运动
217 | speed 1.0        # 降低速度
218 | kp1 15           # 降低响应
219 | kp2 15
220 | ```
221 | 
222 | ### 4. 调试技巧
223 | ```bash
224 | # 单独测试每个电机
225 | 1-90             # 只测试电机1
226 | 2-90             # 只测试电机2
227 | 
228 | # 检查状态
229 | status           # 查看详细状态
230 | config           # 检查配置
231 | ```
232 | 
233 | ## 🔧 故障排除
234 | 
235 | ### 电机无响应？
236 | 1. 检查CAN连接和电机电源
237 | 2. 确认电机ID设置是否正确（`config`查看）
238 | 3. 尝试单独控制每个电机
239 | 
240 | ### 同步控制不同步？
241 | 1. 调节KP参数使两个电机响应一致
242 | 2. 检查机械负载是否均匀
243 | 3. 使用`status`查看实际位置差异
244 | 
245 | ### 配置丢失？
246 | - 首次运行会自动使用默认配置
247 | - 重新设置后会自动保存
248 | 
249 | ### 振动明显？
250 | 1. 降低KP参数
251 | 2. 降低运动速度
252 | 3. 检查机械安装
253 | 
254 | ---
255 | 
256 | **总结**：这个双电机控制程序提供了完整的多电机控制能力，支持独立控制和同步控制，适用于各种需要协调运动的应用场景。


--------------------------------------------------------------------------------
/arduino/advanced_motor_control/README.md:
--------------------------------------------------------------------------------
  1 | # 高级电机控制程序
  2 | 
  3 | **基于小米电机官方功能码表格的完整控制系统**
  4 | 
  5 | ## 🎯 项目特点
  6 | 
  7 | - ✅ **完整功能码支持**：基于官方功能码表格，支持所有参数
  8 | - ✅ **实时状态监控**：温度、电压、故障、位置、速度全面监控
  9 | - ✅ **精确PID调节**：在线调节所有PID参数
 10 | - ✅ **安全保护机制**：自动安全监控和紧急停止
 11 | - ✅ **参数持久化**：配置自动保存到EEPROM
 12 | - ✅ **故障诊断**：详细的故障代码分析和处理
 13 | 
 14 | ## 📋 功能码映射
 15 | 
 16 | ### 🔧 配置参数
 17 | ```cpp
 18 | 0x200a // CAN_ID - 本节点ID (1-127)
 19 | 0x200b // CAN_MASTER - 主机ID (0-127)
 20 | 0x200c // CAN_TIMEOUT - 超时阈值
 21 | 0x200d // motorOverTemp - 保护温度值
 22 | 0x200f // GearRatio - 传动比
 23 | ```
 24 | 
 25 | ### 🎮 控制参数
 26 | ```cpp
 27 | 0x2012 // cur_kp - 电流KP (0-200)
 28 | 0x2013 // cur_ki - 电流KI (0-200)
 29 | 0x2014 // spd_kp - 速度KP (0-200)
 30 | 0x2015 // spd_ki - 速度KI (0-200)
 31 | 0x2016 // loc_kp - 位置KP (0-200)
 32 | ```
 33 | 
 34 | ### ⚡ 限制参数
 35 | ```cpp
 36 | 0x2007 // limit_torque - 转矩限制 (0-12 Nm)
 37 | 0x2018 // limit_spd - 速度限制 (0-200 rad/s)
 38 | 0x2019 // limit_cur - 电流限制 (0-23 A)
 39 | ```
 40 | 
 41 | ### 📊 状态参数
 42 | ```cpp
 43 | 0x3014 // rotation - 圈数
 44 | 0x3015 // modPos - 电机角度(未计圈)
 45 | 0x3016 // mechPos - 负载位置(计圈) ★ 主用
 46 | 0x3017 // mechVel - 负载速度 ★ 主用
 47 | 0x3006 // motorTemp - 电机温度
 48 | 0x300c // vBus - 母线电压
 49 | 0x3022 // faultSta - 故障状态
 50 | 0x3023 // warnSta - 警告状态
 51 | ```
 52 | 
 53 | ## 🎮 控制命令
 54 | 
 55 | ### 基本控制
 56 | ```bash
 57 | pos 1.57          # 设置位置1.57弧度(90度)
 58 | spd 5.0           # 设置速度5.0rad/s
 59 | stop              # 紧急停止
 60 | clear             # 清除故障
 61 | ```
 62 | 
 63 | ### 状态查询
 64 | ```bash
 65 | status            # 显示完整状态信息
 66 | params            # 显示所有可调参数
 67 | info              # 读取设备信息
 68 | ```
 69 | 
 70 | ### 参数调节
 71 | ```bash
 72 | # 直接设置功能码
 73 | set 0x2016 30.0   # 设置位置KP=30.0
 74 | get 0x3016        # 读取当前位置
 75 | 
 76 | # 快捷命令（在tune模式内）
 77 | tune              # 进入PID调节模式
 78 |   kp 25.0         # 设置位置KP
 79 |   kd 5.0          # 设置速度KP
 80 |   ki 0.02         # 设置速度KI
 81 |   test            # 执行阶跃响应测试
 82 |   save            # 保存参数
 83 |   exit            # 退出调节
 84 | ```
 85 | 
 86 | ### 安全功能
 87 | ```bash
 88 | monitor           # 启动连续安全监控
 89 | emergency         # 立即紧急停止
 90 | ```
 91 | 
 92 | ## 🔧 高级功能
 93 | 
 94 | ### 1. 实时状态监控
 95 | ```bash
 96 | status
 97 | ```
 98 | 输出示例：
 99 | ```
100 | === 电机完整状态 ===
101 | 电机ID: 1
102 | 控制模式: 位置
103 | 
104 | --- 运动状态 ---
105 | 位置: 1.571 rad (90.0°)
106 | 速度: 0.015 rad/s (1.4 RPM)
107 | 圈数: 0
108 | 电流: 2.35 A
109 | 
110 | --- 系统状态 ---
111 | 电机温度: 33.3°C
112 | 母线电压: 24.20 V
113 | 故障代码: 0x00000000 ✓
114 | 警告代码: 0x00000000 ✓
115 | ==================
116 | ```
117 | 
118 | ### 2. PID在线调节
119 | ```bash
120 | tune              # 进入调节模式
121 | kp 25.0           # 调整位置响应速度
122 | kd 10.0           # 调整阻尼
123 | ki 0.05           # 减少稳态误差
124 | test              # 阶跃响应测试
125 | save              # 保存到EEPROM
126 | ```
127 | 
128 | ### 3. 故障诊断
129 | ```bash
130 | status            # 查看故障代码
131 | clear             # 清除故障状态
132 | ```
133 | 
134 | ### 4. 参数批量读取
135 | ```bash
136 | params            # 显示所有参数
137 | ```
138 | 
139 | ## 🛡️ 安全保护
140 | 
141 | ### 自动安全监控
142 | - **温度保护**：超过85°C自动停机
143 | - **电压保护**：超出18-32V范围警告
144 | - **位置保护**：超出±12弧度警告
145 | - **故障保护**：检测到故障立即停机
146 | 
147 | ### 安全阈值
148 | ```cpp
149 | 温度阈值: 85°C
150 | 电压范围: 18-32V
151 | 位置范围: -12到12弧度
152 | ```
153 | 
154 | ## 📊 典型应用场景
155 | 
156 | ### 场景1：机器人关节调试
157 | ```bash
158 | # 1. 查看当前状态
159 | status
160 | 
161 | # 2. 调节PID参数
162 | tune
163 | kp 40.0           # 提高响应速度
164 | test              # 测试响应
165 | save              # 保存参数
166 | 
167 | # 3. 位置控制
168 | pos 1.57          # 转90度
169 | pos 0             # 回零
170 | ```
171 | 
172 | ### 场景2：生产线监控
173 | ```bash
174 | # 1. 连续监控
175 | monitor           # 启动安全监控
176 | 
177 | # 2. 定期检查
178 | status            # 每小时检查状态
179 | params            # 检查参数是否正常
180 | 
181 | # 3. 故障处理
182 | clear             # 清除偶发故障
183 | ```
184 | 
185 | ### 场景3：性能优化
186 | ```bash
187 | # 1. 阶跃响应测试
188 | tune
189 | test              # 默认阶跃测试
190 | 
191 | # 2. 参数优化
192 | kp 35.0           # 调整位置KP
193 | kd 8.0            # 调整速度KP
194 | ki 0.03           # 调整速度KI
195 | test              # 再次测试
196 | 
197 | # 3. 保存最优参数
198 | save
199 | ```
200 | 
201 | ## 🚀 快速开始
202 | 
203 | ### 1. 硬件连接
204 | - ESP32 + CAN模块
205 | - 小米电机 + 电源(12-24V)
206 | - CAN总线连接
207 | 
208 | ### 2. 软件配置
209 | - 上传 `advanced_motor_control.ino`
210 | - 打开串口监视器(115200)
211 | - 观察初始化信息
212 | 
213 | ### 3. 基本测试
214 | ```bash
215 | status            # 确认电机正常
216 | pos 1.57          # 测试位置控制
217 | pos 0             # 回到原位
218 | ```
219 | 
220 | ## 🔍 故障排除
221 | 
222 | ### 电机无响应？
223 | 1. 检查CAN连接和波特率(1Mbps)
224 | 2. 确认电机电源(12-24V)
225 | 3. 检查电机ID设置
226 | 
227 | ### PID参数不当？
228 | 1. 使用 `tune` 模式调节
229 | 2. 从小参数开始，逐步增加
230 | 3. 每次调节后 `test` 测试响应
231 | 
232 | ### 温度过高？
233 | 1. 检查负载是否过重
234 | 2. 降低运行速度
235 | 3. 改善散热条件
236 | 
237 | ### 参数设置失败？
238 | 1. 确认功能码正确
239 | 2. 检查参数范围
240 | 3. 重新发送命令
241 | 
242 | ## 💡 专业技巧
243 | 
244 | ### 1. PID调节顺序
245 | ```bash
246 | # 推荐调节顺序
247 | 1. 先调位置KP (kp) - 决定响应速度
248 | 2. 再调速度KP (kd) - 决定阻尼
249 | 3. 最后调速度KI (ki) - 消除稳态误差
250 | ```
251 | 
252 | ### 2. 参数记忆
253 | ```bash
254 | # 所有重要参数会自动保存
255 | - PID参数
256 | - 限制参数
257 | - ID配置
258 | - 重启后自动加载
259 | ```
260 | 
261 | ### 3. 安全操作
262 | ```bash
263 | # 重要操作前检查状态
264 | status            # 确认无故障
265 | monitor           # 启动监控
266 | # 再进行控制操作
267 | ```
268 | 
269 | ### 4. 调试技巧
270 | ```bash
271 | # 阶跃响应测试要点
272 | - 响应时间：通常0.5-2秒
273 | - 超调量：小于10%
274 | - 稳态误差：小于1%
275 | ```
276 | 
277 | ---
278 | 
279 | **总结**：这个高级控制程序提供了小米电机的完整控制能力，通过功能码可以直接访问所有电机参数，实现专业级的电机控制和应用开发。


--------------------------------------------------------------------------------
/python/examples/basic_usage.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python3
  2 | # -*- coding: utf-8 -*-
  3 | """
  4 | RobStride Basic Usage Example
  5 | Demonstrates basic motor control operations
  6 | """
  7 | 
  8 | import sys
  9 | import os
 10 | import time
 11 | import math
 12 | 
 13 | # Add the python directory to path
 14 | sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'python'))
 15 | 
 16 | try:
 17 |     from src.position_control import PositionControllerMIT
 18 |     from src.speed_control import SpeedController
 19 | except ImportError:
 20 |     print("❌ Cannot import motor control modules")
 21 |     sys.exit(1)
 22 | 
 23 | def test_position_control(motor_id=11):
 24 |     """Test position control with basic movements"""
 25 |     print(f"\n🎯 Testing Position Control (Motor {motor_id})")
 26 |     print("=" * 50)
 27 | 
 28 |     controller = PositionControllerMIT(motor_id)
 29 | 
 30 |     try:
 31 |         # Connect to motor
 32 |         if not controller.connect():
 33 |             print("❌ Failed to connect to motor")
 34 |             return False
 35 | 
 36 |         print("✅ Connected successfully!")
 37 | 
 38 |         # Test movements
 39 |         movements = [
 40 |             (0.0, "Home position"),
 41 |             (90.0, "90 degrees"),
 42 |             (-90.0, "-90 degrees"),
 43 |             (180.0, "180 degrees"),
 44 |             (0.0, "Back to home")
 45 |         ]
 46 | 
 47 |         for angle, description in movements:
 48 |             print(f"📍 Moving to {description}: {angle}°")
 49 |             controller.set_angle(angle)
 50 |             time.sleep(2.0)  # Wait for movement to complete
 51 | 
 52 |         print("✅ Position control test completed!")
 53 |         return True
 54 | 
 55 |     except Exception as e:
 56 |         print(f"❌ Position control test failed: {e}")
 57 |         return False
 58 |     finally:
 59 |         controller.stop_and_exit()
 60 | 
 61 | def test_speed_control(motor_id=11):
 62 |     """Test speed control with basic movements"""
 63 |     print(f"\n⚡ Testing Speed Control (Motor {motor_id})")
 64 |     print("=" * 50)
 65 | 
 66 |     controller = SpeedController(motor_id)
 67 | 
 68 |     try:
 69 |         # Connect to motor
 70 |         if not controller.connect():
 71 |             print("❌ Failed to connect to motor")
 72 |             return False
 73 | 
 74 |         print("✅ Connected successfully!")
 75 | 
 76 |         # Test speeds
 77 |         speeds = [
 78 |             (2.0, "Forward 2.0 rad/s"),
 79 |             (5.0, "Forward 5.0 rad/s"),
 80 |             (0.0, "Stop"),
 81 |             (-2.0, "Reverse 2.0 rad/s"),
 82 |             (-5.0, "Reverse 5.0 rad/s"),
 83 |             (0.0, "Stop")
 84 |         ]
 85 | 
 86 |         for speed, description in speeds:
 87 |             print(f"🚀 {description}: {speed} rad/s")
 88 |             controller.set_velocity(speed)
 89 |             time.sleep(3.0)  # Run for 3 seconds
 90 | 
 91 |         print("✅ Speed control test completed!")
 92 |         return True
 93 | 
 94 |     except Exception as e:
 95 |         print(f"❌ Speed control test failed: {e}")
 96 |         return False
 97 |     finally:
 98 |         controller.stop_and_exit()
 99 | 
100 | def interactive_demo(motor_id=11):
101 |     """Interactive demo for user experimentation"""
102 |     print(f"\n🎮 Interactive Demo (Motor {motor_id})")
103 |     print("=" * 50)
104 |     print("Choose control mode:")
105 |     print("1. Position Control")
106 |     print("2. Speed Control")
107 |     print("3. Both (alternating)")
108 | 
109 |     try:
110 |         choice = input("Enter choice (1-3): ").strip()
111 | 
112 |         if choice == '1':
113 |             return test_position_control(motor_id)
114 |         elif choice == '2':
115 |             return test_speed_control(motor_id)
116 |         elif choice == '3':
117 |             result1 = test_position_control(motor_id)
118 |             time.sleep(1.0)
119 |             result2 = test_speed_control(motor_id)
120 |             return result1 and result2
121 |         else:
122 |             print("❌ Invalid choice")
123 |             return False
124 | 
125 |     except KeyboardInterrupt:
126 |         print("\n👋 Demo interrupted by user")
127 |         return False
128 | 
129 | def main():
130 |     """Main function"""
131 |     print("🎯 RobStride Basic Usage Example")
132 |     print("=" * 50)
133 | 
134 |     # Get motor ID from command line or use default
135 |     motor_id = 11
136 |     if len(sys.argv) > 1:
137 |         try:
138 |             motor_id = int(sys.argv[1])
139 |         except ValueError:
140 |             print("❌ Invalid motor ID. Using default (11)")
141 | 
142 |     print(f"Using Motor ID: {motor_id}")
143 | 
144 |     # Check if CAN interface is available
145 |     if not os.path.exists('/sys/class/net/can0'):
146 |         print("⚠️ Warning: can0 interface not found")
147 |         print("Please setup CAN interface:")
148 |         print("  sudo ip link set can0 type can bitrate 1000000")
149 |         print("  sudo ip link set up can0")
150 |         print()
151 | 
152 |     try:
153 |         # Run interactive demo
154 |         success = interactive_demo(motor_id)
155 | 
156 |         if success:
157 |             print("\n🎉 Demo completed successfully!")
158 |             return 0
159 |         else:
160 |             print("\n❌ Demo failed!")
161 |             return 1
162 | 
163 |     except Exception as e:
164 |         print(f"\n💥 Unexpected error: {e}")
165 |         return 1
166 | 
167 | if __name__ == "__main__":
168 |     sys.exit(main())


--------------------------------------------------------------------------------
/arduino/mi_motor_control/README.md:
--------------------------------------------------------------------------------
  1 | # 小米电机ESP32控制程序
  2 | 
  3 | 基于ESP32开发板的小米电机CAN总线控制程序，支持多种控制模式和实时状态监控。
  4 | 
  5 | ## 📋 项目概述
  6 | 
  7 | 本项目实现了通过ESP32控制小米电机的完整解决方案，包括：
  8 | - CAN总线通信（1Mbps）
  9 | - 多种控制模式（速度、位置、电流、运控）
 10 | - 实时状态监控
 11 | - 串口调试接口
 12 | 
 13 | ## 🔧 硬件要求
 14 | 
 15 | ### 主要硬件
 16 | - **ESP32开发板**
 17 | - **CAN模块**（支持1Mbps波特率）
 18 | - **小米电机**（CAN ID=1）
 19 | - **电源**（12-24V DC）
 20 | 
 21 | ### 接线说明
 22 | 
 23 | #### ESP32 ↔ CAN模块
 24 | ```
 25 | ESP32      CAN模块
 26 | GPIO5  ←→ TX
 27 | GPIO4  ←→ RX
 28 | 3.3V   ←→ VCC
 29 | GND    ←→ GND
 30 | ```
 31 | 
 32 | #### CAN模块 ↔ 小米电机
 33 | ```
 34 | CAN模块  小米电机
 35 | CAN_H   ←→ CAN_H
 36 | CAN_L   ←→ CAN_L
 37 | 12-24V  ←→ 电源正
 38 | GND     ←→ 电源负
 39 | ```
 40 | 
 41 | ## 💻 软件环境
 42 | 
 43 | ### Arduino IDE设置
 44 | 1. **安装ESP32开发板支持**
 45 |    ```arduino
 46 |    文件 → 首选项 → 附加开发板管理器网址
 47 |    添加：https://dl.espressif.com/dl/package_esp32_index.json
 48 |    ```
 49 | 
 50 | 2. **安装开发板**
 51 |    ```
 52 |    工具 → 开发板 → 开发板管理器
 53 |    搜索：ESP32
 54 |    安装：Espressif Systems ESP32
 55 |    ```
 56 | 
 57 | 3. **选择开发板**
 58 |    ```
 59 |    工具 → 开发板 → ESP32 Arduino → ESP32 Dev Module
 60 |    ```
 61 | 
 62 | ### 必需库安装
 63 | 在Arduino库管理器中安装以下库：
 64 | - **ESP32-TWAI-CAN** （CAN通信库）
 65 | 
 66 | ## 🚀 快速开始
 67 | 
 68 | ### 1. 下载项目
 69 | ```bash
 70 | # 所有文件位于 /home/w0x7ce/Downloads/test_lz/mi_arduino/
 71 | ```
 72 | 
 73 | ### 2. 配置Arduino IDE
 74 | 1. 打开 `mi_motor_control.ino`
 75 | 2. 选择正确的开发板：ESP32S3 Dev Module
 76 | 3. 选择正确的串口
 77 | 
 78 | ### 3. 上传程序
 79 | 1. 连接ESP32S3到电脑
 80 | 2. 点击上传按钮
 81 | 3. 等待上传完成
 82 | 
 83 | ### 4. 监控运行状态
 84 | 1. 打开串口监视器（波特率115200）
 85 | 2. 观察初始化过程
 86 | 3. 查看实时电机数据
 87 | 
 88 | ## 📱 程序功能
 89 | 
 90 | ### 自动控制模式
 91 | 程序启动后会自动执行以下控制序列：
 92 | - 每5秒切换一次速度
 93 | - 速度序列：0 → 2 → -2 → 1 → -1 → 0 rad/s
 94 | 
 95 | ### 串口命令控制
 96 | 通过串口监视器输入命令：
 97 | 
 98 | | 命令 | 功能 | 示例 |
 99 | |------|------|------|
100 | | `stop` | 停止电机 | `stop` |
101 | | `enable` | 使能电机 | `enable` |
102 | | `disable` | 禁用电机 | `disable` |
103 | | `speed X` | 设置速度X rad/s | `speed 3.5` |
104 | | `pos X` | 位置模式，设置位置X rad | `pos 1.57` |
105 | | `speed_mode` | 切换到速度模式 | `speed_mode` |
106 | | `zero` | 设置零点 | `zero` |
107 | | `help` | 显示帮助 | `help` |
108 | 
109 | ### 实时数据监控
110 | 程序会持续输出以下数据：
111 | ```
112 | M1: 主站ID,电机ID,错误状态,HALL错误,磁编码错误,温度错误,电流错误,电压错误,模式状态,角度,速度,扭矩,温度
113 | ```
114 | 
115 | ## 🎯 控制模式详解
116 | 
117 | ### 1. 速度模式 (SPEED_MODE)
118 | - 设置目标角速度：-30 到 +30 rad/s
119 | - 适用场景：连续旋转控制
120 | 
121 | ### 2. 位置模式 (POS_MODE)
122 | - 设置目标角度：-12.5 到 +12.5 rad
123 | - 可设置最大速度限制
124 | - 适用场景：精确定位控制
125 | 
126 | ### 3. 电流模式 (CUR_MODE)
127 | - 设置目标电流：-23 到 +23 A
128 | - 适用场景：力控制
129 | 
130 | ### 4. 运控模式 (CTRL_MODE)
131 | - 同时控制扭矩、位置、速度、PD参数
132 | - 适用场景：复杂运动控制
133 | 
134 | ## 📊 数据格式说明
135 | 
136 | ### CAN帧结构
137 | - **29位扩展帧ID**：通信类型(5bit) + 数据区2(16bit) + 目标地址(8bit)
138 | - **8字节数据区**：控制参数或反馈数据
139 | 
140 | ### 反馈数据解析
141 | ```cpp
142 | // 角度: (-4π ~ 4π) rad
143 | cur_angle = raw_angle * 0.000383501049 - 4*PI
144 | 
145 | // 速度: (-30 ~ 30) rad/s
146 | cur_speed = raw_speed * 0.000915541314 - 30
147 | 
148 | // 扭矩: (-12 ~ 12) Nm
149 | cur_torque = raw_torque * 0.000366216526 - 12
150 | 
151 | // 温度: 摄氏度
152 | cur_temp = raw_temp / 10.0
153 | ```
154 | 
155 | ## ⚠️ 安全注意事项
156 | 
157 | ### 硬件安全
158 | 1. **电源电压**：确保12-24V DC，不要超过电机额定电压
159 | 2. **CAN总线**：正确连接CAN_H和CAN_L，避免反接
160 | 3. **接地**：确保良好的接地连接
161 | 4. **电流**：注意最大电流限制，避免过载
162 | 
163 | ### 软件安全
164 | 1. **参数范围**：严格按照参数范围设置，避免损坏电机
165 | 2. **使能顺序**：遵循初始化顺序，先配置再使能
166 | 3. **错误监控**：注意错误状态，及时处理异常
167 | 4. **温度监控**：监控电机温度，避免过热
168 | 
169 | ### 操作安全
170 | 1. **机械固定**：确保电机牢固安装，避免转动时移动
171 | 2. **运动范围**：确保运动范围内无障碍物
172 | 3. **紧急停止**：准备紧急停止方案
173 | 4. **逐步测试**：从小参数开始测试，确认正常后再增加
174 | 
175 | ## 🔧 参数配置
176 | 
177 | ### 电机参数
178 | ```cpp
179 | #define MOTER_1_ID      1        // 电机ID
180 | #define MASTER_ID       0        // 主机ID
181 | #define P_MIN           -12.5f   // 位置最小值
182 | #define P_MAX           12.5f    // 位置最大值
183 | #define V_MIN           -30.0f   // 速度最小值
184 | #define V_MAX           30.0f    // 速度最大值
185 | #define T_MIN           -12.0f   // 扭矩最小值
186 | #define T_MAX           12.0f    // 扭矩最大值
187 | ```
188 | 
189 | ### CAN参数
190 | ```cpp
191 | #define CAN_TX          5        // CAN发送引脚
192 | #define CAN_RX          4        // CAN接收引脚
193 | // 波特率：1Mbps（在代码中固定）
194 | ```
195 | 
196 | ## 🐛 故障排除
197 | 
198 | ### 常见问题
199 | 
200 | #### 1. CAN通信失败
201 | - 检查硬件连接
202 | - 确认波特率设置为1Mbps
203 | - 检查终端电阻（120Ω）
204 | 
205 | #### 2. 电机无响应
206 | - 检查电源连接
207 | - 确认电机ID设置正确
208 | - 检查使能命令是否发送
209 | 
210 | #### 3. 数据读取异常
211 | - 增加接收超时时间
212 | - 检查CAN总线干扰
213 | - 验证数据解析逻辑
214 | 
215 | #### 4. 编译错误
216 | - 确认ESP32开发板安装正确
217 | - 检查库文件版本兼容性
218 | - 验证Arduino IDE版本
219 | 
220 | ### 调试方法
221 | 
222 | #### 1. 串口调试
223 | - 打开串口监视器观察初始化过程
224 | - 检查实时数据输出
225 | - 使用串口命令测试功能
226 | 
227 | #### 2. CAN总线监控
228 | ```
229 | # 如果有CAN分析工具，监控总线流量
230 | # 检查发送和接收的CAN帧
231 | ```
232 | 
233 | #### 3. 逐步测试
234 | ```cpp
235 | // 简化测试代码，只测试基本功能
236 | void simple_test() {
237 |     Motor_CAN_Init();
238 |     M1_con.Motor_Con_Init(1);
239 |     M1_con.Motor_Enable();
240 |     delay(1000);
241 |     M1_con.Set_SpeedMode(1.0);
242 | }
243 | ```
244 | 
245 | ## 📈 扩展功能
246 | 
247 | ### 可添加的功能
248 | 1. **多电机控制**：支持同时控制多个电机
249 | 2. **轨迹规划**：实现复杂的运动轨迹
250 | 3. **PID参数整定**：自动优化控制参数
251 | 4. **Web界面**：通过WiFi远程控制
252 | 5. **数据记录**：SD卡存储运行数据
253 | 6. **故障诊断**：自动故障检测和处理
254 | 
255 | ### 性能优化
256 | 1. **实时性**：使用FreeRTOS提高实时性能
257 | 2. **通信效率**：优化CAN总线使用率
258 | 3. **控制精度**：改进控制算法精度
259 | 4. **功耗管理**：优化系统功耗
260 | 
261 | ## 📞 技术支持
262 | 
263 | 如有问题，请检查：
264 | 1. 硬件连接是否正确
265 | 2. 参数配置是否合理
266 | 3. 电源供应是否稳定
267 | 4. 串口输出的错误信息
268 | 
269 | ## 📄 许可证
270 | 
271 | 本项目基于小米电机通信协议开发，仅供学习和研究使用。
272 | 
273 | ---
274 | 
275 | **版本信息：**
276 | - 版本：v1.0
277 | - 更新日期：2024年
278 | - 兼容性：ESP32S3 + 小米电机
279 | - 开发环境：Arduino IDE


--------------------------------------------------------------------------------
/arduino/README.md:
--------------------------------------------------------------------------------
  1 | # RobStride Arduino Control
  2 | 
  3 | Arduino平台实现RobStride电机控制库，支持ESP32和Arduino系列微控制器。
  4 | 
  5 | ## 支持的开发板
  6 | 
  7 | - **ESP32**: 推荐使用，内置TWAI/CAN控制器
  8 | - **Arduino Mega 2560**: 需要外部CAN模块(MCP2515)
  9 | - **Arduino Uno**: 需要外部CAN模块(MCP2515)
 10 | 
 11 | ## 项目结构
 12 | 
 13 | ```
 14 | arduino/
 15 | ├── README.md                    # Arduino文档
 16 | ├── simple_joint_control/        # 简单关节控制
 17 | │   ├── simple_joint_control.ino # 主程序
 18 | │   ├── TWAI_CAN_MI_Motor.h      # CAN驱动头文件
 19 | │   └── TWAI_CAN_MI_Motor.cpp    # CAN驱动实现
 20 | ├── joint_position_control/       # 位置控制
 21 | ├── dual_motor_control/          # 双电机控制
 22 | ├── advanced_motor_control/      # 高级控制
 23 | ├── mi_motor_control/           # 基础控制
 24 | └── sim_setup_motor/            # 电机设置
 25 | ```
 26 | 
 27 | ## 硬件连接
 28 | 
 29 | ### ESP32直接连接
 30 | ```
 31 | ESP32           CAN收发器           RobStride电机
 32 | -----           ----------           ------------
 33 | GPIO5    <----> TX       <----> CAN_H
 34 | GPIO4    <----> RX       <----> CAN_L
 35 | 3.3V     -----> VCC
 36 | GND      -----> GND
 37 | ```
 38 | 
 39 | ### Arduino + MCP2515模块
 40 | ```
 41 | Arduino        MCP2515            CAN收发器           RobStride电机
 42 | --------        --------            ----------           ------------
 43 | D10(SPI_SS)   CS
 44 | D11(SPI_MOSI)  SI/MOSI
 45 | D12(SPI_MISO)  SO/MISO
 46 | D13(SPI_SCK)   SCK/SCLK
 47 | 5V             VCC
 48 | GND            GND
 49 | MCP2515 TX   -> CAN_H -> 电机
 50 | MCP2515 RX   -> CAN_L -> 电机
 51 | ```
 52 | 
 53 | ## 快速开始
 54 | 
 55 | ### 1. 环境配置
 56 | 
 57 | #### Arduino IDE
 58 | 1. 安装Arduino IDE 1.8.19+
 59 | 2. 安装ESP32开发板支持
 60 | 3. 工具 -> 开发板 -> ESP32 Arduino -> ESP32 Dev Module
 61 | 
 62 | #### PlatformIO
 63 | ```ini
 64 | [env:esp32]
 65 | platform = espressif32
 66 | board = esp32dev
 67 | framework = arduino
 68 | ```
 69 | 
 70 | ### 2. 编译上传
 71 | 
 72 | ```bash
 73 | # 使用Arduino IDE
 74 | 1. 打开对应的.ino文件
 75 | 2. 选择正确的开发板
 76 | 3. 上传程序
 77 | 
 78 | # 使用PlatformIO
 79 | pio run --target upload
 80 | ```
 81 | 
 82 | ## 控制示例
 83 | 
 84 | ### 基础位置控制
 85 | 
 86 | ```cpp
 87 | #include "TWAI_CAN_MI_Motor.h"
 88 | 
 89 | TWAI_CAN_MI_Motor motor(11);  // 电机ID=11
 90 | 
 91 | void setup() {
 92 |     Serial.begin(115200);
 93 |     motor.init(CAN_SPEED_1000KBPS);
 94 |     motor.enable_motor();
 95 | }
 96 | 
 97 | void loop() {
 98 |     // 设置位置到90度
 99 |     motor.send_mit_command(PI/2, 30.0, 0.5);
100 |     delay(2000);
101 | 
102 |     // 回到原点
103 |     motor.send_mit_command(0, 30.0, 0.5);
104 |     delay(2000);
105 | }
106 | ```
107 | 
108 | ### 参数调整
109 | 
110 | ```cpp
111 | // 位置增益 (0-500.0)
112 | motor.set_kp(30.0);
113 | 
114 | // 阻尼增益 (0-100.0)
115 | motor.set_kd(0.5);
116 | 
117 | // 速度限制 (rad/s)
118 | motor.set_velocity_limit(20.0);
119 | 
120 | // 扭矩限制 (Nm)
121 | motor.set_torque_limit(30.0);
122 | ```
123 | 
124 | ## 控制模式
125 | 
126 | ### MIT模式 (Mode 0)
127 | 直接控制位置、速度和扭矩。
128 | 
129 | ```cpp
130 | // MIT控制指令
131 | void send_mit_command(float position, float kp, float kd);
132 | ```
133 | 
134 | ### 位置模式 (Mode 1)
135 | 基于内部PID的位置控制。
136 | 
137 | ```cpp
138 | // 设置位置目标
139 | void set_position_target(float position);
140 | 
141 | // 设置PID参数
142 | void set_position_kp(float kp);
143 | void set_position_kd(float kd);
144 | ```
145 | 
146 | ### 速度模式 (Mode 2)
147 | 速度闭环控制。
148 | 
149 | ```cpp
150 | // 设置速度目标
151 | void set_velocity_target(float velocity);
152 | 
153 | // 设置速度PID
154 | void set_velocity_kp(float kp);
155 | void set_velocity_ki(float ki);
156 | ```
157 | 
158 | ## 通信协议
159 | 
160 | ### CAN帧格式
161 | - **波特率**: 1000kbps
162 | - **数据长度**: 8字节
163 | - **扩展ID**: 29位标准CAN扩展帧
164 | 
165 | ### 命令类型
166 | - **0x00**: 获取设备ID
167 | - **0x01**: 操作控制 (MIT模式)
168 | - **0x02**: 操作状态反馈
169 | - **0x03**: 使能电机
170 | - **0x04**: 禁用电机
171 | - **0x18**: 写入参数
172 | 
173 | ## 项目说明
174 | 
175 | ### simple_joint_control
176 | 最简单的关节控制示例，演示基础MIT控制。
177 | - 控制单个电机
178 | - 交互式参数调整
179 | - 实时状态反馈
180 | 
181 | ### joint_position_control
182 | 位置控制专用示例。
183 | - 平滑轨迹规划
184 | - 多段位运动
185 | - 位置误差监控
186 | 
187 | ### dual_motor_control
188 | 双电机协调控制。
189 | - 同步运动
190 | - 主从控制
191 | - 协调轨迹
192 | 
193 | ### advanced_motor_control
194 | 高级控制功能。
195 | - 自适应控制
196 | - 振动抑制
197 | - 温度监控
198 | 
199 | ## 故障排除
200 | 
201 | ### 通信问题
202 | 1. **CAN总线错误**
203 |    - 检查波特率设置 (1000kbps)
204 |    - 验证终端电阻 (120Ω)
205 |    - 确认CAN_H/CAN_L接线
206 | 
207 | 2. **电机无响应**
208 |    - 检查电机ID设置
209 |    - 验证电源供应 (12-48V)
210 |    - 确认使能指令发送成功
211 | 
212 | ### 编译问题
213 | 1. **ESP32编译错误**
214 |    - 更新ESP32开发板包
215 |    - 检查Arduino IDE版本
216 |    - 清理编译缓存
217 | 
218 | 2. **库依赖问题**
219 |    - 确保包含所有头文件
220 |    - 检查TWAI库版本
221 |    - 验证SPI连接(MCP2515)
222 | 
223 | ## 性能指标
224 | 
225 | | 开发板 | 控制频率 | 延迟 | 电机数量 | 特点 |
226 | |--------|----------|------|----------|------|
227 | | ESP32  | 100-200Hz | 2-5ms | 1-8 | 内置CAN，高性能 |
228 | | Arduino Mega | 50-100Hz | 5-10ms | 1-4 | 丰富IO，稳定 |
229 | | Arduino Uno | 30-50Hz | 10-20ms | 1-2 | 低成本，入门级 |
230 | 
231 | ## 开发资源
232 | 
233 | ### Arduino库依赖
234 | ```cpp
235 | #include <Arduino.h>
236 | #include <driver/twai.h>  // ESP32 TWAI驱动
237 | #include <SPI.h>          // SPI通信(MCP2515)
238 | ```
239 | 
240 | ### 调试工具
241 | - **串口监视器**: 115200波特率
242 | - **CAN分析仪**: 实时监控CAN流量
243 | - **逻辑分析仪**: 分析时序信号
244 | 
245 | ## 扩展功能
246 | 
247 | ### 多电机控制
248 | ```cpp
249 | TWAI_CAN_MI_Motor motor1(11);
250 | TWAI_CAN_MI_Motor motor2(12);
251 | TWAI_CAN_MI_Motor motor3(13);
252 | 
253 | void setup() {
254 |     motor1.init(CAN_SPEED_1000KBPS);
255 |     motor2.init(CAN_SPEED_1000KBPS);
256 |     motor3.init(CAN_SPEED_1000KBPS);
257 | }
258 | ```
259 | 
260 | ### 传感器集成
261 | ```cpp
262 | // 编码器反馈
263 | float encoder_position = read_encoder();
264 | 
265 | // 力矩传感器
266 | float torque_feedback = read_torque_sensor();
267 | ```
268 | 
269 | ### 网络控制
270 | ```cpp
271 | // WiFi控制
272 | #include <WiFi.h>
273 | #include <WebServer.h>
274 | 
275 | // 通过网页控制电机
276 | void handleControlRequest() {
277 |     float target_pos = server.arg("pos").toFloat();
278 |     motor.send_mit_command(target_pos, kp, kd);
279 | }
280 | ```
281 | 
282 | ## 许可证
283 | 
284 | MIT License - 详见[../LICENSE](../LICENSE)文件
285 | 
286 | ## 技术支持
287 | 
288 | - 项目源码: https://github.com/tianrking/robstride-control
289 | - 问题报告: https://github.com/tianrking/robstride-control/issues


--------------------------------------------------------------------------------
/python/src/speed_control.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python3
  2 | # -*- coding: utf-8 -*-
  3 | """
  4 | RobStride 速度模式控制脚本 (修复版)
  5 | 模式: Mode 2 (Speed Control Mode)
  6 | 通信: 使用 WRITE_PARAMETER 更新 VELOCITY_TARGET (spd_ref)
  7 | 
  8 | 用法: python3 speed_control.py <motor_id>
  9 | """
 10 | 
 11 | import sys
 12 | import os
 13 | import time
 14 | import threading
 15 | import signal
 16 | from typing import Optional
 17 | 
 18 | # 尝试导入 SDK
 19 | sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
 20 | try:
 21 |     from robstride_dynamics import RobstrideBus, Motor, ParameterType
 22 | except ImportError:
 23 |     # 假设当前目录结构
 24 |     try:
 25 |         from bus import RobstrideBus, Motor
 26 |         from protocol import ParameterType
 27 |     except ImportError as e:
 28 |         print(f"❌ 无法导入 SDK: {e}")
 29 |         sys.exit(1)
 30 | 
 31 | class SpeedController:
 32 |     def __init__(self, motor_id: int, channel='can0'):
 33 |         self.motor_id = motor_id
 34 |         self.motor_name = f"motor_{motor_id}"
 35 |         self.channel = channel
 36 |         
 37 |         self.bus: Optional[RobstrideBus] = None
 38 |         self.lock = threading.Lock()  # 互斥锁，防止Socket冲突
 39 |         
 40 |         self.running = True
 41 |         self.connected = False
 42 |         self.target_velocity = 0.0
 43 |         self.current_status = None
 44 |         
 45 |         # 默认参数
 46 |         self.max_velocity = 20.0  # rad/s 安全限制
 47 |         self.kp = 2.0
 48 |         self.ki = 0.5
 49 | 
 50 |     def _signal_handler(self, signum, frame):
 51 |         self.stop_and_exit()
 52 | 
 53 |     def connect(self):
 54 |         print(f"🔍 正在连接 CAN 通道 {self.channel}...")
 55 |         
 56 |         # 定义电机
 57 |         motors = {
 58 |             self.motor_name: Motor(id=self.motor_id, model="rs-06") # 模型型号请根据实际修改
 59 |         }
 60 |         
 61 |         # 简单的校准参数
 62 |         calibration = {
 63 |             self.motor_name: {"direction": 1, "homing_offset": 0.0}
 64 |         }
 65 | 
 66 |         try:
 67 |             self.bus = RobstrideBus(self.channel, motors, calibration)
 68 |             self.bus.connect(handshake=True)
 69 |             
 70 |             # 激活电机
 71 |             print(f"⚡ 激活电机 ID: {self.motor_id} ...")
 72 |             self.bus.enable(self.motor_name)
 73 |             time.sleep(0.5)
 74 | 
 75 |             # 设置为速度模式 (Mode 2)
 76 |             print("⚙️ 设置为速度控制模式 (Mode 2)...")
 77 |             self.bus.write(self.motor_name, ParameterType.MODE, 2)
 78 |             
 79 |             # 初始化 PID 和限制
 80 |             print("⚙️ 写入控制参数...")
 81 |             self.bus.write(self.motor_name, ParameterType.VELOCITY_LIMIT, self.max_velocity)
 82 |             self.bus.write(self.motor_name, ParameterType.VELOCITY_KP, self.kp)
 83 |             self.bus.write(self.motor_name, ParameterType.VELOCITY_KI, self.ki)
 84 |             
 85 |             # 归零目标
 86 |             self.bus.write(self.motor_name, ParameterType.VELOCITY_TARGET, 0.0)
 87 |             
 88 |             self.connected = True
 89 |             print("✅ 初始化完成！")
 90 |             return True
 91 |             
 92 |         except Exception as e:
 93 |             print(f"❌ 连接失败: {e}")
 94 |             return False
 95 | 
 96 |     def loop(self):
 97 |         """控制线程：持续发送心跳/速度指令并读取状态"""
 98 |         print("🔄 控制循环已启动")
 99 |         
100 |         while self.running and self.connected:
101 |             try:
102 |                 with self.lock:
103 |                     # 在 Mode 2 下，我们需要写入 VELOCITY_TARGET
104 |                     # bus.write 会等待回包 (receive_status_frame)，所以这本身就是一种状态读取
105 |                     # Protocol 0x700A = VELOCITY_TARGET
106 |                     
107 |                     self.bus.write(self.motor_name, ParameterType.VELOCITY_TARGET, self.target_velocity)
108 |                     
109 |                     # 如果想读取更详细的状态（如当前扭矩），可以使用 read_operation_frame
110 |                     # 但 write 的回包里其实已经包含 status 数据了，SDK 的 write 内部调用了 receive_status_frame
111 |                     # 这里我们不做额外的读取以保持高频率
112 |                     
113 |                 time.sleep(0.05) # 20Hz 刷新率，防止总线拥堵
114 |                 
115 |             except Exception as e:
116 |                 print(f"⚠️ 通信错误: {e}")
117 |                 time.sleep(0.5)
118 | 
119 |     def set_velocity(self, vel: float):
120 |         """设置目标速度（带限幅）"""
121 |         vel = max(-self.max_velocity, min(self.max_velocity, vel))
122 |         self.target_velocity = vel
123 |         print(f" -> 目标设定: {self.target_velocity:.2f} rad/s")
124 | 
125 |     def stop_and_exit(self):
126 |         print("\n🛑 正在停止...")
127 |         self.running = False
128 |         self.target_velocity = 0.0
129 |         
130 |         if self.bus and self.connected:
131 |             try:
132 |                 with self.lock:
133 |                     # 先停
134 |                     self.bus.write(self.motor_name, ParameterType.VELOCITY_TARGET, 0.0)
135 |                     time.sleep(0.2)
136 |                     # 禁用
137 |                     self.bus.disable(self.motor_name)
138 |             except Exception:
139 |                 pass
140 |             self.bus.disconnect()
141 |         sys.exit(0)
142 | 
143 |     def run_interactive(self):
144 |         # 启动后台发送线程
145 |         t = threading.Thread(target=self.loop, daemon=True)
146 |         t.start()
147 | 
148 |         print("\n" + "="*40)
149 |         print(f"🎮 速度控制台 (ID: {self.motor_id})")
150 |         print("="*40)
151 |         print("👉 直接输入数字 (rad/s) 回车即可改变速度")
152 |         print("👉 输入 '0' 停止")
153 |         print("👉 输入 'q' 退出")
154 |         print(f"⚠️  当前安全限速: ±{self.max_velocity} rad/s")
155 |         print("-" * 40)
156 | 
157 |         while True:
158 |             try:
159 |                 cmd = input(f"[{self.target_velocity:.1f} rad/s] >> ").strip().lower()
160 |                 
161 |                 if not cmd:
162 |                     continue
163 |                     
164 |                 if cmd in ['q', 'quit', 'exit']:
165 |                     break
166 |                 
167 |                 try:
168 |                     vel = float(cmd)
169 |                     self.set_velocity(vel)
170 |                 except ValueError:
171 |                     print("❌ 无效输入，请输入数字")
172 | 
173 |             except KeyboardInterrupt:
174 |                 break
175 |         
176 |         self.stop_and_exit()
177 | 
178 | def main():
179 |     if len(sys.argv) < 2:
180 |         print("用法: python3 speed_control.py <motor_id>")
181 |         sys.exit(1)
182 |         
183 |     motor_id = int(sys.argv[1])
184 |     
185 |     controller = SpeedController(motor_id)
186 |     signal.signal(signal.SIGINT, controller._signal_handler)
187 |     
188 |     if controller.connect():
189 |         controller.run_interactive()
190 | 
191 | if __name__ == "__main__":
192 |     main()


--------------------------------------------------------------------------------
/arduino/dual_motor_control/TWAI_CAN_MI_Motor.h:
--------------------------------------------------------------------------------
  1 | #ifndef _TWAI_CAN_MI_MOTOR_H__
  2 | #define _TWAI_CAN_MI_MOTOR_H__
  3 | 
  4 | /*
  5 |     小米电机驱动器通信协议及使用说明
  6 | 
  7 |     电机通信为 CAN 2.0 通信接口，波特率 1Mbps，采用扩展帧格式，如下所示：
  8 |     数据域          29 位 ID        8Byte 数据区
  9 |     大小 Bit28~bit24 bit23~8 bit7~0 Byte0~Byte7
 10 |     描述 通信类型 数据区 2 目标地址 数据区 1
 11 | 
 12 |     电机支持的控制模式包括：
 13 |     运控模式：给定电机运控 5 个参数；
 14 |     电流模式：给定电机指定的 Iq 电流；
 15 |     速度模式：给定电机指定的运行速度；
 16 |     位置模式：给定电机指定的位置，电机将运行到该指定的位置；
 17 | 
 18 |     本台测试用的小米电机CAN ID=1，主机ID=0
 19 | 
 20 |     注意配置顺序：
 21 |         1. 电机CAN初始化->电机ID初始化->设定电机机械零点->设置电机控制模式->设置对应模式的控制参数->使能(启动)电机
 22 |         2. 必须给电机发数据才会有数据返回
 23 | 
 24 |     测试用例：
 25 |     MI_Motor_ M1_con;
 26 | 
 27 |     void setup() {
 28 |         Motor_CAN_Init();
 29 |         M1_con.Motor_Con_Init(MOTER_1_ID);
 30 |         M1_con.Motor_Set_Zero();
 31 |         M1_con.Change_Mode(SPEED_MODE);
 32 |         M1_con.Motor_Enable();
 33 |     }
 34 | 
 35 |     void loop() {
 36 |         M1_con.Motor_Data_Updata(20);
 37 |         Serial.printf("M1数据: %d,%d,%d,%d,%d,%d,%d,%d,%d,angle:%.2f,speed:%.2f,torque:%.2f,temp:%.2f,\r\n",
 38 |                                       M1_con.motor_rx_data.master_id,M1_con.motor_rx_data.motor_id,
 39 |                                       M1_con.motor_rx_data.err_sta,M1_con.motor_rx_data.HALL_err,
 40 |                                       M1_con.motor_rx_data.magnet_err,M1_con.motor_rx_data.temp_err,
 41 |                                       M1_con.motor_rx_data.current_err,M1_con.motor_rx_data.voltage_err,
 42 |                                       M1_con.motor_rx_data.mode_sta,
 43 |                                       M1_con.motor_rx_data.cur_angle,M1_con.motor_rx_data.cur_speed,
 44 |                                       M1_con.motor_rx_data.cur_torque,M1_con.motor_rx_data.cur_temp);
 45 |         M1_con.Set_SpeedMode(-1);
 46 |         vTaskDelay(20);
 47 |     }
 48 | */
 49 | 
 50 | #include "Arduino.h"
 51 | #include <ESP32-TWAI-CAN.hpp>
 52 | 
 53 | /*CAN设置*/
 54 | #define CAN_TX          5
 55 | #define CAN_RX          4
 56 | 
 57 | #define MASTER_ID       0
 58 | // 电机ID通过动态配置设置，这里不再定义固定值
 59 | 
 60 | /*基础配置*/
 61 | #define P_MIN           -12.5f
 62 | #define P_MAX           12.5f
 63 | #define V_MIN           -30.0f
 64 | #define V_MAX           30.0f
 65 | #define KP_MIN          0.0f
 66 | #define KP_MAX          500.0f
 67 | #define KD_MIN          0.0f
 68 | #define KD_MAX          5.0f
 69 | #define T_MIN           -12.0f
 70 | #define T_MAX           12.0f
 71 | 
 72 | /*数据配置*/
 73 | #define RUN_MODE        0x7005    // 运行模式, uint8, 1字节, 0运控模式,1位置模式，2速度模式，3电流模式
 74 | #define CTRL_MODE       0
 75 | #define POS_MODE        1
 76 | #define SPEED_MODE      2
 77 | #define CUR_MODE        3
 78 | #define IQ_REF          0x7006    // 电流模式 Iq 指令, float, 4字节, -23~23A
 79 | #define SPD_REF         0x700A    // 转速模式转速指令, float, 4字节, -30~30rad/s
 80 | #define LIMIT_TORQUE    0x700B    // 转矩限制, float, 4字节, 0~12Nm
 81 | #define CUR_KP          0x7010    // 电流的 Kp, float, 4字节, 默认值 0.125
 82 | #define CUR_KI          0x7011    // 电流的 Ki, float, 4字节, 默认值 0.0158
 83 | #define CUR_FILT_GAIN   0x7014    // 电流滤波系数 filt_gain, float, 4字节, 0~1.0，默认值 0.1
 84 | #define LOC_REF         0x7016    // 位置模式角度指令, float, 4字节, rad
 85 | #define LIMIT_SPD       0x7017    // 位置模式速度限制, float, 4字节, 0~30rad/s
 86 | #define LIMIT_CUR       0x7018    // 速度位置模式电流限制, float, 4字节, 0~23A
 87 | #define MECH_POS        0x7019    // 负载端计圈机械角度, float, 4字节, rad, 只读
 88 | #define IQF             0x701A    // iq 滤波值, float, 4字节, -23~23A, 只读
 89 | #define MECH_VEL        0x701B    // 负载端转速, float, 4字节, -30~30rad/s, 只读
 90 | #define VBUS            0x701C    // 母线电压, float, 4字节, V, 只读
 91 | #define ROTATION        0x701D    // 圈数, int16, 2字节, 圈数, 可读写
 92 | #define LOC_KP          0x701E    // 位置的 kp, float, 4字节, 默认值 30, 可读写
 93 | #define SPD_KP          0x701F    // 速度的 kp, float, 4字节, 默认值 1, 可读写
 94 | #define SPD_KI          0x7020    // 速度的 ki, float, 4字节, 默认值 0.002, 可读写
 95 | 
 96 | //拆分29位ID
 97 | #define RX_29ID_DISASSEMBLE_MASTER_ID(id)       (uint8_t)((id)&0xFF)
 98 | #define RX_29ID_DISASSEMBLE_MOTOR_ID(id)        (uint8_t)(((id)>>8)&0xFF)
 99 | //右移16位获取所有故障信息(bit21-16，共6位，111111->0x3F)，三元运算符判断是否有故障。0无1有
100 | #define RX_29ID_DISASSEMBLE_ERR_STA(id)         (uint8_t)(((((id)>>16)&0x3F) > 0) ? 1 : 0)
101 | #define RX_29ID_DISASSEMBLE_HALL_ERR(id)        (uint8_t)(((id)>>20)&0X01)
102 | #define RX_29ID_DISASSEMBLE_MAGNET_ERR(id)      (uint8_t)(((id)>>19)&0x01)
103 | #define RX_29ID_DISASSEMBLE_TEMP_ERR(id)        (uint8_t)(((id)>>18)&0x01)
104 | #define RX_29ID_DISASSEMBLE_CURRENT_ERR(id)     (uint8_t)(((id)>>17)&0x01)
105 | #define RX_29ID_DISASSEMBLE_VOLTAGE_ERR(id)     (uint8_t)(((id)>>16)&0x01)
106 | //模式状态: 0:Reset模式[复位]; 1:Cali 模式[标定]; 2:Motor模式[运行]
107 | #define RX_29ID_DISASSEMBLE_MODE_STA(id)        (uint8_t)(((id)>>22)&0x03)
108 | 
109 | //拆分数据
110 | #define RX_DATA_DISASSEMBLE_CUR_ANGLE(data)     (uint16_t)((data[0]<<8)|data[1])
111 | #define RX_DATA_DISASSEMBLE_CUR_SPEED(data)     (uint16_t)((data[2]<<8)|data[3])
112 | #define RX_DATA_DISASSEMBLE_CUR_TORQUE(data)    (uint16_t)((data[4]<<8)|data[5])
113 | #define RX_DATA_DISASSEMBLE_CUR_TEMP(data)      (uint16_t)((data[6]<<8)|data[7])
114 | 
115 | //转换系数
116 | //转换过程：angle=data×8PI/65535-4PI,   范围：+-4PI
117 | #define INT2ANGLE     0.000383501049
118 | //转换过程：speed=data×60/65535-30,     范围：+-30rad/s
119 | #define INT2SPEED     0.000915541314
120 | //转换过程：torque=data×24/65535-12,    范围：+-12N·m
121 | #define INT2TORQUE    0.000366216526
122 | 
123 | //发送数据包
124 | typedef struct {
125 |   uint32_t id:8;      //8位CAN ID
126 |   uint32_t data:16;   //16位数据
127 |   uint32_t mode:5;    //5位模式
128 |   uint32_t res:3;     //3位保留
129 |   uint8_t tx_data[8];
130 | } can_frame_t;
131 | 
132 | //解析返回数据包
133 | typedef struct {
134 |   //29位ID解码状态
135 |   uint8_t master_id;
136 |   uint8_t motor_id;
137 |   uint8_t err_sta;
138 |   uint8_t HALL_err;
139 |   uint8_t magnet_err;
140 |   uint8_t temp_err;
141 |   uint8_t current_err;
142 |   uint8_t voltage_err;
143 |   uint8_t mode_sta;
144 | 
145 |   //具体数据
146 |   float cur_angle;      //(-4π~4π)
147 |   float cur_speed;      //(-30rad/s~30rad/s)
148 |   float cur_torque;     //（-12Nm~12Nm）
149 |   float cur_temp;       //Temp(摄氏度）*10
150 | } can_rx_frame_t;
151 | 
152 | class MI_Motor_
153 | {
154 |     private:
155 |         uint8_t id;
156 |         CanFrame rxFrame;               //接收原始数据
157 | 
158 |     public:
159 |         can_rx_frame_t motor_rx_data;   //返回解析数据
160 | 
161 |         void Motor_Enable(void);
162 |         void Motor_Reset(void);
163 |         void Motor_Set_Zero(void);
164 |         void Motor_ControlMode(float torque, float MechPosition, float speed, float kp, float kd);
165 |         void Set_SingleParameter(uint16_t parameter, float index);
166 |         void Decoding_Motor_data(CanFrame* can_Frame_point, can_rx_frame_t* motor_frame_point);
167 |         void Set_SpeedMode(float speed);
168 |         void Set_PosMode(float position, float max_speed);
169 |         void Set_CurMode(float current);
170 |         void Change_Mode(uint8_t mode);
171 |         void Motor_Con_Init(uint8_t motor_id);
172 |         uint8_t Motor_Data_Updata(uint32_t timeout);
173 | };
174 | 
175 | extern void Motor_CAN_Init(void);
176 | 
177 | #endif


--------------------------------------------------------------------------------
/arduino/mi_motor_control/TWAI_CAN_MI_Motor.h:
--------------------------------------------------------------------------------
  1 | #ifndef _TWAI_CAN_MI_MOTOR_H__
  2 | #define _TWAI_CAN_MI_MOTOR_H__
  3 | 
  4 | /*
  5 |     小米电机驱动器通信协议及使用说明
  6 | 
  7 |     电机通信为 CAN 2.0 通信接口，波特率 1Mbps，采用扩展帧格式，如下所示：
  8 |     数据域          29 位 ID        8Byte 数据区
  9 |     大小 Bit28~bit24 bit23~8 bit7~0 Byte0~Byte7
 10 |     描述 通信类型 数据区 2 目标地址 数据区 1
 11 | 
 12 |     电机支持的控制模式包括：
 13 |     运控模式：给定电机运控 5 个参数；
 14 |     电流模式：给定电机指定的 Iq 电流；
 15 |     速度模式：给定电机指定的运行速度；
 16 |     位置模式：给定电机指定的位置，电机将运行到该指定的位置；
 17 | 
 18 |     本台测试用的小米电机CAN ID=1，主机ID=0
 19 | 
 20 |     注意配置顺序：
 21 |         1. 电机CAN初始化->电机ID初始化->设定电机机械零点->设置电机控制模式->设置对应模式的控制参数->使能(启动)电机
 22 |         2. 必须给电机发数据才会有数据返回
 23 | 
 24 |     测试用例：
 25 |     MI_Motor_ M1_con;
 26 | 
 27 |     void setup() {
 28 |         Motor_CAN_Init();
 29 |         M1_con.Motor_Con_Init(MOTER_1_ID);
 30 |         M1_con.Motor_Set_Zero();
 31 |         M1_con.Change_Mode(SPEED_MODE);
 32 |         M1_con.Motor_Enable();
 33 |     }
 34 | 
 35 |     void loop() {
 36 |         M1_con.Motor_Data_Updata(20);
 37 |         Serial.printf("M1数据: %d,%d,%d,%d,%d,%d,%d,%d,%d,angle:%.2f,speed:%.2f,torque:%.2f,temp:%.2f,\r\n",
 38 |                                       M1_con.motor_rx_data.master_id,M1_con.motor_rx_data.motor_id,
 39 |                                       M1_con.motor_rx_data.err_sta,M1_con.motor_rx_data.HALL_err,
 40 |                                       M1_con.motor_rx_data.magnet_err,M1_con.motor_rx_data.temp_err,
 41 |                                       M1_con.motor_rx_data.current_err,M1_con.motor_rx_data.voltage_err,
 42 |                                       M1_con.motor_rx_data.mode_sta,
 43 |                                       M1_con.motor_rx_data.cur_angle,M1_con.motor_rx_data.cur_speed,
 44 |                                       M1_con.motor_rx_data.cur_torque,M1_con.motor_rx_data.cur_temp);
 45 |         M1_con.Set_SpeedMode(-1);
 46 |         vTaskDelay(20);
 47 |     }
 48 | */
 49 | 
 50 | #include "Arduino.h"
 51 | #include <ESP32-TWAI-CAN.hpp>
 52 | 
 53 | /*CAN设置*/
 54 | #define CAN_TX          5
 55 | #define CAN_RX          4
 56 | 
 57 | #define MASTER_ID       0
 58 | #define MOTER_1_ID      11
 59 | #define MOTER_2_ID      2
 60 | 
 61 | /*基础配置*/
 62 | #define P_MIN           -12.5f
 63 | #define P_MAX           12.5f
 64 | #define V_MIN           -30.0f
 65 | #define V_MAX           30.0f
 66 | #define KP_MIN          0.0f
 67 | #define KP_MAX          500.0f
 68 | #define KD_MIN          0.0f
 69 | #define KD_MAX          5.0f
 70 | #define T_MIN           -12.0f
 71 | #define T_MAX           12.0f
 72 | 
 73 | /*数据配置*/
 74 | #define RUN_MODE        0x7005    // 运行模式, uint8, 1字节, 0运控模式,1位置模式，2速度模式，3电流模式
 75 | #define CTRL_MODE       0
 76 | #define POS_MODE        1
 77 | #define SPEED_MODE      2
 78 | #define CUR_MODE        3
 79 | #define IQ_REF          0x7006    // 电流模式 Iq 指令, float, 4字节, -23~23A
 80 | #define SPD_REF         0x700A    // 转速模式转速指令, float, 4字节, -30~30rad/s
 81 | #define LIMIT_TORQUE    0x700B    // 转矩限制, float, 4字节, 0~12Nm
 82 | #define CUR_KP          0x7010    // 电流的 Kp, float, 4字节, 默认值 0.125
 83 | #define CUR_KI          0x7011    // 电流的 Ki, float, 4字节, 默认值 0.0158
 84 | #define CUR_FILT_GAIN   0x7014    // 电流滤波系数 filt_gain, float, 4字节, 0~1.0，默认值 0.1
 85 | #define LOC_REF         0x7016    // 位置模式角度指令, float, 4字节, rad
 86 | #define LIMIT_SPD       0x7017    // 位置模式速度限制, float, 4字节, 0~30rad/s
 87 | #define LIMIT_CUR       0x7018    // 速度位置模式电流限制, float, 4字节, 0~23A
 88 | #define MECH_POS        0x7019    // 负载端计圈机械角度, float, 4字节, rad, 只读
 89 | #define IQF             0x701A    // iq 滤波值, float, 4字节, -23~23A, 只读
 90 | #define MECH_VEL        0x701B    // 负载端转速, float, 4字节, -30~30rad/s, 只读
 91 | #define VBUS            0x701C    // 母线电压, float, 4字节, V, 只读
 92 | #define ROTATION        0x701D    // 圈数, int16, 2字节, 圈数, 可读写
 93 | #define LOC_KP          0x701E    // 位置的 kp, float, 4字节, 默认值 30, 可读写
 94 | #define SPD_KP          0x701F    // 速度的 kp, float, 4字节, 默认值 1, 可读写
 95 | #define SPD_KI          0x7020    // 速度的 ki, float, 4字节, 默认值 0.002, 可读写
 96 | 
 97 | //拆分29位ID
 98 | #define RX_29ID_DISASSEMBLE_MASTER_ID(id)       (uint8_t)((id)&0xFF)
 99 | #define RX_29ID_DISASSEMBLE_MOTOR_ID(id)        (uint8_t)(((id)>>8)&0xFF)
100 | //右移16位获取所有故障信息(bit21-16，共6位，111111->0x3F)，三元运算符判断是否有故障。0无1有
101 | #define RX_29ID_DISASSEMBLE_ERR_STA(id)         (uint8_t)(((((id)>>16)&0x3F) > 0) ? 1 : 0)
102 | #define RX_29ID_DISASSEMBLE_HALL_ERR(id)        (uint8_t)(((id)>>20)&0X01)
103 | #define RX_29ID_DISASSEMBLE_MAGNET_ERR(id)      (uint8_t)(((id)>>19)&0x01)
104 | #define RX_29ID_DISASSEMBLE_TEMP_ERR(id)        (uint8_t)(((id)>>18)&0x01)
105 | #define RX_29ID_DISASSEMBLE_CURRENT_ERR(id)     (uint8_t)(((id)>>17)&0x01)
106 | #define RX_29ID_DISASSEMBLE_VOLTAGE_ERR(id)     (uint8_t)(((id)>>16)&0x01)
107 | //模式状态: 0:Reset模式[复位]; 1:Cali 模式[标定]; 2:Motor模式[运行]
108 | #define RX_29ID_DISASSEMBLE_MODE_STA(id)        (uint8_t)(((id)>>22)&0x03)
109 | 
110 | //拆分数据
111 | #define RX_DATA_DISASSEMBLE_CUR_ANGLE(data)     (uint16_t)((data[0]<<8)|data[1])
112 | #define RX_DATA_DISASSEMBLE_CUR_SPEED(data)     (uint16_t)((data[2]<<8)|data[3])
113 | #define RX_DATA_DISASSEMBLE_CUR_TORQUE(data)    (uint16_t)((data[4]<<8)|data[5])
114 | #define RX_DATA_DISASSEMBLE_CUR_TEMP(data)      (uint16_t)((data[6]<<8)|data[7])
115 | 
116 | //转换系数
117 | //转换过程：angle=data×8PI/65535-4PI,   范围：+-4PI
118 | #define INT2ANGLE     0.000383501049
119 | //转换过程：speed=data×60/65535-30,     范围：+-30rad/s
120 | #define INT2SPEED     0.000915541314
121 | //转换过程：torque=data×24/65535-12,    范围：+-12N·m
122 | #define INT2TORQUE    0.000366216526
123 | 
124 | //发送数据包
125 | typedef struct {
126 |   uint32_t id:8;      //8位CAN ID
127 |   uint32_t data:16;   //16位数据
128 |   uint32_t mode:5;    //5位模式
129 |   uint32_t res:3;     //3位保留
130 |   uint8_t tx_data[8];
131 | } can_frame_t;
132 | 
133 | //解析返回数据包
134 | typedef struct {
135 |   //29位ID解码状态
136 |   uint8_t master_id;
137 |   uint8_t motor_id;
138 |   uint8_t err_sta;
139 |   uint8_t HALL_err;
140 |   uint8_t magnet_err;
141 |   uint8_t temp_err;
142 |   uint8_t current_err;
143 |   uint8_t voltage_err;
144 |   uint8_t mode_sta;
145 | 
146 |   //具体数据
147 |   float cur_angle;      //(-4π~4π)
148 |   float cur_speed;      //(-30rad/s~30rad/s)
149 |   float cur_torque;     //（-12Nm~12Nm）
150 |   float cur_temp;       //Temp(摄氏度）*10
151 | } can_rx_frame_t;
152 | 
153 | class MI_Motor_
154 | {
155 |     private:
156 |         uint8_t id;
157 |         CanFrame rxFrame;               //接收原始数据
158 | 
159 |     public:
160 |         can_rx_frame_t motor_rx_data;   //返回解析数据
161 | 
162 |         void Motor_Enable(void);
163 |         void Motor_Reset(void);
164 |         void Motor_Set_Zero(void);
165 |         void Motor_ControlMode(float torque, float MechPosition, float speed, float kp, float kd);
166 |         void Set_SingleParameter(uint16_t parameter, float index);
167 |         void Decoding_Motor_data(CanFrame* can_Frame_point, can_rx_frame_t* motor_frame_point);
168 |         void Set_SpeedMode(float speed);
169 |         void Set_PosMode(float position, float max_speed);
170 |         void Set_CurMode(float current);
171 |         void Change_Mode(uint8_t mode);
172 |         void Motor_Con_Init(uint8_t motor_id);
173 |         uint8_t Motor_Data_Updata(uint32_t timeout);
174 | };
175 | 
176 | extern void Motor_CAN_Init(void);
177 | 
178 | #endif


--------------------------------------------------------------------------------
/arduino/sim_setup_motor/TWAI_CAN_MI_Motor.h:
--------------------------------------------------------------------------------
  1 | #ifndef _TWAI_CAN_MI_MOTOR_H__
  2 | #define _TWAI_CAN_MI_MOTOR_H__
  3 | 
  4 | /*
  5 |     小米电机驱动器通信协议及使用说明
  6 | 
  7 |     电机通信为 CAN 2.0 通信接口，波特率 1Mbps，采用扩展帧格式，如下所示：
  8 |     数据域          29 位 ID        8Byte 数据区
  9 |     大小 Bit28~bit24 bit23~8 bit7~0 Byte0~Byte7
 10 |     描述 通信类型 数据区 2 目标地址 数据区 1
 11 | 
 12 |     电机支持的控制模式包括：
 13 |     运控模式：给定电机运控 5 个参数；
 14 |     电流模式：给定电机指定的 Iq 电流；
 15 |     速度模式：给定电机指定的运行速度；
 16 |     位置模式：给定电机指定的位置，电机将运行到该指定的位置；
 17 | 
 18 |     本台测试用的小米电机CAN ID=1，主机ID=0
 19 | 
 20 |     注意配置顺序：
 21 |         1. 电机CAN初始化->电机ID初始化->设定电机机械零点->设置电机控制模式->设置对应模式的控制参数->使能(启动)电机
 22 |         2. 必须给电机发数据才会有数据返回
 23 | 
 24 |     测试用例：
 25 |     MI_Motor_ M1_con;
 26 | 
 27 |     void setup() {
 28 |         Motor_CAN_Init();
 29 |         M1_con.Motor_Con_Init(MOTER_1_ID);
 30 |         M1_con.Motor_Set_Zero();
 31 |         M1_con.Change_Mode(SPEED_MODE);
 32 |         M1_con.Motor_Enable();
 33 |     }
 34 | 
 35 |     void loop() {
 36 |         M1_con.Motor_Data_Updata(20);
 37 |         Serial.printf("M1数据: %d,%d,%d,%d,%d,%d,%d,%d,%d,angle:%.2f,speed:%.2f,torque:%.2f,temp:%.2f,\r\n",
 38 |                                       M1_con.motor_rx_data.master_id,M1_con.motor_rx_data.motor_id,
 39 |                                       M1_con.motor_rx_data.err_sta,M1_con.motor_rx_data.HALL_err,
 40 |                                       M1_con.motor_rx_data.magnet_err,M1_con.motor_rx_data.temp_err,
 41 |                                       M1_con.motor_rx_data.current_err,M1_con.motor_rx_data.voltage_err,
 42 |                                       M1_con.motor_rx_data.mode_sta,
 43 |                                       M1_con.motor_rx_data.cur_angle,M1_con.motor_rx_data.cur_speed,
 44 |                                       M1_con.motor_rx_data.cur_torque,M1_con.motor_rx_data.cur_temp);
 45 |         M1_con.Set_SpeedMode(-1);
 46 |         vTaskDelay(20);
 47 |     }
 48 | */
 49 | 
 50 | #include "Arduino.h"
 51 | #include <ESP32-TWAI-CAN.hpp>
 52 | 
 53 | /*CAN设置*/
 54 | #define CAN_TX          5
 55 | #define CAN_RX          4
 56 | 
 57 | #define MASTER_ID       0
 58 | #define MOTER_1_ID      1
 59 | #define MOTER_2_ID      2
 60 | 
 61 | /*基础配置*/
 62 | #define P_MIN           -12.5f
 63 | #define P_MAX           12.5f
 64 | #define V_MIN           -30.0f
 65 | #define V_MAX           30.0f
 66 | #define KP_MIN          0.0f
 67 | #define KP_MAX          500.0f
 68 | #define KD_MIN          0.0f
 69 | #define KD_MAX          5.0f
 70 | #define T_MIN           -12.0f
 71 | #define T_MAX           12.0f
 72 | 
 73 | /*数据配置*/
 74 | #define RUN_MODE        0x7005    // 运行模式, uint8, 1字节, 0运控模式,1位置模式，2速度模式，3电流模式
 75 | #define CTRL_MODE       0
 76 | #define POS_MODE        1
 77 | #define SPEED_MODE      2
 78 | #define CUR_MODE        3
 79 | #define IQ_REF          0x7006    // 电流模式 Iq 指令, float, 4字节, -23~23A
 80 | #define SPD_REF         0x700A    // 转速模式转速指令, float, 4字节, -30~30rad/s
 81 | #define LIMIT_TORQUE    0x700B    // 转矩限制, float, 4字节, 0~12Nm
 82 | #define CUR_KP          0x7010    // 电流的 Kp, float, 4字节, 默认值 0.125
 83 | #define CUR_KI          0x7011    // 电流的 Ki, float, 4字节, 默认值 0.0158
 84 | #define CUR_FILT_GAIN   0x7014    // 电流滤波系数 filt_gain, float, 4字节, 0~1.0，默认值 0.1
 85 | #define LOC_REF         0x7016    // 位置模式角度指令, float, 4字节, rad
 86 | #define LIMIT_SPD       0x7017    // 位置模式速度限制, float, 4字节, 0~30rad/s
 87 | #define LIMIT_CUR       0x7018    // 速度位置模式电流限制, float, 4字节, 0~23A
 88 | #define MECH_POS        0x7019    // 负载端计圈机械角度, float, 4字节, rad, 只读
 89 | #define IQF             0x701A    // iq 滤波值, float, 4字节, -23~23A, 只读
 90 | #define MECH_VEL        0x701B    // 负载端转速, float, 4字节, -30~30rad/s, 只读
 91 | #define VBUS            0x701C    // 母线电压, float, 4字节, V, 只读
 92 | #define ROTATION        0x701D    // 圈数, int16, 2字节, 圈数, 可读写
 93 | #define LOC_KP          0x701E    // 位置的 kp, float, 4字节, 默认值 30, 可读写
 94 | #define SPD_KP          0x701F    // 速度的 kp, float, 4字节, 默认值 1, 可读写
 95 | #define SPD_KI          0x7020    // 速度的 ki, float, 4字节, 默认值 0.002, 可读写
 96 | 
 97 | //拆分29位ID
 98 | #define RX_29ID_DISASSEMBLE_MASTER_ID(id)       (uint8_t)((id)&0xFF)
 99 | #define RX_29ID_DISASSEMBLE_MOTOR_ID(id)        (uint8_t)(((id)>>8)&0xFF)
100 | //右移16位获取所有故障信息(bit21-16，共6位，111111->0x3F)，三元运算符判断是否有故障。0无1有
101 | #define RX_29ID_DISASSEMBLE_ERR_STA(id)         (uint8_t)(((((id)>>16)&0x3F) > 0) ? 1 : 0)
102 | #define RX_29ID_DISASSEMBLE_HALL_ERR(id)        (uint8_t)(((id)>>20)&0X01)
103 | #define RX_29ID_DISASSEMBLE_MAGNET_ERR(id)      (uint8_t)(((id)>>19)&0x01)
104 | #define RX_29ID_DISASSEMBLE_TEMP_ERR(id)        (uint8_t)(((id)>>18)&0x01)
105 | #define RX_29ID_DISASSEMBLE_CURRENT_ERR(id)     (uint8_t)(((id)>>17)&0x01)
106 | #define RX_29ID_DISASSEMBLE_VOLTAGE_ERR(id)     (uint8_t)(((id)>>16)&0x01)
107 | //模式状态: 0:Reset模式[复位]; 1:Cali 模式[标定]; 2:Motor模式[运行]
108 | #define RX_29ID_DISASSEMBLE_MODE_STA(id)        (uint8_t)(((id)>>22)&0x03)
109 | 
110 | //拆分数据
111 | #define RX_DATA_DISASSEMBLE_CUR_ANGLE(data)     (uint16_t)((data[0]<<8)|data[1])
112 | #define RX_DATA_DISASSEMBLE_CUR_SPEED(data)     (uint16_t)((data[2]<<8)|data[3])
113 | #define RX_DATA_DISASSEMBLE_CUR_TORQUE(data)    (uint16_t)((data[4]<<8)|data[5])
114 | #define RX_DATA_DISASSEMBLE_CUR_TEMP(data)      (uint16_t)((data[6]<<8)|data[7])
115 | 
116 | //转换系数
117 | //转换过程：angle=data×8PI/65535-4PI,   范围：+-4PI
118 | #define INT2ANGLE     0.000383501049
119 | //转换过程：speed=data×60/65535-30,     范围：+-30rad/s
120 | #define INT2SPEED     0.000915541314
121 | //转换过程：torque=data×24/65535-12,    范围：+-12N·m
122 | #define INT2TORQUE    0.000366216526
123 | 
124 | //发送数据包
125 | typedef struct {
126 |   uint32_t id:8;      //8位CAN ID
127 |   uint32_t data:16;   //16位数据
128 |   uint32_t mode:5;    //5位模式
129 |   uint32_t res:3;     //3位保留
130 |   uint8_t tx_data[8];
131 | } can_frame_t;
132 | 
133 | //解析返回数据包
134 | typedef struct {
135 |   //29位ID解码状态
136 |   uint8_t master_id;
137 |   uint8_t motor_id;
138 |   uint8_t err_sta;
139 |   uint8_t HALL_err;
140 |   uint8_t magnet_err;
141 |   uint8_t temp_err;
142 |   uint8_t current_err;
143 |   uint8_t voltage_err;
144 |   uint8_t mode_sta;
145 | 
146 |   //具体数据
147 |   float cur_angle;      //(-4π~4π)
148 |   float cur_speed;      //(-30rad/s~30rad/s)
149 |   float cur_torque;     //（-12Nm~12Nm）
150 |   float cur_temp;       //Temp(摄氏度）*10
151 | } can_rx_frame_t;
152 | 
153 | class MI_Motor_
154 | {
155 |     private:
156 |         uint8_t id;
157 |         CanFrame rxFrame;               //接收原始数据
158 | 
159 |     public:
160 |         can_rx_frame_t motor_rx_data;   //返回解析数据
161 | 
162 |         void Motor_Enable(void);
163 |         void Motor_Reset(void);
164 |         void Motor_Set_Zero(void);
165 |         void Motor_ControlMode(float torque, float MechPosition, float speed, float kp, float kd);
166 |         void Set_SingleParameter(uint16_t parameter, float index);
167 |         void Decoding_Motor_data(CanFrame* can_Frame_point, can_rx_frame_t* motor_frame_point);
168 |         void Set_SpeedMode(float speed);
169 |         void Set_PosMode(float position, float max_speed);
170 |         void Set_CurMode(float current);
171 |         void Change_Mode(uint8_t mode);
172 |         void Motor_Con_Init(uint8_t motor_id);
173 |         uint8_t Motor_Data_Updata(uint32_t timeout);
174 | };
175 | 
176 | extern void Motor_CAN_Init(void);
177 | 
178 | #endif


--------------------------------------------------------------------------------
/arduino/advanced_motor_control/TWAI_CAN_MI_Motor.h:
--------------------------------------------------------------------------------
  1 | #ifndef _TWAI_CAN_MI_MOTOR_H__
  2 | #define _TWAI_CAN_MI_MOTOR_H__
  3 | 
  4 | /*
  5 |     小米电机驱动器通信协议及使用说明
  6 | 
  7 |     电机通信为 CAN 2.0 通信接口，波特率 1Mbps，采用扩展帧格式，如下所示：
  8 |     数据域          29 位 ID        8Byte 数据区
  9 |     大小 Bit28~bit24 bit23~8 bit7~0 Byte0~Byte7
 10 |     描述 通信类型 数据区 2 目标地址 数据区 1
 11 | 
 12 |     电机支持的控制模式包括：
 13 |     运控模式：给定电机运控 5 个参数；
 14 |     电流模式：给定电机指定的 Iq 电流；
 15 |     速度模式：给定电机指定的运行速度；
 16 |     位置模式：给定电机指定的位置，电机将运行到该指定的位置；
 17 | 
 18 |     本台测试用的小米电机CAN ID=1，主机ID=0
 19 | 
 20 |     注意配置顺序：
 21 |         1. 电机CAN初始化->电机ID初始化->设定电机机械零点->设置电机控制模式->设置对应模式的控制参数->使能(启动)电机
 22 |         2. 必须给电机发数据才会有数据返回
 23 | 
 24 |     测试用例：
 25 |     MI_Motor_ M1_con;
 26 | 
 27 |     void setup() {
 28 |         Motor_CAN_Init();
 29 |         M1_con.Motor_Con_Init(MOTER_1_ID);
 30 |         M1_con.Motor_Set_Zero();
 31 |         M1_con.Change_Mode(SPEED_MODE);
 32 |         M1_con.Motor_Enable();
 33 |     }
 34 | 
 35 |     void loop() {
 36 |         M1_con.Motor_Data_Updata(20);
 37 |         Serial.printf("M1数据: %d,%d,%d,%d,%d,%d,%d,%d,%d,angle:%.2f,speed:%.2f,torque:%.2f,temp:%.2f,\r\n",
 38 |                                       M1_con.motor_rx_data.master_id,M1_con.motor_rx_data.motor_id,
 39 |                                       M1_con.motor_rx_data.err_sta,M1_con.motor_rx_data.HALL_err,
 40 |                                       M1_con.motor_rx_data.magnet_err,M1_con.motor_rx_data.temp_err,
 41 |                                       M1_con.motor_rx_data.current_err,M1_con.motor_rx_data.voltage_err,
 42 |                                       M1_con.motor_rx_data.mode_sta,
 43 |                                       M1_con.motor_rx_data.cur_angle,M1_con.motor_rx_data.cur_speed,
 44 |                                       M1_con.motor_rx_data.cur_torque,M1_con.motor_rx_data.cur_temp);
 45 |         M1_con.Set_SpeedMode(-1);
 46 |         vTaskDelay(20);
 47 |     }
 48 | */
 49 | 
 50 | #include "Arduino.h"
 51 | #include <ESP32-TWAI-CAN.hpp>
 52 | 
 53 | /*CAN设置*/
 54 | #define CAN_TX          5
 55 | #define CAN_RX          4
 56 | 
 57 | #define MASTER_ID       0
 58 | #define MOTER_1_ID      1
 59 | #define MOTER_2_ID      2
 60 | 
 61 | /*基础配置*/
 62 | #define P_MIN           -12.5f
 63 | #define P_MAX           12.5f
 64 | #define V_MIN           -30.0f
 65 | #define V_MAX           30.0f
 66 | #define KP_MIN          0.0f
 67 | #define KP_MAX          500.0f
 68 | #define KD_MIN          0.0f
 69 | #define KD_MAX          5.0f
 70 | #define T_MIN           -12.0f
 71 | #define T_MAX           12.0f
 72 | 
 73 | /*数据配置*/
 74 | #define RUN_MODE        0x7005    // 运行模式, uint8, 1字节, 0运控模式,1位置模式，2速度模式，3电流模式
 75 | #define CTRL_MODE       0
 76 | #define POS_MODE        1
 77 | #define SPEED_MODE      2
 78 | #define CUR_MODE        3
 79 | #define IQ_REF          0x7006    // 电流模式 Iq 指令, float, 4字节, -23~23A
 80 | #define SPD_REF         0x700A    // 转速模式转速指令, float, 4字节, -30~30rad/s
 81 | #define LIMIT_TORQUE    0x700B    // 转矩限制, float, 4字节, 0~12Nm
 82 | #define CUR_KP          0x7010    // 电流的 Kp, float, 4字节, 默认值 0.125
 83 | #define CUR_KI          0x7011    // 电流的 Ki, float, 4字节, 默认值 0.0158
 84 | #define CUR_FILT_GAIN   0x7014    // 电流滤波系数 filt_gain, float, 4字节, 0~1.0，默认值 0.1
 85 | #define LOC_REF         0x7016    // 位置模式角度指令, float, 4字节, rad
 86 | #define LIMIT_SPD       0x7017    // 位置模式速度限制, float, 4字节, 0~30rad/s
 87 | #define LIMIT_CUR       0x7018    // 速度位置模式电流限制, float, 4字节, 0~23A
 88 | #define MECH_POS        0x7019    // 负载端计圈机械角度, float, 4字节, rad, 只读
 89 | #define IQF             0x701A    // iq 滤波值, float, 4字节, -23~23A, 只读
 90 | #define MECH_VEL        0x701B    // 负载端转速, float, 4字节, -30~30rad/s, 只读
 91 | #define VBUS            0x701C    // 母线电压, float, 4字节, V, 只读
 92 | #define ROTATION        0x701D    // 圈数, int16, 2字节, 圈数, 可读写
 93 | #define LOC_KP          0x701E    // 位置的 kp, float, 4字节, 默认值 30, 可读写
 94 | #define SPD_KP          0x701F    // 速度的 kp, float, 4字节, 默认值 1, 可读写
 95 | #define SPD_KI          0x7020    // 速度的 ki, float, 4字节, 默认值 0.002, 可读写
 96 | 
 97 | //拆分29位ID
 98 | #define RX_29ID_DISASSEMBLE_MASTER_ID(id)       (uint8_t)((id)&0xFF)
 99 | #define RX_29ID_DISASSEMBLE_MOTOR_ID(id)        (uint8_t)(((id)>>8)&0xFF)
100 | //右移16位获取所有故障信息(bit21-16，共6位，111111->0x3F)，三元运算符判断是否有故障。0无1有
101 | #define RX_29ID_DISASSEMBLE_ERR_STA(id)         (uint8_t)(((((id)>>16)&0x3F) > 0) ? 1 : 0)
102 | #define RX_29ID_DISASSEMBLE_HALL_ERR(id)        (uint8_t)(((id)>>20)&0X01)
103 | #define RX_29ID_DISASSEMBLE_MAGNET_ERR(id)      (uint8_t)(((id)>>19)&0x01)
104 | #define RX_29ID_DISASSEMBLE_TEMP_ERR(id)        (uint8_t)(((id)>>18)&0x01)
105 | #define RX_29ID_DISASSEMBLE_CURRENT_ERR(id)     (uint8_t)(((id)>>17)&0x01)
106 | #define RX_29ID_DISASSEMBLE_VOLTAGE_ERR(id)     (uint8_t)(((id)>>16)&0x01)
107 | //模式状态: 0:Reset模式[复位]; 1:Cali 模式[标定]; 2:Motor模式[运行]
108 | #define RX_29ID_DISASSEMBLE_MODE_STA(id)        (uint8_t)(((id)>>22)&0x03)
109 | 
110 | //拆分数据
111 | #define RX_DATA_DISASSEMBLE_CUR_ANGLE(data)     (uint16_t)((data[0]<<8)|data[1])
112 | #define RX_DATA_DISASSEMBLE_CUR_SPEED(data)     (uint16_t)((data[2]<<8)|data[3])
113 | #define RX_DATA_DISASSEMBLE_CUR_TORQUE(data)    (uint16_t)((data[4]<<8)|data[5])
114 | #define RX_DATA_DISASSEMBLE_CUR_TEMP(data)      (uint16_t)((data[6]<<8)|data[7])
115 | 
116 | //转换系数
117 | //转换过程：angle=data×8PI/65535-4PI,   范围：+-4PI
118 | #define INT2ANGLE     0.000383501049
119 | //转换过程：speed=data×60/65535-30,     范围：+-30rad/s
120 | #define INT2SPEED     0.000915541314
121 | //转换过程：torque=data×24/65535-12,    范围：+-12N·m
122 | #define INT2TORQUE    0.000366216526
123 | 
124 | //发送数据包
125 | typedef struct {
126 |   uint32_t id:8;      //8位CAN ID
127 |   uint32_t data:16;   //16位数据
128 |   uint32_t mode:5;    //5位模式
129 |   uint32_t res:3;     //3位保留
130 |   uint8_t tx_data[8];
131 | } can_frame_t;
132 | 
133 | //解析返回数据包
134 | typedef struct {
135 |   //29位ID解码状态
136 |   uint8_t master_id;
137 |   uint8_t motor_id;
138 |   uint8_t err_sta;
139 |   uint8_t HALL_err;
140 |   uint8_t magnet_err;
141 |   uint8_t temp_err;
142 |   uint8_t current_err;
143 |   uint8_t voltage_err;
144 |   uint8_t mode_sta;
145 | 
146 |   //具体数据
147 |   float cur_angle;      //(-4π~4π)
148 |   float cur_speed;      //(-30rad/s~30rad/s)
149 |   float cur_torque;     //（-12Nm~12Nm）
150 |   float cur_temp;       //Temp(摄氏度）*10
151 | } can_rx_frame_t;
152 | 
153 | class MI_Motor_
154 | {
155 |     private:
156 |         uint8_t id;
157 |         CanFrame rxFrame;               //接收原始数据
158 | 
159 |     public:
160 |         can_rx_frame_t motor_rx_data;   //返回解析数据
161 | 
162 |         void Motor_Enable(void);
163 |         void Motor_Reset(void);
164 |         void Motor_Set_Zero(void);
165 |         void Motor_ControlMode(float torque, float MechPosition, float speed, float kp, float kd);
166 |         void Set_SingleParameter(uint16_t parameter, float index);
167 |         void Decoding_Motor_data(CanFrame* can_Frame_point, can_rx_frame_t* motor_frame_point);
168 |         void Set_SpeedMode(float speed);
169 |         void Set_PosMode(float position, float max_speed);
170 |         void Set_CurMode(float current);
171 |         void Change_Mode(uint8_t mode);
172 |         void Motor_Con_Init(uint8_t motor_id);
173 |         uint8_t Motor_Data_Updata(uint32_t timeout);
174 | };
175 | 
176 | extern void Motor_CAN_Init(void);
177 | 
178 | #endif


--------------------------------------------------------------------------------
/arduino/joint_position_control/TWAI_CAN_MI_Motor.h:
--------------------------------------------------------------------------------
  1 | #ifndef _TWAI_CAN_MI_MOTOR_H__
  2 | #define _TWAI_CAN_MI_MOTOR_H__
  3 | 
  4 | /*
  5 |     小米电机驱动器通信协议及使用说明
  6 | 
  7 |     电机通信为 CAN 2.0 通信接口，波特率 1Mbps，采用扩展帧格式，如下所示：
  8 |     数据域          29 位 ID        8Byte 数据区
  9 |     大小 Bit28~bit24 bit23~8 bit7~0 Byte0~Byte7
 10 |     描述 通信类型 数据区 2 目标地址 数据区 1
 11 | 
 12 |     电机支持的控制模式包括：
 13 |     运控模式：给定电机运控 5 个参数；
 14 |     电流模式：给定电机指定的 Iq 电流；
 15 |     速度模式：给定电机指定的运行速度；
 16 |     位置模式：给定电机指定的位置，电机将运行到该指定的位置；
 17 | 
 18 |     本台测试用的小米电机CAN ID=1，主机ID=0
 19 | 
 20 |     注意配置顺序：
 21 |         1. 电机CAN初始化->电机ID初始化->设定电机机械零点->设置电机控制模式->设置对应模式的控制参数->使能(启动)电机
 22 |         2. 必须给电机发数据才会有数据返回
 23 | 
 24 |     测试用例：
 25 |     MI_Motor_ M1_con;
 26 | 
 27 |     void setup() {
 28 |         Motor_CAN_Init();
 29 |         M1_con.Motor_Con_Init(MOTER_1_ID);
 30 |         M1_con.Motor_Set_Zero();
 31 |         M1_con.Change_Mode(SPEED_MODE);
 32 |         M1_con.Motor_Enable();
 33 |     }
 34 | 
 35 |     void loop() {
 36 |         M1_con.Motor_Data_Updata(20);
 37 |         Serial.printf("M1数据: %d,%d,%d,%d,%d,%d,%d,%d,%d,angle:%.2f,speed:%.2f,torque:%.2f,temp:%.2f,\r\n",
 38 |                                       M1_con.motor_rx_data.master_id,M1_con.motor_rx_data.motor_id,
 39 |                                       M1_con.motor_rx_data.err_sta,M1_con.motor_rx_data.HALL_err,
 40 |                                       M1_con.motor_rx_data.magnet_err,M1_con.motor_rx_data.temp_err,
 41 |                                       M1_con.motor_rx_data.current_err,M1_con.motor_rx_data.voltage_err,
 42 |                                       M1_con.motor_rx_data.mode_sta,
 43 |                                       M1_con.motor_rx_data.cur_angle,M1_con.motor_rx_data.cur_speed,
 44 |                                       M1_con.motor_rx_data.cur_torque,M1_con.motor_rx_data.cur_temp);
 45 |         M1_con.Set_SpeedMode(-1);
 46 |         vTaskDelay(20);
 47 |     }
 48 | */
 49 | 
 50 | #include "Arduino.h"
 51 | #include <ESP32-TWAI-CAN.hpp>
 52 | 
 53 | /*CAN设置*/
 54 | #define CAN_TX          5
 55 | #define CAN_RX          4
 56 | 
 57 | #define MASTER_ID       0
 58 | #define MOTER_1_ID      1
 59 | #define MOTER_2_ID      2
 60 | 
 61 | /*基础配置*/
 62 | #define P_MIN           -12.5f
 63 | #define P_MAX           12.5f
 64 | #define V_MIN           -30.0f
 65 | #define V_MAX           30.0f
 66 | #define KP_MIN          0.0f
 67 | #define KP_MAX          500.0f
 68 | #define KD_MIN          0.0f
 69 | #define KD_MAX          5.0f
 70 | #define T_MIN           -12.0f
 71 | #define T_MAX           12.0f
 72 | 
 73 | /*数据配置*/
 74 | #define RUN_MODE        0x7005    // 运行模式, uint8, 1字节, 0运控模式,1位置模式，2速度模式，3电流模式
 75 | #define CTRL_MODE       0
 76 | #define POS_MODE        1
 77 | #define SPEED_MODE      2
 78 | #define CUR_MODE        3
 79 | #define IQ_REF          0x7006    // 电流模式 Iq 指令, float, 4字节, -23~23A
 80 | #define SPD_REF         0x700A    // 转速模式转速指令, float, 4字节, -30~30rad/s
 81 | #define LIMIT_TORQUE    0x700B    // 转矩限制, float, 4字节, 0~12Nm
 82 | #define CUR_KP          0x7010    // 电流的 Kp, float, 4字节, 默认值 0.125
 83 | #define CUR_KI          0x7011    // 电流的 Ki, float, 4字节, 默认值 0.0158
 84 | #define CUR_FILT_GAIN   0x7014    // 电流滤波系数 filt_gain, float, 4字节, 0~1.0，默认值 0.1
 85 | #define LOC_REF         0x7016    // 位置模式角度指令, float, 4字节, rad
 86 | #define LIMIT_SPD       0x7017    // 位置模式速度限制, float, 4字节, 0~30rad/s
 87 | #define LIMIT_CUR       0x7018    // 速度位置模式电流限制, float, 4字节, 0~23A
 88 | #define MECH_POS        0x7019    // 负载端计圈机械角度, float, 4字节, rad, 只读
 89 | #define IQF             0x701A    // iq 滤波值, float, 4字节, -23~23A, 只读
 90 | #define MECH_VEL        0x701B    // 负载端转速, float, 4字节, -30~30rad/s, 只读
 91 | #define VBUS            0x701C    // 母线电压, float, 4字节, V, 只读
 92 | #define ROTATION        0x701D    // 圈数, int16, 2字节, 圈数, 可读写
 93 | #define LOC_KP          0x701E    // 位置的 kp, float, 4字节, 默认值 30, 可读写
 94 | #define SPD_KP          0x701F    // 速度的 kp, float, 4字节, 默认值 1, 可读写
 95 | #define SPD_KI          0x7020    // 速度的 ki, float, 4字节, 默认值 0.002, 可读写
 96 | 
 97 | //拆分29位ID
 98 | #define RX_29ID_DISASSEMBLE_MASTER_ID(id)       (uint8_t)((id)&0xFF)
 99 | #define RX_29ID_DISASSEMBLE_MOTOR_ID(id)        (uint8_t)(((id)>>8)&0xFF)
100 | //右移16位获取所有故障信息(bit21-16，共6位，111111->0x3F)，三元运算符判断是否有故障。0无1有
101 | #define RX_29ID_DISASSEMBLE_ERR_STA(id)         (uint8_t)(((((id)>>16)&0x3F) > 0) ? 1 : 0)
102 | #define RX_29ID_DISASSEMBLE_HALL_ERR(id)        (uint8_t)(((id)>>20)&0X01)
103 | #define RX_29ID_DISASSEMBLE_MAGNET_ERR(id)      (uint8_t)(((id)>>19)&0x01)
104 | #define RX_29ID_DISASSEMBLE_TEMP_ERR(id)        (uint8_t)(((id)>>18)&0x01)
105 | #define RX_29ID_DISASSEMBLE_CURRENT_ERR(id)     (uint8_t)(((id)>>17)&0x01)
106 | #define RX_29ID_DISASSEMBLE_VOLTAGE_ERR(id)     (uint8_t)(((id)>>16)&0x01)
107 | //模式状态: 0:Reset模式[复位]; 1:Cali 模式[标定]; 2:Motor模式[运行]
108 | #define RX_29ID_DISASSEMBLE_MODE_STA(id)        (uint8_t)(((id)>>22)&0x03)
109 | 
110 | //拆分数据
111 | #define RX_DATA_DISASSEMBLE_CUR_ANGLE(data)     (uint16_t)((data[0]<<8)|data[1])
112 | #define RX_DATA_DISASSEMBLE_CUR_SPEED(data)     (uint16_t)((data[2]<<8)|data[3])
113 | #define RX_DATA_DISASSEMBLE_CUR_TORQUE(data)    (uint16_t)((data[4]<<8)|data[5])
114 | #define RX_DATA_DISASSEMBLE_CUR_TEMP(data)      (uint16_t)((data[6]<<8)|data[7])
115 | 
116 | //转换系数
117 | //转换过程：angle=data×8PI/65535-4PI,   范围：+-4PI
118 | #define INT2ANGLE     0.000383501049
119 | //转换过程：speed=data×60/65535-30,     范围：+-30rad/s
120 | #define INT2SPEED     0.000915541314
121 | //转换过程：torque=data×24/65535-12,    范围：+-12N·m
122 | #define INT2TORQUE    0.000366216526
123 | 
124 | //发送数据包
125 | typedef struct {
126 |   uint32_t id:8;      //8位CAN ID
127 |   uint32_t data:16;   //16位数据
128 |   uint32_t mode:5;    //5位模式
129 |   uint32_t res:3;     //3位保留
130 |   uint8_t tx_data[8];
131 | } can_frame_t;
132 | 
133 | //解析返回数据包
134 | typedef struct {
135 |   //29位ID解码状态
136 |   uint8_t master_id;
137 |   uint8_t motor_id;
138 |   uint8_t err_sta;
139 |   uint8_t HALL_err;
140 |   uint8_t magnet_err;
141 |   uint8_t temp_err;
142 |   uint8_t current_err;
143 |   uint8_t voltage_err;
144 |   uint8_t mode_sta;
145 | 
146 |   //具体数据
147 |   float cur_angle;      //(-4π~4π)
148 |   float cur_speed;      //(-30rad/s~30rad/s)
149 |   float cur_torque;     //（-12Nm~12Nm）
150 |   float cur_temp;       //Temp(摄氏度）*10
151 | } can_rx_frame_t;
152 | 
153 | class MI_Motor_
154 | {
155 |     private:
156 |         uint8_t id;
157 |         CanFrame rxFrame;               //接收原始数据
158 | 
159 |     public:
160 |         can_rx_frame_t motor_rx_data;   //返回解析数据
161 | 
162 |         void Motor_Enable(void);
163 |         void Motor_Reset(void);
164 |         void Motor_Set_Zero(void);
165 |         void Motor_ControlMode(float torque, float MechPosition, float speed, float kp, float kd);
166 |         void Set_SingleParameter(uint16_t parameter, float index);
167 |         void Decoding_Motor_data(CanFrame* can_Frame_point, can_rx_frame_t* motor_frame_point);
168 |         void Set_SpeedMode(float speed);
169 |         void Set_PosMode(float position, float max_speed);
170 |         void Set_CurMode(float current);
171 |         void Change_Mode(uint8_t mode);
172 |         void Motor_Con_Init(uint8_t motor_id);
173 |         uint8_t Motor_Data_Updata(uint32_t timeout);
174 | };
175 | 
176 | extern void Motor_CAN_Init(void);
177 | 
178 | #endif


--------------------------------------------------------------------------------
/arduino/simple_joint_control/TWAI_CAN_MI_Motor.h:
--------------------------------------------------------------------------------
  1 | #ifndef _TWAI_CAN_MI_MOTOR_H__
  2 | #define _TWAI_CAN_MI_MOTOR_H__
  3 | 
  4 | /*
  5 |     小米电机驱动器通信协议及使用说明
  6 | 
  7 |     电机通信为 CAN 2.0 通信接口，波特率 1Mbps，采用扩展帧格式，如下所示：
  8 |     数据域          29 位 ID        8Byte 数据区
  9 |     大小 Bit28~bit24 bit23~8 bit7~0 Byte0~Byte7
 10 |     描述 通信类型 数据区 2 目标地址 数据区 1
 11 | 
 12 |     电机支持的控制模式包括：
 13 |     运控模式：给定电机运控 5 个参数；
 14 |     电流模式：给定电机指定的 Iq 电流；
 15 |     速度模式：给定电机指定的运行速度；
 16 |     位置模式：给定电机指定的位置，电机将运行到该指定的位置；
 17 | 
 18 |     本台测试用的小米电机CAN ID=1，主机ID=0
 19 | 
 20 |     注意配置顺序：
 21 |         1. 电机CAN初始化->电机ID初始化->设定电机机械零点->设置电机控制模式->设置对应模式的控制参数->使能(启动)电机
 22 |         2. 必须给电机发数据才会有数据返回
 23 | 
 24 |     测试用例：
 25 |     MI_Motor_ M1_con;
 26 | 
 27 |     void setup() {
 28 |         Motor_CAN_Init();
 29 |         M1_con.Motor_Con_Init(MOTER_1_ID);
 30 |         M1_con.Motor_Set_Zero();
 31 |         M1_con.Change_Mode(SPEED_MODE);
 32 |         M1_con.Motor_Enable();
 33 |     }
 34 | 
 35 |     void loop() {
 36 |         M1_con.Motor_Data_Updata(20);
 37 |         Serial.printf("M1数据: %d,%d,%d,%d,%d,%d,%d,%d,%d,angle:%.2f,speed:%.2f,torque:%.2f,temp:%.2f,\r\n",
 38 |                                       M1_con.motor_rx_data.master_id,M1_con.motor_rx_data.motor_id,
 39 |                                       M1_con.motor_rx_data.err_sta,M1_con.motor_rx_data.HALL_err,
 40 |                                       M1_con.motor_rx_data.magnet_err,M1_con.motor_rx_data.temp_err,
 41 |                                       M1_con.motor_rx_data.current_err,M1_con.motor_rx_data.voltage_err,
 42 |                                       M1_con.motor_rx_data.mode_sta,
 43 |                                       M1_con.motor_rx_data.cur_angle,M1_con.motor_rx_data.cur_speed,
 44 |                                       M1_con.motor_rx_data.cur_torque,M1_con.motor_rx_data.cur_temp);
 45 |         M1_con.Set_SpeedMode(-1);
 46 |         vTaskDelay(20);
 47 |     }
 48 | */
 49 | 
 50 | #include "Arduino.h"
 51 | #include <ESP32-TWAI-CAN.hpp>
 52 | 
 53 | /*CAN设置*/
 54 | #define CAN_TX          5
 55 | #define CAN_RX          4
 56 | 
 57 | #define MASTER_ID       0
 58 | #define MOTER_1_ID      11
 59 | #define MOTER_2_ID      12
 60 | 
 61 | /*基础配置*/
 62 | #define P_MIN           -12.5f
 63 | #define P_MAX           12.5f
 64 | #define V_MIN           -30.0f
 65 | #define V_MAX           30.0f
 66 | #define KP_MIN          0.0f
 67 | #define KP_MAX          500.0f
 68 | #define KD_MIN          0.0f
 69 | #define KD_MAX          5.0f
 70 | #define T_MIN           -12.0f
 71 | #define T_MAX           12.0f
 72 | 
 73 | /*数据配置*/
 74 | #define RUN_MODE        0x7005    // 运行模式, uint8, 1字节, 0运控模式,1位置模式，2速度模式，3电流模式
 75 | #define CTRL_MODE       0
 76 | #define POS_MODE        1
 77 | #define SPEED_MODE      2
 78 | #define CUR_MODE        3
 79 | #define IQ_REF          0x7006    // 电流模式 Iq 指令, float, 4字节, -23~23A
 80 | #define SPD_REF         0x700A    // 转速模式转速指令, float, 4字节, -30~30rad/s
 81 | #define LIMIT_TORQUE    0x700B    // 转矩限制, float, 4字节, 0~12Nm
 82 | #define CUR_KP          0x7010    // 电流的 Kp, float, 4字节, 默认值 0.125
 83 | #define CUR_KI          0x7011    // 电流的 Ki, float, 4字节, 默认值 0.0158
 84 | #define CUR_FILT_GAIN   0x7014    // 电流滤波系数 filt_gain, float, 4字节, 0~1.0，默认值 0.1
 85 | #define LOC_REF         0x7016    // 位置模式角度指令, float, 4字节, rad
 86 | #define LIMIT_SPD       0x7017    // 位置模式速度限制, float, 4字节, 0~30rad/s
 87 | #define LIMIT_CUR       0x7018    // 速度位置模式电流限制, float, 4字节, 0~23A
 88 | #define MECH_POS        0x7019    // 负载端计圈机械角度, float, 4字节, rad, 只读
 89 | #define IQF             0x701A    // iq 滤波值, float, 4字节, -23~23A, 只读
 90 | #define MECH_VEL        0x701B    // 负载端转速, float, 4字节, -30~30rad/s, 只读
 91 | #define VBUS            0x701C    // 母线电压, float, 4字节, V, 只读
 92 | #define ROTATION        0x701D    // 圈数, int16, 2字节, 圈数, 可读写
 93 | #define LOC_KP          0x701E    // 位置的 kp, float, 4字节, 默认值 30, 可读写
 94 | #define SPD_KP          0x701F    // 速度的 kp, float, 4字节, 默认值 1, 可读写
 95 | #define SPD_KI          0x7020    // 速度的 ki, float, 4字节, 默认值 0.002, 可读写
 96 | 
 97 | //拆分29位ID
 98 | #define RX_29ID_DISASSEMBLE_MASTER_ID(id)       (uint8_t)((id)&0xFF)
 99 | #define RX_29ID_DISASSEMBLE_MOTOR_ID(id)        (uint8_t)(((id)>>8)&0xFF)
100 | //右移16位获取所有故障信息(bit21-16，共6位，111111->0x3F)，三元运算符判断是否有故障。0无1有
101 | #define RX_29ID_DISASSEMBLE_ERR_STA(id)         (uint8_t)(((((id)>>16)&0x3F) > 0) ? 1 : 0)
102 | #define RX_29ID_DISASSEMBLE_HALL_ERR(id)        (uint8_t)(((id)>>20)&0X01)
103 | #define RX_29ID_DISASSEMBLE_MAGNET_ERR(id)      (uint8_t)(((id)>>19)&0x01)
104 | #define RX_29ID_DISASSEMBLE_TEMP_ERR(id)        (uint8_t)(((id)>>18)&0x01)
105 | #define RX_29ID_DISASSEMBLE_CURRENT_ERR(id)     (uint8_t)(((id)>>17)&0x01)
106 | #define RX_29ID_DISASSEMBLE_VOLTAGE_ERR(id)     (uint8_t)(((id)>>16)&0x01)
107 | //模式状态: 0:Reset模式[复位]; 1:Cali 模式[标定]; 2:Motor模式[运行]
108 | #define RX_29ID_DISASSEMBLE_MODE_STA(id)        (uint8_t)(((id)>>22)&0x03)
109 | 
110 | //拆分数据
111 | #define RX_DATA_DISASSEMBLE_CUR_ANGLE(data)     (uint16_t)((data[0]<<8)|data[1])
112 | #define RX_DATA_DISASSEMBLE_CUR_SPEED(data)     (uint16_t)((data[2]<<8)|data[3])
113 | #define RX_DATA_DISASSEMBLE_CUR_TORQUE(data)    (uint16_t)((data[4]<<8)|data[5])
114 | #define RX_DATA_DISASSEMBLE_CUR_TEMP(data)      (uint16_t)((data[6]<<8)|data[7])
115 | 
116 | //转换系数
117 | //转换过程：angle=data×8PI/65535-4PI,   范围：+-4PI
118 | #define INT2ANGLE     0.000383501049
119 | //转换过程：speed=data×60/65535-30,     范围：+-30rad/s
120 | #define INT2SPEED     0.000915541314
121 | //转换过程：torque=data×24/65535-12,    范围：+-12N·m
122 | #define INT2TORQUE    0.000366216526
123 | 
124 | //发送数据包
125 | typedef struct {
126 |   uint32_t id:8;      //8位CAN ID
127 |   uint32_t data:16;   //16位数据
128 |   uint32_t mode:5;    //5位模式
129 |   uint32_t res:3;     //3位保留
130 |   uint8_t tx_data[8];
131 | } can_frame_t;
132 | 
133 | //解析返回数据包
134 | typedef struct {
135 |   //29位ID解码状态
136 |   uint8_t master_id;
137 |   uint8_t motor_id;
138 |   uint8_t err_sta;
139 |   uint8_t HALL_err;
140 |   uint8_t magnet_err;
141 |   uint8_t temp_err;
142 |   uint8_t current_err;
143 |   uint8_t voltage_err;
144 |   uint8_t mode_sta;
145 | 
146 |   //具体数据
147 |   float cur_angle;      //(-4π~4π)
148 |   float cur_speed;      //(-30rad/s~30rad/s)
149 |   float cur_torque;     //（-12Nm~12Nm）
150 |   float cur_temp;       //Temp(摄氏度）*10
151 | } can_rx_frame_t;
152 | 
153 | class MI_Motor_
154 | {
155 |     private:
156 |         uint8_t id;
157 |         CanFrame rxFrame;               //接收原始数据
158 | 
159 |     public:
160 |         can_rx_frame_t motor_rx_data;   //返回解析数据
161 | 
162 |         void Motor_Enable(void);
163 |         void Motor_Reset(void);
164 |         void Motor_Set_Zero(void);
165 |         void Motor_ControlMode(float torque, float MechPosition, float speed, float kp, float kd);
166 |         void Set_SingleParameter(uint16_t parameter, float index);
167 |         void Decoding_Motor_data(CanFrame* can_Frame_point, can_rx_frame_t* motor_frame_point);
168 |         void Set_SpeedMode(float speed);
169 |         void Set_PosMode(float position, float max_speed);
170 |         void Set_CurMode(float current);
171 |         void Change_Mode(uint8_t mode);
172 |         void Motor_Con_Init(uint8_t motor_id);
173 |         uint8_t Motor_Data_Updata(uint32_t timeout);
174 | };
175 | 
176 | extern void Motor_CAN_Init(void);
177 | 
178 | #endif


--------------------------------------------------------------------------------
/cpp/README.md:
--------------------------------------------------------------------------------
  1 | # RobStride Control C++
  2 | 
  3 | C++ 实现 RobStride 电机控制库，提供高性能的实时控制功能。
  4 | 
  5 | ## 特性
  6 | 
  7 | - ⚡ **高性能**：200Hz 控制频率，1ms 延迟
  8 | - 🔧 **直接控制**：基于 SocketCAN 的底层实现
  9 | - 🛡️ **类型安全**：强类型检查，内存安全
 10 | - 📦 **易于集成**：标准 CMake 构建系统
 11 | - 🎯 **专业级**：适用于工业级应用
 12 | 
 13 | ## 系统要求
 14 | 
 15 | - Linux 系统 (Ubuntu 18.04+, Debian 10+)
 16 | - GCC 7+ 或 Clang 8+
 17 | - CMake 3.12+
 18 | - SocketCAN 支持
 19 | 
 20 | ## 安装依赖
 21 | 
 22 | ```bash
 23 | # Ubuntu/Debian
 24 | sudo apt-get update
 25 | sudo apt-get install build-essential cmake can-utils
 26 | 
 27 | # 使用项目脚本
 28 | make install-deps
 29 | ```
 30 | 
 31 | ## 编译
 32 | 
 33 | ### 使用 Makefile
 34 | 
 35 | ```bash
 36 | # 编译
 37 | make
 38 | 
 39 | # 调试版本
 40 | make debug
 41 | 
 42 | # 发布版本
 43 | make release
 44 | 
 45 | # 安装到系统
 46 | sudo make install
 47 | ```
 48 | 
 49 | ### 使用 CMake
 50 | 
 51 | ```bash
 52 | mkdir build && cd build
 53 | cmake ..
 54 | make -j$(nproc)
 55 | 
 56 | # 可选：安装
 57 | sudo make install
 58 | ```
 59 | 
 60 | ## 使用方法
 61 | 
 62 | ### 基本使用
 63 | 
 64 | ```cpp
 65 | #include "can_interface.h"
 66 | #include "protocol.h"
 67 | 
 68 | int main(int argc, char* argv[]) {
 69 |     int motor_id = 11;
 70 | 
 71 |     // 初始化 CAN 接口
 72 |     CanInterface can;
 73 |     if (!can.init("can0")) {
 74 |         std::cerr << "Failed to initialize CAN" << std::endl;
 75 |         return 1;
 76 |     }
 77 | 
 78 |     // 设置电机参数
 79 |     enable_motor(can.socket(), motor_id);
 80 |     set_mode_raw(can.socket(), motor_id, ControlMode::MIT_MODE);
 81 | 
 82 |     // 设置限制
 83 |     write_limit(can.socket(), motor_id, ParamID::VELOCITY_LIMIT, 20.0);
 84 |     write_limit(can.socket(), motor_id, ParamID::TORQUE_LIMIT, 20.0);
 85 | 
 86 |     // 位置控制
 87 |     double target_pos = M_PI / 2; // 90度
 88 |     write_operation_frame(can.socket(), motor_id, target_pos, 30.0, 0.5);
 89 | 
 90 |     return 0;
 91 | }
 92 | ```
 93 | 
 94 | ### 编译运行
 95 | 
 96 | ```bash
 97 | # 编译
 98 | make
 99 | 
100 | # 运行（需要 sudo 权限）
101 | sudo ./build/robstride-mit-position 11
102 | ```
103 | 
104 | ## API 参考
105 | 
106 | ### CanInterface 类
107 | 
108 | ```cpp
109 | class CanInterface {
110 | public:
111 |     CanInterface();
112 |     ~CanInterface();
113 | 
114 |     bool init(const std::string& interface = "can0");
115 |     void close();
116 |     bool send_frame(uint32_t can_id, const uint8_t* data, uint8_t dlc);
117 |     bool read_frame(can_frame* frame, int timeout_ms = 100);
118 |     bool is_ready() const;
119 | };
120 | ```
121 | 
122 | ### 协议函数
123 | 
124 | ```cpp
125 | // 电机控制
126 | bool enable_motor(int socket, int motor_id);
127 | bool set_mode_raw(int socket, int motor_id, int8_t mode);
128 | bool write_operation_frame(int socket, int motor_id,
129 |                           double pos, double kp, double kd);
130 | 
131 | // 参数设置
132 | bool write_limit(int socket, int motor_id, uint16_t param_id, float limit);
133 | 
134 | // 状态读取
135 | bool read_operation_frame(int socket);
136 | ```
137 | 
138 | ### 协议常量
139 | 
140 | ```cpp
141 | namespace CommType {
142 |     constexpr uint32_t ENABLE = 3;
143 |     constexpr uint32_t OPERATION_CONTROL = 1;
144 |     constexpr uint32_t WRITE_PARAMETER = 18;
145 | }
146 | 
147 | namespace ControlMode {
148 |     constexpr int8_t MIT_MODE = 0;
149 |     constexpr int8_t POSITION_MODE = 1;
150 |     constexpr int8_t SPEED_MODE = 2;
151 | }
152 | ```
153 | 
154 | ## 控制模式
155 | 
156 | ### MIT 模式 (Mode 0)
157 | 
158 | ```cpp
159 | // 切换到 MIT 模式
160 | set_mode_raw(socket, motor_id, ControlMode::MIT_MODE);
161 | 
162 | // 发送位置指令
163 | double position = M_PI / 2;    // 90度
164 | double kp = 30.0;              // 位置增益
165 | double kd = 0.5;               // 阻尼增益
166 | 
167 | while (running) {
168 |     write_operation_frame(socket, motor_id, position, kp, kd);
169 |     read_operation_frame(socket);  // 清空接收缓冲区
170 | 
171 |     std::this_thread::sleep_for(std::chrono::milliseconds(20)); // 50Hz
172 | }
173 | ```
174 | 
175 | ### 参数设置
176 | 
177 | ```cpp
178 | // 设置控制参数
179 | write_limit(socket, motor_id, ParamID::VELOCITY_LIMIT, 20.0);
180 | write_limit(socket, motor_id, ParamID::TORQUE_LIMIT, 20.0);
181 | write_limit(socket, motor_id, ParamID::POSITION_KP, 30.0);
182 | write_limit(socket, motor_id, ParamID::VELOCITY_KP, 0.5);
183 | ```
184 | 
185 | ## 交互式控制
186 | 
187 | 程序启动后提供交互式界面：
188 | 
189 | ```
190 | 🎯 MIT 位置控制台 (ID: 11)
191 | ========================================
192 | 👉 输入数字 (度) 回车即可改变位置
193 | 👉 'kp <值>' (例如: kp 100) 来调节刚度
194 | 👉 'kd <值>' (例如: kd 2.0) 来调节阻尼 (防抖)
195 | 👉 '0' 或 'home' 回到零点
196 | 👉 'q' 退出
197 | ⚠️  当前 Kp=100 | Kd=2.0
198 | ----------------------------------------
199 | [0.0°] >> 90
200 |  -> 目标设定: 90.0°
201 | ```
202 | 
203 | ## 性能优化
204 | 
205 | ### 编译优化
206 | 
207 | ```bash
208 | # 发布版本（优化）
209 | CXXFLAGS="-O3 -DNDEBUG" make
210 | 
211 | # 启用 LTO
212 | cmake -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=TRUE ..
213 | ```
214 | 
215 | ### 运行时优化
216 | 
217 | ```cpp
218 | // 高优先级调度
219 | struct sched_param param;
220 | param.sched_priority = 99;
221 | sched_setscheduler(0, SCHED_FIFO, &param);
222 | 
223 | // CPU 亲和性
224 | cpu_set_t cpuset;
225 | CPU_ZERO(&cpuset);
226 | CPU_SET(2, &cpuset);  // 绑定到 CPU 2
227 | sched_setaffinity(0, sizeof(cpu_set_t), &cpuset);
228 | ```
229 | 
230 | ### 内存优化
231 | 
232 | ```cpp
233 | // 使用对象池
234 | std::vector<can_frame> frame_pool;
235 | frame_pool.reserve(1000);  // 预分配
236 | 
237 | // 避免动态分配
238 | uint8_t data[8];  // 栈分配
239 | ```
240 | 
241 | ## 调试
242 | 
243 | ### CAN 监控
244 | 
245 | ```bash
246 | # 监控 CAN 流量
247 | sudo candump can0
248 | 
249 | # 过滤特定 ID
250 | sudo candump can0,0C0:7FF
251 | ```
252 | 
253 | ### 调试输出
254 | 
255 | ```cpp
256 | // 编译时启用调试
257 | #ifdef DEBUG
258 |     std::cout << "Debug: " << message << std::endl;
259 | #endif
260 | 
261 | // 运行时调试
262 | const bool debug = true;
263 | if (debug) {
264 |     printf("Pos: %.3f, Kp: %.1f, Kd: %.1f\n", pos, kp, kd);
265 | }
266 | ```
267 | 
268 | ## 测试
269 | 
270 | ### 单元测试
271 | 
272 | ```bash
273 | # 安装 Google Test
274 | sudo apt-get install libgtest-dev
275 | 
276 | # 编译测试
277 | cmake -DBUILD_TESTING=ON ..
278 | make
279 | 
280 | # 运行测试
281 | ./tests/robstride_test
282 | ```
283 | 
284 | ### 集成测试
285 | 
286 | ```bash
287 | # 电机连接测试
288 | make test
289 | 
290 | # 手动测试
291 | sudo ./build/robstride-mit-position --test
292 | 
293 | # 运行示例程序
294 | g++ -std=c++17 -I../include examples/basic_control.cpp -o basic_control
295 | sudo ./basic_control 11
296 | ```
297 | 
298 | ## 故障排除
299 | 
300 | ### 编译错误
301 | 
302 | ```bash
303 | # 检查 C++ 标准
304 | g++ --version  # 需要 GCC 7+
305 | 
306 | # 检查 CMake
307 | cmake --version  # 需要 3.12+
308 | 
309 | # 清理重新编译
310 | make clean
311 | make
312 | ```
313 | 
314 | ### 运行时错误
315 | 
316 | ```bash
317 | # 检查 CAN 接口
318 | ip link show can0
319 | 
320 | # 检查权限
321 | groups  # 应该包含 dialout
322 | 
323 | # 检查设备
324 | ls -l /sys/class/net/can0
325 | ```
326 | 
327 | ## 部署
328 | 
329 | ### 系统服务
330 | 
331 | ```bash
332 | # 创建服务文件
333 | sudo cp scripts/robstride.service /etc/systemd/system/
334 | sudo systemctl enable robstride
335 | sudo systemctl start robstride
336 | ```
337 | 
338 | ### Docker
339 | 
340 | ```dockerfile
341 | FROM ubuntu:20.04
342 | RUN apt-get update && apt-get install -y build-essential cmake
343 | COPY . /app
344 | WORKDIR /app
345 | RUN make
346 | CMD ["./build/robstride-mit-position"]
347 | ```
348 | 
349 | ## 许可证
350 | 
351 | MIT License - 详见 [LICENSE](../LICENSE) 文件
352 | 
353 | ## 贡献
354 | 
355 | 欢迎提交 Issue 和 Pull Request！
356 | 
357 | ## 支持
358 | 
359 | - 📖 [完整文档](../docs/)
360 | - 🐛 [问题反馈](https://github.com/tianrking/robstride-control/issues)


--------------------------------------------------------------------------------
/README_zh.md:
--------------------------------------------------------------------------------
  1 | # RobStride Control
  2 | 
  3 | RobStride电机控制库，提供Python、C++、Rust、Arduino四种语言的电机控制实现。
  4 | 
  5 | [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT)
  6 | [![C++](https://img.shields.io/badge/C++-17-blue.svg)](https://en.wikipedia.org/wiki/C%2B%2B17)
  7 | [![Python](https://img.shields.io/badge/Python-3.8%2B-green.svg)](https://www.python.org/downloads/)
  8 | [![Rust](https://img.shields.io/badge/Rust-1.70%2B-orange.svg)](https://www.rust-lang.org/)
  9 | [![Arduino](https://img.shields.io/badge/Arduino-ESP32-00979D.svg)](https://www.arduino.cc/)
 10 | 
 11 | ## 项目特性
 12 | 
 13 | - **多语言支持**：Python（易用性）、C++（高性能）、Rust（安全性）、Arduino（嵌入式）
 14 | - **多种控制模式**：MIT模式、位置控制、速度控制、扭矩控制
 15 | - **实时性能**：高频率控制循环，低延迟响应
 16 | - **工业级可靠性**：完善的错误处理和安全保护机制
 17 | - **跨平台支持**：Linux系统、嵌入式控制器
 18 | 
 19 | ## 支持的电机型号
 20 | 
 21 | | 型号 | 最大扭矩 | 最大速度 | KP范围 | KD范围 |
 22 | |------|----------|----------|--------|--------|
 23 | | RS-00 | 17 Nm | 50 rad/s | 500.0 | 5.0 |
 24 | | RS-01 | 17 Nm | 44 rad/s | 500.0 | 5.0 |
 25 | | RS-02 | 17 Nm | 44 rad/s | 500.0 | 5.0 |
 26 | | RS-03 | 60 Nm | 50 rad/s | 5000.0| 100.0|
 27 | | RS-04 | 120 Nm| 15 rad/s | 5000.0| 100.0|
 28 | | RS-05 | 17 Nm | 33 rad/s | 500.0 | 5.0 |
 29 | | RS-06 | 60 Nm | 20 rad/s | 5000.0| 100.0|
 30 | 
 31 | ## 项目结构
 32 | 
 33 | ```
 34 | RobStride-Control/
 35 | ├── python/              # Python实现
 36 | │   ├── src/            # 源代码
 37 | │   ├── examples/       # 示例程序
 38 | │   └── robstride_dynamics/  # Python SDK
 39 | ├── cpp/                 # C++实现
 40 | │   ├── src/            # 源代码
 41 | │   ├── include/        # 头文件
 42 | │   └── examples/       # 示例程序
 43 | ├── rust/                # Rust实现
 44 | │   ├── src/            # 源代码
 45 | │   └── examples/       # 示例程序
 46 | ├── arduino/             # Arduino实现
 47 | │   ├── simple_joint_control/    # 简单关节控制
 48 | │   ├── joint_position_control/   # 位置控制
 49 | │   ├── dual_motor_control/       # 双电机控制
 50 | │   └── advanced_motor_control/   # 高级控制
 51 | └── scripts/             # 构建和安装脚本
 52 | ```
 53 | 
 54 | ## 快速开始
 55 | 
 56 | ### 系统要求
 57 | 
 58 | - **操作系统**：Linux (Ubuntu 18.04+ / Debian 10+)
 59 | - **硬件**：支持SocketCAN的CAN接口
 60 | - **权限**：root权限或sudo访问CAN设备
 61 | 
 62 | ### 环境配置
 63 | 
 64 | ```bash
 65 | # 安装CAN工具
 66 | sudo apt-get install can-utils
 67 | 
 68 | # 设置CAN接口
 69 | sudo ip link set can0 type can bitrate 1000000
 70 | sudo ip link set up can0
 71 | 
 72 | # 运行环境配置脚本
 73 | ./scripts/setup.sh
 74 | ```
 75 | 
 76 | ### Python版本
 77 | 
 78 | ```bash
 79 | cd python
 80 | pip install -r requirements.txt
 81 | 
 82 | # 位置控制
 83 | python3 src/position_control.py 11
 84 | 
 85 | # 速度控制
 86 | python3 src/speed_control.py 11
 87 | 
 88 | # 示例程序
 89 | python3 examples/basic_usage.py 11
 90 | ```
 91 | 
 92 | ### C++版本
 93 | 
 94 | ```bash
 95 | cd cpp
 96 | make install-deps
 97 | make
 98 | 
 99 | # 运行主程序
100 | sudo ./build/robstride-mit-position 11
101 | 
102 | # 运行示例
103 | g++ -std=c++17 -I../include examples/basic_control.cpp -o basic_control
104 | sudo ./basic_control 11
105 | ```
106 | 
107 | ### Rust版本
108 | 
109 | ```bash
110 | cd rust
111 | cargo build --release
112 | 
113 | # 运行主程序
114 | cargo run --release --bin robstride-mit-position -- 11
115 | 
116 | # 运行示例
117 | cargo run --release --bin basic_control -- 11
118 | ```
119 | 
120 | ### Arduino版本
121 | 
122 | ```bash
123 | cd arduino
124 | 
125 | # 使用Arduino IDE
126 | 1. 打开 simple_joint_control/simple_joint_control.ino
127 | 2. 选择 ESP32 Dev Module
128 | 3. 上传程序
129 | 
130 | # 使用PlatformIO
131 | pio run --target upload
132 | 
133 | # 不同控制示例
134 | - simple_joint_control     # 基础关节控制
135 | - joint_position_control    # 位置控制
136 | - dual_motor_control        # 双电机控制
137 | - advanced_motor_control    # 高级控制
138 | ```
139 | 
140 | ## 性能指标
141 | 
142 | | 实现语言 | 控制频率 | 延迟 | CPU使用率 | 内存占用 | 平台 |
143 | |----------|----------|------|-----------|----------|------|
144 | | Python   | 100 Hz   | 5ms  | 15%       | 50MB     | Linux |
145 | | C++      | 200 Hz   | 1ms  | 5%        | 10MB     | Linux |
146 | | Rust     | 150 Hz   | 2ms  | 8%        | 15MB     | Linux |
147 | | Arduino  | 50-200Hz | 2-20ms| 5-15%     | 10-50KB  | ESP32/MCU |
148 | 
149 | ## 控制模式
150 | 
151 | ### MIT模式 (Mode 0)
152 | 直接发送位置、速度、扭矩目标，适用于高性能控制应用。
153 | - **控制频率**：50-100Hz
154 | - **应用场景**：机器人关节控制、精密定位
155 | 
156 | ### 位置模式 (Mode 1)
157 | 基于内部位置环的位置控制。
158 | - **控制频率**：20-50Hz
159 | - **应用场景**：点到点运动，轨迹跟踪
160 | 
161 | ### 速度模式 (Mode 2)
162 | 速度闭环控制。
163 | - **控制频率**：20-50Hz
164 | - **应用场景**：传送带控制，轮式机器人
165 | 
166 | ## 使用示例
167 | 
168 | ### Python MIT位置控制
169 | 
170 | ```python
171 | from src.position_control import PositionControllerMIT
172 | 
173 | # 初始化控制器
174 | controller = PositionControllerMIT(motor_id=11)
175 | controller.connect()
176 | 
177 | # 设置位置到90度
178 | controller.set_angle(90.0)
179 | 
180 | # 调整控制参数
181 | controller.set_kp(30.0)  # 位置增益
182 | controller.set_kd(0.5)   # 阻尼增益
183 | ```
184 | 
185 | ### C++ MIT位置控制
186 | 
187 | ```cpp
188 | #include "can_interface.h"
189 | #include "protocol.h"
190 | 
191 | CanInterface can;
192 | can.init("can0");
193 | 
194 | // 设置电机参数
195 | enable_motor(can.socket(), 11);
196 | set_mode_raw(can.socket(), 11, ControlMode::MIT_MODE);
197 | 
198 | // 发送位置指令
199 | write_operation_frame(can.socket(), 11, M_PI/2, 30.0, 0.5);
200 | ```
201 | 
202 | ### Rust MIT位置控制
203 | 
204 | ```rust
205 | let socket = Arc::new(Mutex::new(CanSocket::open("can0")?));
206 | 
207 | // 启用电机并设置模式
208 | enable_motor(&socket, 11)?;
209 | set_mode_raw(&socket, 11, 0)?;
210 | 
211 | // 发送位置指令
212 | write_operation_frame(&socket.lock()?, 11, std::f64::consts::PI/2.0, 30.0, 0.5)?;
213 | ```
214 | 
215 | ### Arduino MIT位置控制
216 | 
217 | ```cpp
218 | #include "TWAI_CAN_MI_Motor.h"
219 | 
220 | TWAI_CAN_MI_Motor motor(11);  // 电机ID=11
221 | 
222 | void setup() {
223 |     Serial.begin(115200);
224 |     motor.init(CAN_SPEED_1000KBPS);
225 |     motor.enable_motor();
226 | }
227 | 
228 | void loop() {
229 |     // 设置位置到90度
230 |     motor.send_mit_command(PI/2, 30.0, 0.5);
231 |     delay(2000);
232 | 
233 |     // 回到原点
234 |     motor.send_mit_command(0, 30.0, 0.5);
235 |     delay(2000);
236 | }
237 | ```
238 | 
239 | ## 交互式控制
240 | 
241 | ### 位置控制命令
242 | - `90` - 设置到90度
243 | - `-45` - 设置到-45度
244 | - `kp 30` - 设置位置增益为30
245 | - `kd 0.5` - 设置阻尼增益为0.5
246 | - `home` - 回零位
247 | - `status` - 显示状态
248 | - `quit` - 退出
249 | 
250 | ### 速度控制命令
251 | - `5.0` - 设置正向5 rad/s
252 | - `-3.0` - 设置反向3 rad/s
253 | - `stop` - 停止电机
254 | - `status` - 显示状态
255 | - `quit` - 退出
256 | 
257 | ## 故障排除
258 | 
259 | ### 常见问题
260 | 
261 | 1. **找不到电机**
262 |    ```bash
263 |    # 检查CAN连接
264 |    sudo ip link show can0
265 |    # 扫描电机
266 |    python3 -c "from robstride_dynamics import RobstrideBus; print(RobstrideBus.scan_channel('can0'))"
267 |    ```
268 | 
269 | 2. **权限被拒绝**
270 |    ```bash
271 |    # 添加用户到dialout组
272 |    sudo usermod -a -G dialout $USER
273 |    # 重启系统
274 |    ```
275 | 
276 | 3. **控制不稳定**
277 |    - 调整Kp/Kd参数
278 |    - 检查CAN总线负载
279 |    - 验证电源稳定性
280 | 
281 | ### 调试工具
282 | 
283 | ```bash
284 | # 监控CAN流量
285 | sudo candump can0
286 | 
287 | # 过滤特定ID
288 | sudo candump can0,0C0:7FF
289 | 
290 | # 检查CAN统计
291 | sudo ip -details link show can0
292 | ```
293 | 
294 | ## 开发指南
295 | 
296 | ### 编译要求
297 | 
298 | - **Python**: Python 3.8+, python-can
299 | - **C++**: GCC 7+或Clang 8+, CMake 3.12+
300 | - **Rust**: Rust 1.70+, Cargo
301 | - **Arduino**: Arduino IDE 1.8.19+, ESP32支持
302 | 
303 | ### 测试
304 | 
305 | ```bash
306 | # Python测试
307 | python3 examples/basic_usage.py 11
308 | 
309 | # C++测试
310 | make test
311 | 
312 | # Rust测试
313 | cargo test
314 | 
315 | # Arduino测试
316 | 通过Arduino IDE串口监视器查看调试信息
317 | ```
318 | 
319 | ### 贡献代码
320 | 
321 | 1. Fork项目
322 | 2. 创建功能分支
323 | 3. 提交更改
324 | 4. 创建Pull Request
325 | 
326 | ## 许可证
327 | 
328 | MIT License - 详见[LICENSE](LICENSE)文件
329 | 
330 | ## 技术支持
331 | 
332 | - Issues: https://github.com/tianrking/robstride-control/issues


--------------------------------------------------------------------------------
/arduino/mi_motor_control/mi_motor_control.ino:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * 小米电机控制程序 - ESP32版本
  3 |  *
  4 |  * 功能：
  5 |  * - 控制小米电机ID=1
  6 |  * - 支持速度、位置、电流、运控四种模式
  7 |  * - 实时读取电机状态数据
  8 |  *
  9 |  * 硬件连接：
 10 |  * - ESP32开发板
 11 |  * - CAN模块：TX->GPIO5, RX->GPIO4
 12 |  * - 小米电机：CAN_H, CAN_L, 电源(12-24V)
 13 |  *
 14 |  * 使用说明：
 15 |  * 1. 确保硬件连接正确
 16 |  * 2. 上传程序到ESP32
 17 |  * 3. 打开串口监视器(115200波特率)
 18 |  * 4. 观察电机状态数据
 19 |  *
 20 |  * 作者：基于小米电机通信协议实现
 21 |  * 日期：2024年
 22 |  */
 23 | 
 24 | #include <Arduino.h>
 25 | #include "TWAI_CAN_MI_Motor.h"
 26 | 
 27 | // 创建电机控制对象
 28 | MI_Motor_ M1_con;
 29 | 
 30 | // 控制参数
 31 | float target_speed = 0.0;      // 目标速度 (rad/s)
 32 | float target_position = 0.0;   // 目标位置 (rad)
 33 | float target_current = 0.0;    // 目标电流 (A)
 34 | uint8_t current_mode = SPEED_MODE;  // 当前控制模式
 35 | 
 36 | // 定时器变量
 37 | unsigned long last_update_time = 0;
 38 | unsigned long last_control_time = 0;
 39 | const unsigned long UPDATE_INTERVAL = 50;   // 数据更新间隔(ms)
 40 | const unsigned long CONTROL_INTERVAL = 100; // 控制命令间隔(ms)
 41 | 
 42 | // 控制状态
 43 | bool motor_enabled = false;
 44 | bool system_initialized = false;
 45 | 
 46 | void setup() {
 47 |     // 初始化串口
 48 |     Serial.begin(115200);
 49 |     Serial.println();
 50 |     Serial.println("=== 小米电机控制程序 ESP32 ===");
 51 |     Serial.println("正在初始化系统...");
 52 | 
 53 |     // 初始化CAN总线
 54 |     Serial.println("1. 初始化CAN总线...");
 55 |     Motor_CAN_Init();
 56 |     delay(100);
 57 | 
 58 |     // 初始化电机
 59 |     Serial.println("2. 初始化电机(ID=1)...");
 60 |     M1_con.Motor_Con_Init(MOTER_1_ID);
 61 |     delay(100);
 62 | 
 63 |     // 设置电机零点
 64 |     Serial.println("3. 设置电机机械零点...");
 65 |     M1_con.Motor_Set_Zero();
 66 |     delay(500);
 67 | 
 68 |     // 设置为速度模式
 69 |     Serial.println("4. 设置速度控制模式...");
 70 |     M1_con.Change_Mode(SPEED_MODE);
 71 |     delay(200);
 72 | 
 73 |     // 使能电机
 74 |     Serial.println("5. 使能电机...");
 75 |     M1_con.Motor_Enable();
 76 |     delay(500);
 77 | 
 78 |     // 设置初始速度为0
 79 |     M1_con.Set_SpeedMode(0.0);
 80 |     delay(200);
 81 | 
 82 |     motor_enabled = true;
 83 |     system_initialized = true;
 84 | 
 85 |     Serial.println("=== 系统初始化完成 ===");
 86 |     Serial.println("电机已使能，开始运行...");
 87 |     Serial.println();
 88 |     Serial.println("实时数据：");
 89 |     Serial.println("格式：主站ID,电机ID,错误状态,HALL错误,磁编码错误,温度错误,电流错误,电压错误,模式状态,角度,速度,扭矩,温度");
 90 |     Serial.println("----------------------------------------------------------------------------------");
 91 | 
 92 |     last_update_time = millis();
 93 |     last_control_time = millis();
 94 | }
 95 | 
 96 | void loop() {
 97 |     unsigned long current_time = millis();
 98 | 
 99 |     // 定期更新电机数据
100 |     if (current_time - last_update_time >= UPDATE_INTERVAL) {
101 |         last_update_time = current_time;
102 | 
103 |         // 更新电机数据
104 |         uint8_t result = M1_con.Motor_Data_Updata(20);
105 | 
106 |         if (result == 0) {
107 |             // 成功读取数据
108 |             print_motor_data();
109 |         } else if (result == 1) {
110 |             Serial.println("警告：接收到其他电机的数据");
111 |         } else {
112 |             Serial.println("警告：未接收到电机数据");
113 |         }
114 |     }
115 | 
116 |     // 定期发送控制命令
117 |     if (current_time - last_control_time >= CONTROL_INTERVAL) {
118 |         last_control_time = current_time;
119 | 
120 |         // 执行控制逻辑
121 |         motor_control_logic();
122 |     }
123 | 
124 |     // 处理串口命令（可选）
125 |     if (Serial.available()) {
126 |         handle_serial_command();
127 |     }
128 | 
129 |     // 小延时，防止CPU占用过高
130 |     delay(10);
131 | }
132 | 
133 | void motor_control_logic() {
134 |     static int step = 0;
135 |     static unsigned long step_start_time = 0;
136 |     static float speed_sequence[] = {0.0, 2.0, -2.0, 1.0, -1.0, 0.0};  // 速度序列
137 |     static int speed_count = sizeof(speed_sequence) / sizeof(speed_sequence[0]);
138 | 
139 |     // 每5秒改变一次速度
140 |     if (millis() - step_start_time > 5000) {
141 |         step_start_time = millis();
142 | 
143 |         target_speed = speed_sequence[step % speed_count];
144 |         step++;
145 | 
146 |         Serial.print("设置目标速度：");
147 |         Serial.print(target_speed);
148 |         Serial.println(" rad/s");
149 | 
150 |         // 发送速度命令
151 |         M1_con.Set_SpeedMode(target_speed);
152 |     }
153 | }
154 | 
155 | void print_motor_data() {
156 |     can_rx_frame_t* data = &M1_con.motor_rx_data;
157 | 
158 |     Serial.printf("M1: %d,%d,%d,%d,%d,%d,%d,%d,%d,",
159 |                   data->master_id, data->motor_id,
160 |                   data->err_sta, data->HALL_err,
161 |                   data->magnet_err, data->temp_err,
162 |                   data->current_err, data->voltage_err,
163 |                   data->mode_sta);
164 | 
165 |     Serial.printf("angle:%.2f,speed:%.2f,torque:%.2f,temp:%.1f\r\n",
166 |                   data->cur_angle, data->cur_speed,
167 |                   data->cur_torque, data->cur_temp);
168 | 
169 |     // 检查错误状态
170 |     if (data->err_sta) {
171 |         Serial.print("⚠️  电机错误：");
172 |         if (data->HALL_err) Serial.print("HALL ");
173 |         if (data->magnet_err) Serial.print("磁编码器 ");
174 |         if (data->temp_err) Serial.print("温度 ");
175 |         if (data->current_err) Serial.print("电流 ");
176 |         if (data->voltage_err) Serial.print("电压 ");
177 |         Serial.println();
178 |     }
179 | 
180 |     // 检查温度
181 |     if (data->cur_temp > 60.0) {
182 |         Serial.printf("🌡️  温度警告：%.1f°C\r\n", data->cur_temp);
183 |     }
184 | }
185 | 
186 | void handle_serial_command() {
187 |     String command = Serial.readStringUntil('\n');
188 |     command.trim();
189 | 
190 |     if (command == "stop") {
191 |         Serial.println("停止电机");
192 |         M1_con.Set_SpeedMode(0.0);
193 |         target_speed = 0.0;
194 | 
195 |     } else if (command == "enable") {
196 |         Serial.println("使能电机");
197 |         M1_con.Motor_Enable();
198 |         motor_enabled = true;
199 | 
200 |     } else if (command == "disable") {
201 |         Serial.println("禁用电机");
202 |         M1_con.Motor_Reset();
203 |         motor_enabled = false;
204 | 
205 |     } else if (command.startsWith("speed")) {
206 |         float speed = command.substring(6).toFloat();
207 |         if (abs(speed) <= 30.0) {
208 |             Serial.printf("设置速度：%.2f rad/s\r\n", speed);
209 |             M1_con.Set_SpeedMode(speed);
210 |             target_speed = speed;
211 |         } else {
212 |             Serial.println("速度超出范围(-30到30 rad/s)");
213 |         }
214 | 
215 |     } else if (command.startsWith("pos")) {
216 |         float pos = command.substring(4).toFloat();
217 |         if (abs(pos) <= 12.5) {
218 |             Serial.printf("切换到位置模式，设置位置：%.2f rad\r\n", pos);
219 |             M1_con.Change_Mode(POS_MODE);
220 |             delay(100);
221 |             M1_con.Set_PosMode(pos, 5.0);  // 限制最大速度5 rad/s
222 |             current_mode = POS_MODE;
223 |             target_position = pos;
224 |         } else {
225 |             Serial.println("位置超出范围(-12.5到12.5 rad)");
226 |         }
227 | 
228 |     } else if (command == "speed_mode") {
229 |         Serial.println("切换到速度模式");
230 |         M1_con.Change_Mode(SPEED_MODE);
231 |         delay(100);
232 |         current_mode = SPEED_MODE;
233 | 
234 |     } else if (command == "help") {
235 |         print_help();
236 | 
237 |     } else if (command == "zero") {
238 |         Serial.println("设置零点");
239 |         M1_con.Motor_Set_Zero();
240 |         delay(500);
241 | 
242 |     } else {
243 |         Serial.println("未知命令，输入'help'查看帮助");
244 |     }
245 | }
246 | 
247 | void print_help() {
248 |     Serial.println("=== 命令帮助 ===");
249 |     Serial.println("stop          - 停止电机");
250 |     Serial.println("enable        - 使能电机");
251 |     Serial.println("disable       - 禁用电机");
252 |     Serial.println("speed X       - 设置速度X rad/s (范围: -30到30)");
253 |     Serial.println("pos X         - 位置模式，设置位置X rad (范围: -12.5到12.5)");
254 |     Serial.println("speed_mode    - 切换到速度模式");
255 |     Serial.println("zero          - 设置当前位置为零点");
256 |     Serial.println("help          - 显示帮助");
257 |     Serial.println();
258 | }
259 | 
260 | // 程序结束时的清理工作
261 | void cleanup() {
262 |     if (motor_enabled) {
263 |         Serial.println("正在停止电机...");
264 |         M1_con.Motor_Reset();
265 |         delay(100);
266 |     }
267 | }


--------------------------------------------------------------------------------
/python/examples/advanced_control.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python3
  2 | # -*- coding: utf-8 -*-
  3 | """
  4 | RobStride Advanced Control Example
  5 | Demonstrates advanced motor control techniques including trajectory planning
  6 | """
  7 | 
  8 | import sys
  9 | import os
 10 | import time
 11 | import math
 12 | import numpy as np
 13 | from typing import List, Tuple
 14 | 
 15 | # Add the python directory to path
 16 | sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'python'))
 17 | 
 18 | try:
 19 |     from src.position_control import PositionControllerMIT
 20 | except ImportError:
 21 |     print("❌ Cannot import motor control modules")
 22 |     sys.exit(1)
 23 | 
 24 | class TrajectoryPlanner:
 25 |     """Simple trajectory planner for smooth motion"""
 26 | 
 27 |     def __init__(self, max_velocity: float = 2.0, max_acceleration: float = 5.0):
 28 |         self.max_velocity = max_velocity  # rad/s
 29 |         self.max_acceleration = max_acceleration  # rad/s²
 30 | 
 31 |     def plan_trajectory(self, start_pos: float, end_pos: float, duration: float) -> List[Tuple[float, float]]:
 32 |         """
 33 |         Plan a smooth trajectory from start_pos to end_pos
 34 |         Returns list of (position, time) tuples
 35 |         """
 36 |         # Generate time points
 37 |         dt = 0.02  # 50Hz update rate
 38 |         num_points = int(duration / dt)
 39 |         times = np.linspace(0, duration, num_points)
 40 | 
 41 |         # Cubic polynomial trajectory
 42 |         # s(t) = at³ + bt² + ct + d
 43 |         # Constraints: s(0)=start, s(T)=end, s'(0)=0, s'(T)=0
 44 | 
 45 |         T = duration
 46 |         d = start_pos
 47 |         a = 2 * (end_pos - start_pos) / (T**3)
 48 |         b = -3 * (end_pos - start_pos) / (T**2)
 49 |         c = 0
 50 | 
 51 |         positions = []
 52 |         for t in times:
 53 |             pos = a * t**3 + b * t**2 + c * t + d
 54 |             positions.append((pos, t))
 55 | 
 56 |         return positions
 57 | 
 58 | class AdvancedController:
 59 |     """Advanced motor controller with trajectory planning"""
 60 | 
 61 |     def __init__(self, motor_id: int):
 62 |         self.motor_id = motor_id
 63 |         self.controller = PositionControllerMIT(motor_id)
 64 |         self.planner = TrajectoryPlanner()
 65 | 
 66 |     def connect(self) -> bool:
 67 |         """Connect to motor"""
 68 |         return self.controller.connect()
 69 | 
 70 |     def execute_trajectory(self, target_angle: float, duration: float = 3.0):
 71 |         """Execute smooth trajectory to target position"""
 72 |         print(f"🎯 Planning trajectory to {target_angle}° over {duration}s")
 73 | 
 74 |         # Get current position (assume starting at 0 for simplicity)
 75 |         start_pos = 0.0  # This should be read from motor in real implementation
 76 |         end_pos = math.radians(target_angle)
 77 | 
 78 |         # Plan trajectory
 79 |         trajectory = self.planner.plan_trajectory(start_pos, end_pos, duration)
 80 | 
 81 |         print(f"📈 Executing {len(trajectory)} points...")
 82 | 
 83 |         # Execute trajectory
 84 |         for pos, t in trajectory:
 85 |             angle_deg = math.degrees(pos)
 86 |             self.controller.set_angle(angle_deg)
 87 |             time.sleep(0.02)  # Match trajectory planning rate
 88 | 
 89 |     def execute_sine_wave(self, amplitude: float = 45.0, frequency: float = 0.5, duration: float = 10.0):
 90 |         """Execute sinusoidal motion"""
 91 |         print(f"🌊 Executing sine wave: ±{amplitude}° at {frequency}Hz for {duration}s")
 92 | 
 93 |         start_time = time.time()
 94 |         dt = 0.02  # 50Hz update rate
 95 | 
 96 |         while time.time() - start_time < duration:
 97 |             t = time.time() - start_time
 98 |             angle = amplitude * math.sin(2 * math.pi * frequency * t)
 99 |             self.controller.set_angle(angle)
100 |             time.sleep(dt)
101 | 
102 |     def execute_square_pattern(self, size: float = 90.0, duration: float = 2.0):
103 |         """Execute square movement pattern"""
104 |         print(f"⬜ Executing square pattern: {size}° sides, {duration}s per side")
105 | 
106 |         corners = [size, size, -size, -size, size]  # Return to start
107 | 
108 |         for corner in corners:
109 |             print(f"📍 Moving to {corner}°")
110 |             self.controller.set_angle(corner)
111 |             time.sleep(duration)
112 | 
113 |     def calibrate_motor(self):
114 |         """Motor calibration routine"""
115 |         print("🔧 Starting motor calibration...")
116 | 
117 |         # Test different Kp/Kd values
118 |         test_positions = [30.0, -30.0, 0.0]
119 |         kp_values = [10.0, 30.0, 50.0]
120 |         kd_values = [0.2, 0.5, 1.0]
121 | 
122 |         print("📊 Testing control parameters...")
123 | 
124 |         for kp in kp_values:
125 |             for kd in kd_values:
126 |                 print(f"Testing Kp={kp}, Kd={kd}")
127 |                 self.controller.set_kp(kp)
128 |                 self.controller.set_kd(kd)
129 | 
130 |                 # Test response
131 |                 start_time = time.time()
132 |                 self.controller.set_angle(test_positions[0])
133 |                 time.sleep(2.0)  # Wait for response
134 | 
135 |         print("✅ Calibration completed!")
136 | 
137 |     def stop_and_exit(self):
138 |         """Stop motor and cleanup"""
139 |         self.controller.stop_and_exit()
140 | 
141 | def main():
142 |     """Main function demonstrating advanced control techniques"""
143 |     print("🚀 RobStride Advanced Control Example")
144 |     print("=" * 60)
145 | 
146 |     # Get motor ID from command line
147 |     motor_id = 11
148 |     if len(sys.argv) > 1:
149 |         try:
150 |             motor_id = int(sys.argv[1])
151 |         except ValueError:
152 |             print("❌ Invalid motor ID. Using default (11)")
153 | 
154 |     print(f"Using Motor ID: {motor_id}")
155 | 
156 |     # Initialize controller
157 |     controller = AdvancedController(motor_id)
158 | 
159 |     try:
160 |         # Connect to motor
161 |         if not controller.connect():
162 |             print("❌ Failed to connect to motor")
163 |             return 1
164 | 
165 |         print("✅ Connected successfully!")
166 | 
167 |         # Demonstration menu
168 |         print("\n📋 Select demonstration:")
169 |         print("1. Smooth trajectory planning")
170 |         print("2. Sinusoidal motion")
171 |         print("3. Square pattern movement")
172 |         print("4. Motor calibration")
173 |         print("5. All demonstrations (sequentially)")
174 | 
175 |         choice = input("Enter choice (1-5): ").strip()
176 | 
177 |         if choice == '1':
178 |             # Trajectory planning demo
179 |             controller.execute_trajectory(180.0, 4.0)
180 |             time.sleep(1.0)
181 |             controller.execute_trajectory(-180.0, 4.0)
182 |             controller.execute_trajectory(0.0, 2.0)
183 | 
184 |         elif choice == '2':
185 |             # Sine wave demo
186 |             controller.execute_sine_wave(amplitude=90.0, frequency=0.3, duration=15.0)
187 | 
188 |         elif choice == '3':
189 |             # Square pattern demo
190 |             controller.execute_square_pattern(size=60.0, duration=1.5)
191 | 
192 |         elif choice == '4':
193 |             # Calibration demo
194 |             controller.calibrate_motor()
195 | 
196 |         elif choice == '5':
197 |             # All demos
198 |             print("\n🎭 Running all demonstrations...")
199 | 
200 |             print("\n1️⃣ Trajectory Planning")
201 |             controller.execute_trajectory(120.0, 3.0)
202 |             time.sleep(0.5)
203 |             controller.execute_trajectory(0.0, 2.0)
204 |             time.sleep(1.0)
205 | 
206 |             print("\n2️⃣ Sinusoidal Motion")
207 |             controller.execute_sine_wave(amplitude=45.0, frequency=0.5, duration=8.0)
208 |             time.sleep(1.0)
209 | 
210 |             print("\n3️⃣ Square Pattern")
211 |             controller.execute_square_pattern(size=30.0, duration=1.0)
212 | 
213 |         else:
214 |             print("❌ Invalid choice")
215 |             return 1
216 | 
217 |         print("\n🎉 Advanced control demonstration completed!")
218 |         return 0
219 | 
220 |     except KeyboardInterrupt:
221 |         print("\n👋 Demonstration interrupted by user")
222 |         return 0
223 |     except Exception as e:
224 |         print(f"\n💥 Unexpected error: {e}")
225 |         return 1
226 |     finally:
227 |         controller.stop_and_exit()
228 | 
229 | if __name__ == "__main__":
230 |     sys.exit(main())


--------------------------------------------------------------------------------
/arduino/dual_motor_control/TWAI_CAN_MI_Motor.cpp:
--------------------------------------------------------------------------------
  1 | #include "Arduino.h"
  2 | #include "TWAI_CAN_MI_Motor.h"
  3 | 
  4 | //把浮点数转换成uint_16 用在位置 扭矩 上面
  5 | static int float_to_uint(float x, float x_min, float x_max, int bits)
  6 | {
  7 |   float span = x_max - x_min;
  8 |   float offset = x_min;
  9 |   if (x > x_max) x = x_max;
 10 |   else if (x < x_min) x = x_min;
 11 |   return (int)((x - offset) * ((float)((1 << bits) - 1)) / span);
 12 | }
 13 | 
 14 | //底层的CAN发送指令，小米电机采用扩展帧，数据帧的格式
 15 | static void CAN_Send_Frame(can_frame_t* frame)
 16 | {
 17 |     CanFrame obdFrame = { 0 };
 18 |     uint32_t id_val, data_val, mode_val;
 19 |     uint32_t combined_val;
 20 | 
 21 |     obdFrame.extd = 1;              //0-标准帧; 1-扩展帧
 22 |     obdFrame.rtr = 0;               //0-数据帧; 1-远程帧
 23 |     obdFrame.ss = 0;                //0-错误重发; 1-单次发送(仲裁或丢失时消息不会被重发)，对接收消息无效
 24 |     obdFrame.self = 0;              //0-不接收自己发送的消息，1-接收自己发送的消息，对接收消息无效
 25 |     obdFrame.dlc_non_comp = 0;      //0-数据长度不大于8(ISO 11898-1); 1-数据长度大于8(非标);
 26 | 
 27 |     //拼接ID
 28 |     id_val = frame->id;
 29 |     data_val = frame->data;
 30 |     mode_val = frame->mode;
 31 |     combined_val |= (mode_val << 24);
 32 |     combined_val |= (data_val << 8);
 33 |     combined_val |= id_val;
 34 | 
 35 |     obdFrame.identifier = combined_val; //普通帧直接写id，扩展帧需要计算。11/29位ID
 36 |     obdFrame.data_length_code = 8;      //要发送的字节数
 37 | 
 38 |     for (int i = 0; i < 8; i++)
 39 |     {
 40 |         obdFrame.data[i] = frame->tx_data[i];
 41 |     }
 42 |     ESP32Can.writeFrame(obdFrame);
 43 | }
 44 | 
 45 | /**
 46 |       * @brief          电机使能
 47 |       */
 48 | void MI_Motor_::Motor_Enable(void)
 49 | {
 50 |     can_frame_t motor_con_frame;
 51 |     motor_con_frame.mode = 3;           //模式3，使能
 52 |     motor_con_frame.id = this->id;      //目标电机ID
 53 |     motor_con_frame.data = MASTER_ID;   //本机ID
 54 |     for (int i = 0; i < 8; i++)
 55 |     {
 56 |         motor_con_frame.tx_data[i] = 0;
 57 |     }
 58 |     CAN_Send_Frame(&motor_con_frame);
 59 | }
 60 | 
 61 | /**
 62 |       * @brief          电机停止
 63 |       */
 64 | void MI_Motor_::Motor_Reset(void)
 65 | {
 66 |     can_frame_t motor_con_frame;
 67 |     motor_con_frame.mode = 4;           //模式4，停止
 68 |     motor_con_frame.id = this->id;      //目标电机ID
 69 |     motor_con_frame.data = MASTER_ID;   //本机ID
 70 |     for (int i = 0; i < 8; i++)
 71 |     {
 72 |         motor_con_frame.tx_data[i] = 0;
 73 |     }
 74 |     CAN_Send_Frame(&motor_con_frame);
 75 | }
 76 | 
 77 | /**
 78 |       * @brief          设置电机机械零点
 79 |       */
 80 | void MI_Motor_::Motor_Set_Zero(void)
 81 | {
 82 |     can_frame_t motor_con_frame;
 83 |     motor_con_frame.mode = 6;           //模式6，设置零点
 84 |     motor_con_frame.id = this->id;      //目标电机ID
 85 |     motor_con_frame.data = MASTER_ID;   //本机ID
 86 |     for (int i = 0; i < 8; i++)
 87 |     {
 88 |         motor_con_frame.tx_data[i] = 0;
 89 |     }
 90 |     CAN_Send_Frame(&motor_con_frame);
 91 | }
 92 | 
 93 | /**
 94 |       * @brief          运动控制模式
 95 |       * @param[in]      torque 扭矩，N·m，-12到+12
 96 |       * @param[in]      MechPosition 角度，rad，-12.5到+12.5
 97 |       * @param[in]      speed 角速度，rad/s，-30到+30
 98 |       * @param[in]      kp 比例增益，0到500
 99 |       * @param[in]      kd 微分增益，0到5
100 |       */
101 | void MI_Motor_::Motor_ControlMode(float torque, float MechPosition, float speed, float kp, float kd)
102 | {
103 |     can_frame_t motor_con_frame;
104 |     motor_con_frame.mode = 1;           //模式1,运控模式
105 |     motor_con_frame.id = this->id;      //目标电机ID
106 |     motor_con_frame.data = float_to_uint(torque, T_MIN, T_MAX, 16);
107 | 
108 |     motor_con_frame.tx_data[0] = float_to_uint(MechPosition, P_MIN, P_MAX, 16) >> 8;
109 |     motor_con_frame.tx_data[1] = float_to_uint(MechPosition, P_MIN, P_MAX, 16);
110 |     motor_con_frame.tx_data[2] = float_to_uint(speed, V_MIN, V_MAX, 16) >> 8;
111 |     motor_con_frame.tx_data[3] = float_to_uint(speed, V_MIN, V_MAX, 16);
112 |     motor_con_frame.tx_data[4] = float_to_uint(kp, KP_MIN, KP_MAX, 16) >> 8;
113 |     motor_con_frame.tx_data[5] = float_to_uint(kp, KP_MIN, KP_MAX, 16);
114 |     motor_con_frame.tx_data[6] = float_to_uint(kd, KD_MIN, KD_MAX, 16) >> 8;
115 |     motor_con_frame.tx_data[7] = float_to_uint(kd, KD_MIN, KD_MAX, 16);
116 |     CAN_Send_Frame(&motor_con_frame);
117 | }
118 | 
119 | /**
120 |       * @brief          设置单参数
121 |       * @param[in]      parameter 参数数据
122 |       * @param[in]      index 参数列表，详见电机说明书的4.1.11
123 |       */
124 | void MI_Motor_::Set_SingleParameter(uint16_t parameter, float index)
125 | {
126 |     can_frame_t motor_con_frame;
127 |     motor_con_frame.mode = 18;          //模式18,单参数修改模式
128 |     motor_con_frame.id = this->id;      //目标电机ID
129 |     motor_con_frame.data = MASTER_ID;   //来标识主CAN_ID
130 | 
131 |     motor_con_frame.tx_data[2] = 0;
132 |     motor_con_frame.tx_data[3] = 0;     //Byte2~3: 00
133 | 
134 |     memcpy(&motor_con_frame.tx_data[0], &parameter, 2);   //Byte0~1: index，参数列表详见4.1.11
135 |     memcpy(&motor_con_frame.tx_data[4], &index, 4);       //Byte4~7: 参数数据，1字节数据在Byte4
136 |     CAN_Send_Frame(&motor_con_frame);
137 | }
138 | 
139 | /**
140 |       * @brief          解析电机发送数据包
141 |       * @param[in]      can_Frame_point CAN原始数据包的指针
142 |       * @param[in]      motor_frame_point 电机解析数据包的指针
143 |       */
144 | void MI_Motor_::Decoding_Motor_data(CanFrame* can_Frame_point, can_rx_frame_t* motor_frame_point)
145 | {
146 |     motor_frame_point->master_id = RX_29ID_DISASSEMBLE_MASTER_ID(can_Frame_point->identifier);
147 |     motor_frame_point->motor_id = RX_29ID_DISASSEMBLE_MOTOR_ID(can_Frame_point->identifier);
148 |     motor_frame_point->err_sta = RX_29ID_DISASSEMBLE_ERR_STA(can_Frame_point->identifier);
149 |     motor_frame_point->HALL_err = RX_29ID_DISASSEMBLE_HALL_ERR(can_Frame_point->identifier);
150 |     motor_frame_point->magnet_err = RX_29ID_DISASSEMBLE_MAGNET_ERR(can_Frame_point->identifier);
151 |     motor_frame_point->temp_err = RX_29ID_DISASSEMBLE_TEMP_ERR(can_Frame_point->identifier);
152 |     motor_frame_point->current_err = RX_29ID_DISASSEMBLE_CURRENT_ERR(can_Frame_point->identifier);
153 |     motor_frame_point->voltage_err = RX_29ID_DISASSEMBLE_VOLTAGE_ERR(can_Frame_point->identifier);
154 |     motor_frame_point->mode_sta = RX_29ID_DISASSEMBLE_MODE_STA(can_Frame_point->identifier);
155 | 
156 |     //弧度单位
157 |     motor_frame_point->cur_angle = RX_DATA_DISASSEMBLE_CUR_ANGLE(can_Frame_point->data) * INT2ANGLE - 4 * PI;
158 |     motor_frame_point->cur_speed = RX_DATA_DISASSEMBLE_CUR_SPEED(can_Frame_point->data) * INT2SPEED - 30;
159 |     motor_frame_point->cur_torque = RX_DATA_DISASSEMBLE_CUR_TORQUE(can_Frame_point->data) * INT2TORQUE - 12;
160 |     motor_frame_point->cur_temp = RX_DATA_DISASSEMBLE_CUR_TEMP(can_Frame_point->data) / 10.0f;
161 | }
162 | 
163 | /**
164 |       * @brief          设置速度模式的参数
165 |       * @param[in]      speed 角速度，rad/s，-30到+30
166 |       */
167 | void MI_Motor_::Set_SpeedMode(float speed)
168 | {
169 |     Set_SingleParameter(SPD_REF, speed);
170 | }
171 | 
172 | /**
173 |       * @brief          设置位置模式的参数
174 |       * @param[in]      position 角度，rad，-12.5到+12.5
175 |       * @param[in]      max_speed 角速度，rad/s，0到+30
176 |       */
177 | void MI_Motor_::Set_PosMode(float position, float max_speed)
178 | {
179 |     Set_SingleParameter(LIMIT_SPD, max_speed);
180 |     Set_SingleParameter(LOC_REF, position);
181 | }
182 | 
183 | /**
184 |       * @brief          设置电流模式的参数
185 |       * @param[in]      current 电流，A，-23到+23
186 |       */
187 | void MI_Motor_::Set_CurMode(float current)
188 | {
189 |     Set_SingleParameter(IQ_REF, current);
190 | }
191 | 
192 | /**
193 |       * @brief          设置运动模式
194 |       * @param[in]      mode 运控模式：CTRL_MODE 电流模式：CUR_MODE 速度模式：SPEED_MODE 位置模式：POS_MODE
195 |       */
196 | void MI_Motor_::Change_Mode(uint8_t mode)
197 | {
198 |     uint16_t parameter = RUN_MODE;
199 |     can_frame_t motor_con_frame;
200 |     motor_con_frame.mode = 18;  //模式18，单参数写入
201 |     motor_con_frame.data = MASTER_ID;
202 |     motor_con_frame.id = this->id;    //电机ID
203 |     for (int j = 0; j <= 7; j++) {
204 |         motor_con_frame.tx_data[j] = 0;
205 |     }
206 |     motor_con_frame.tx_data[4] = mode;
207 |     memcpy(&motor_con_frame.tx_data[0], &parameter, 2);
208 |     CAN_Send_Frame(&motor_con_frame);
209 | }
210 | 
211 | /**
212 |       * @brief          电机初始化，在上位机查看电机ID和主机ID，主机ID默认为0
213 |       * @param[in]      motor_id 电机ID
214 |       */
215 | void MI_Motor_::Motor_Con_Init(uint8_t motor_id)
216 | {
217 |     this->id = motor_id;
218 | }
219 | 
220 | /**
221 |       * @brief          CAN初始化
222 |       */
223 | void Motor_CAN_Init(void)
224 | {
225 |     ESP32Can.setPins(CAN_TX, CAN_RX);                //设置pin脚
226 |     ESP32Can.setRxQueueSize(50);                     //设置缓存区大小
227 |     ESP32Can.setTxQueueSize(50);                     //设置缓存区大小
228 |     ESP32Can.setSpeed(ESP32Can.convertSpeed(1000));  //设置速度，一定要1M，其它库都没有1M，就这个库有
229 |     ESP32Can.begin();
230 | }
231 | 
232 | /**
233 |       * @brief          更新电机数据
234 |       * @param[in]      timeout 超时时间
235 |       * @retval         err_sta 0：正常读取；1：读取到数据，但不是自己的；2：没有读取到数据
236 |       */
237 | uint8_t MI_Motor_::Motor_Data_Updata(uint32_t timeout)
238 | {
239 |     uint8_t err_sta = 0;
240 |     if (ESP32Can.readFrame(rxFrame, timeout)) {
241 |         if (RX_29ID_DISASSEMBLE_MOTOR_ID(rxFrame.identifier) == this->id) {
242 |             Decoding_Motor_data(&rxFrame, &motor_rx_data);
243 |         } else {
244 |             err_sta = 1;
245 |         }
246 |     } else {
247 |         err_sta = 2;
248 |     }
249 | 
250 |     return err_sta;
251 | }


--------------------------------------------------------------------------------
/arduino/mi_motor_control/TWAI_CAN_MI_Motor.cpp:
--------------------------------------------------------------------------------
  1 | #include "Arduino.h"
  2 | #include "TWAI_CAN_MI_Motor.h"
  3 | 
  4 | //把浮点数转换成uint_16 用在位置 扭矩 上面
  5 | static int float_to_uint(float x, float x_min, float x_max, int bits)
  6 | {
  7 |   float span = x_max - x_min;
  8 |   float offset = x_min;
  9 |   if (x > x_max) x = x_max;
 10 |   else if (x < x_min) x = x_min;
 11 |   return (int)((x - offset) * ((float)((1 << bits) - 1)) / span);
 12 | }
 13 | 
 14 | //底层的CAN发送指令，小米电机采用扩展帧，数据帧的格式
 15 | static void CAN_Send_Frame(can_frame_t* frame)
 16 | {
 17 |     CanFrame obdFrame = { 0 };
 18 |     uint32_t id_val, data_val, mode_val;
 19 |     uint32_t combined_val;
 20 | 
 21 |     obdFrame.extd = 1;              //0-标准帧; 1-扩展帧
 22 |     obdFrame.rtr = 0;               //0-数据帧; 1-远程帧
 23 |     obdFrame.ss = 0;                //0-错误重发; 1-单次发送(仲裁或丢失时消息不会被重发)，对接收消息无效
 24 |     obdFrame.self = 0;              //0-不接收自己发送的消息，1-接收自己发送的消息，对接收消息无效
 25 |     obdFrame.dlc_non_comp = 0;      //0-数据长度不大于8(ISO 11898-1); 1-数据长度大于8(非标);
 26 | 
 27 |     //拼接ID
 28 |     id_val = frame->id;
 29 |     data_val = frame->data;
 30 |     mode_val = frame->mode;
 31 |     combined_val |= (mode_val << 24);
 32 |     combined_val |= (data_val << 8);
 33 |     combined_val |= id_val;
 34 | 
 35 |     obdFrame.identifier = combined_val; //普通帧直接写id，扩展帧需要计算。11/29位ID
 36 |     obdFrame.data_length_code = 8;      //要发送的字节数
 37 | 
 38 |     for (int i = 0; i < 8; i++)
 39 |     {
 40 |         obdFrame.data[i] = frame->tx_data[i];
 41 |     }
 42 |     ESP32Can.writeFrame(obdFrame);
 43 | }
 44 | 
 45 | /**
 46 |       * @brief          电机使能
 47 |       */
 48 | void MI_Motor_::Motor_Enable(void)
 49 | {
 50 |     can_frame_t motor_con_frame;
 51 |     motor_con_frame.mode = 3;           //模式3，使能
 52 |     motor_con_frame.id = this->id;      //目标电机ID
 53 |     motor_con_frame.data = MASTER_ID;   //本机ID
 54 |     for (int i = 0; i < 8; i++)
 55 |     {
 56 |         motor_con_frame.tx_data[i] = 0;
 57 |     }
 58 |     CAN_Send_Frame(&motor_con_frame);
 59 | }
 60 | 
 61 | /**
 62 |       * @brief          电机停止
 63 |       */
 64 | void MI_Motor_::Motor_Reset(void)
 65 | {
 66 |     can_frame_t motor_con_frame;
 67 |     motor_con_frame.mode = 4;           //模式4，停止
 68 |     motor_con_frame.id = this->id;      //目标电机ID
 69 |     motor_con_frame.data = MASTER_ID;   //本机ID
 70 |     for (int i = 0; i < 8; i++)
 71 |     {
 72 |         motor_con_frame.tx_data[i] = 0;
 73 |     }
 74 |     CAN_Send_Frame(&motor_con_frame);
 75 | }
 76 | 
 77 | /**
 78 |       * @brief          设置电机机械零点
 79 |       */
 80 | void MI_Motor_::Motor_Set_Zero(void)
 81 | {
 82 |     can_frame_t motor_con_frame;
 83 |     motor_con_frame.mode = 6;           //模式6，设置零点
 84 |     motor_con_frame.id = this->id;      //目标电机ID
 85 |     motor_con_frame.data = MASTER_ID;   //本机ID
 86 |     for (int i = 0; i < 8; i++)
 87 |     {
 88 |         motor_con_frame.tx_data[i] = 0;
 89 |     }
 90 |     CAN_Send_Frame(&motor_con_frame);
 91 | }
 92 | 
 93 | /**
 94 |       * @brief          运动控制模式
 95 |       * @param[in]      torque 扭矩，N·m，-12到+12
 96 |       * @param[in]      MechPosition 角度，rad，-12.5到+12.5
 97 |       * @param[in]      speed 角速度，rad/s，-30到+30
 98 |       * @param[in]      kp 比例增益，0到500
 99 |       * @param[in]      kd 微分增益，0到5
100 |       */
101 | void MI_Motor_::Motor_ControlMode(float torque, float MechPosition, float speed, float kp, float kd)
102 | {
103 |     can_frame_t motor_con_frame;
104 |     motor_con_frame.mode = 1;           //模式1,运控模式
105 |     motor_con_frame.id = this->id;      //目标电机ID
106 |     motor_con_frame.data = float_to_uint(torque, T_MIN, T_MAX, 16);
107 | 
108 |     motor_con_frame.tx_data[0] = float_to_uint(MechPosition, P_MIN, P_MAX, 16) >> 8;
109 |     motor_con_frame.tx_data[1] = float_to_uint(MechPosition, P_MIN, P_MAX, 16);
110 |     motor_con_frame.tx_data[2] = float_to_uint(speed, V_MIN, V_MAX, 16) >> 8;
111 |     motor_con_frame.tx_data[3] = float_to_uint(speed, V_MIN, V_MAX, 16);
112 |     motor_con_frame.tx_data[4] = float_to_uint(kp, KP_MIN, KP_MAX, 16) >> 8;
113 |     motor_con_frame.tx_data[5] = float_to_uint(kp, KP_MIN, KP_MAX, 16);
114 |     motor_con_frame.tx_data[6] = float_to_uint(kd, KD_MIN, KD_MAX, 16) >> 8;
115 |     motor_con_frame.tx_data[7] = float_to_uint(kd, KD_MIN, KD_MAX, 16);
116 |     CAN_Send_Frame(&motor_con_frame);
117 | }
118 | 
119 | /**
120 |       * @brief          设置单参数
121 |       * @param[in]      parameter 参数数据
122 |       * @param[in]      index 参数列表，详见电机说明书的4.1.11
123 |       */
124 | void MI_Motor_::Set_SingleParameter(uint16_t parameter, float index)
125 | {
126 |     can_frame_t motor_con_frame;
127 |     motor_con_frame.mode = 18;          //模式18,单参数修改模式
128 |     motor_con_frame.id = this->id;      //目标电机ID
129 |     motor_con_frame.data = MASTER_ID;   //来标识主CAN_ID
130 | 
131 |     motor_con_frame.tx_data[2] = 0;
132 |     motor_con_frame.tx_data[3] = 0;     //Byte2~3: 00
133 | 
134 |     memcpy(&motor_con_frame.tx_data[0], &parameter, 2);   //Byte0~1: index，参数列表详见4.1.11
135 |     memcpy(&motor_con_frame.tx_data[4], &index, 4);       //Byte4~7: 参数数据，1字节数据在Byte4
136 |     CAN_Send_Frame(&motor_con_frame);
137 | }
138 | 
139 | /**
140 |       * @brief          解析电机发送数据包
141 |       * @param[in]      can_Frame_point CAN原始数据包的指针
142 |       * @param[in]      motor_frame_point 电机解析数据包的指针
143 |       */
144 | void MI_Motor_::Decoding_Motor_data(CanFrame* can_Frame_point, can_rx_frame_t* motor_frame_point)
145 | {
146 |     motor_frame_point->master_id = RX_29ID_DISASSEMBLE_MASTER_ID(can_Frame_point->identifier);
147 |     motor_frame_point->motor_id = RX_29ID_DISASSEMBLE_MOTOR_ID(can_Frame_point->identifier);
148 |     motor_frame_point->err_sta = RX_29ID_DISASSEMBLE_ERR_STA(can_Frame_point->identifier);
149 |     motor_frame_point->HALL_err = RX_29ID_DISASSEMBLE_HALL_ERR(can_Frame_point->identifier);
150 |     motor_frame_point->magnet_err = RX_29ID_DISASSEMBLE_MAGNET_ERR(can_Frame_point->identifier);
151 |     motor_frame_point->temp_err = RX_29ID_DISASSEMBLE_TEMP_ERR(can_Frame_point->identifier);
152 |     motor_frame_point->current_err = RX_29ID_DISASSEMBLE_CURRENT_ERR(can_Frame_point->identifier);
153 |     motor_frame_point->voltage_err = RX_29ID_DISASSEMBLE_VOLTAGE_ERR(can_Frame_point->identifier);
154 |     motor_frame_point->mode_sta = RX_29ID_DISASSEMBLE_MODE_STA(can_Frame_point->identifier);
155 | 
156 |     //弧度单位
157 |     motor_frame_point->cur_angle = RX_DATA_DISASSEMBLE_CUR_ANGLE(can_Frame_point->data) * INT2ANGLE - 4 * PI;
158 |     motor_frame_point->cur_speed = RX_DATA_DISASSEMBLE_CUR_SPEED(can_Frame_point->data) * INT2SPEED - 30;
159 |     motor_frame_point->cur_torque = RX_DATA_DISASSEMBLE_CUR_TORQUE(can_Frame_point->data) * INT2TORQUE - 12;
160 |     motor_frame_point->cur_temp = RX_DATA_DISASSEMBLE_CUR_TEMP(can_Frame_point->data) / 10.0f;
161 | }
162 | 
163 | /**
164 |       * @brief          设置速度模式的参数
165 |       * @param[in]      speed 角速度，rad/s，-30到+30
166 |       */
167 | void MI_Motor_::Set_SpeedMode(float speed)
168 | {
169 |     Set_SingleParameter(SPD_REF, speed);
170 | }
171 | 
172 | /**
173 |       * @brief          设置位置模式的参数
174 |       * @param[in]      position 角度，rad，-12.5到+12.5
175 |       * @param[in]      max_speed 角速度，rad/s，0到+30
176 |       */
177 | void MI_Motor_::Set_PosMode(float position, float max_speed)
178 | {
179 |     Set_SingleParameter(LIMIT_SPD, max_speed);
180 |     Set_SingleParameter(LOC_REF, position);
181 | }
182 | 
183 | /**
184 |       * @brief          设置电流模式的参数
185 |       * @param[in]      current 电流，A，-23到+23
186 |       */
187 | void MI_Motor_::Set_CurMode(float current)
188 | {
189 |     Set_SingleParameter(IQ_REF, current);
190 | }
191 | 
192 | /**
193 |       * @brief          设置运动模式
194 |       * @param[in]      mode 运控模式：CTRL_MODE 电流模式：CUR_MODE 速度模式：SPEED_MODE 位置模式：POS_MODE
195 |       */
196 | void MI_Motor_::Change_Mode(uint8_t mode)
197 | {
198 |     uint16_t parameter = RUN_MODE;
199 |     can_frame_t motor_con_frame;
200 |     motor_con_frame.mode = 18;  //模式18，单参数写入
201 |     motor_con_frame.data = MASTER_ID;
202 |     motor_con_frame.id = this->id;    //电机ID
203 |     for (int j = 0; j <= 7; j++) {
204 |         motor_con_frame.tx_data[j] = 0;
205 |     }
206 |     motor_con_frame.tx_data[4] = mode;
207 |     memcpy(&motor_con_frame.tx_data[0], &parameter, 2);
208 |     CAN_Send_Frame(&motor_con_frame);
209 | }
210 | 
211 | /**
212 |       * @brief          电机初始化，在上位机查看电机ID和主机ID，主机ID默认为0
213 |       * @param[in]      motor_id 电机ID
214 |       */
215 | void MI_Motor_::Motor_Con_Init(uint8_t motor_id)
216 | {
217 |     this->id = motor_id;
218 | }
219 | 
220 | /**
221 |       * @brief          CAN初始化
222 |       */
223 | void Motor_CAN_Init(void)
224 | {
225 |     ESP32Can.setPins(CAN_TX, CAN_RX);                //设置pin脚
226 |     ESP32Can.setRxQueueSize(50);                     //设置缓存区大小
227 |     ESP32Can.setTxQueueSize(50);                     //设置缓存区大小
228 |     ESP32Can.setSpeed(ESP32Can.convertSpeed(1000));  //设置速度，一定要1M，其它库都没有1M，就这个库有
229 |     ESP32Can.begin();
230 | }
231 | 
232 | /**
233 |       * @brief          更新电机数据
234 |       * @param[in]      timeout 超时时间
235 |       * @retval         err_sta 0：正常读取；1：读取到数据，但不是自己的；2：没有读取到数据
236 |       */
237 | uint8_t MI_Motor_::Motor_Data_Updata(uint32_t timeout)
238 | {
239 |     uint8_t err_sta = 0;
240 |     if (ESP32Can.readFrame(rxFrame, timeout)) {
241 |         if (RX_29ID_DISASSEMBLE_MOTOR_ID(rxFrame.identifier) == this->id) {
242 |             Decoding_Motor_data(&rxFrame, &motor_rx_data);
243 |         } else {
244 |             err_sta = 1;
245 |         }
246 |     } else {
247 |         err_sta = 2;
248 |     }
249 | 
250 |     return err_sta;
251 | }


--------------------------------------------------------------------------------
/arduino/sim_setup_motor/TWAI_CAN_MI_Motor.cpp:
--------------------------------------------------------------------------------
  1 | #include "Arduino.h"
  2 | #include "TWAI_CAN_MI_Motor.h"
  3 | 
  4 | //把浮点数转换成uint_16 用在位置 扭矩 上面
  5 | static int float_to_uint(float x, float x_min, float x_max, int bits)
  6 | {
  7 |   float span = x_max - x_min;
  8 |   float offset = x_min;
  9 |   if (x > x_max) x = x_max;
 10 |   else if (x < x_min) x = x_min;
 11 |   return (int)((x - offset) * ((float)((1 << bits) - 1)) / span);
 12 | }
 13 | 
 14 | //底层的CAN发送指令，小米电机采用扩展帧，数据帧的格式
 15 | static void CAN_Send_Frame(can_frame_t* frame)
 16 | {
 17 |     CanFrame obdFrame = { 0 };
 18 |     uint32_t id_val, data_val, mode_val;
 19 |     uint32_t combined_val;
 20 | 
 21 |     obdFrame.extd = 1;              //0-标准帧; 1-扩展帧
 22 |     obdFrame.rtr = 0;               //0-数据帧; 1-远程帧
 23 |     obdFrame.ss = 0;                //0-错误重发; 1-单次发送(仲裁或丢失时消息不会被重发)，对接收消息无效
 24 |     obdFrame.self = 0;              //0-不接收自己发送的消息，1-接收自己发送的消息，对接收消息无效
 25 |     obdFrame.dlc_non_comp = 0;      //0-数据长度不大于8(ISO 11898-1); 1-数据长度大于8(非标);
 26 | 
 27 |     //拼接ID
 28 |     id_val = frame->id;
 29 |     data_val = frame->data;
 30 |     mode_val = frame->mode;
 31 |     combined_val |= (mode_val << 24);
 32 |     combined_val |= (data_val << 8);
 33 |     combined_val |= id_val;
 34 | 
 35 |     obdFrame.identifier = combined_val; //普通帧直接写id，扩展帧需要计算。11/29位ID
 36 |     obdFrame.data_length_code = 8;      //要发送的字节数
 37 | 
 38 |     for (int i = 0; i < 8; i++)
 39 |     {
 40 |         obdFrame.data[i] = frame->tx_data[i];
 41 |     }
 42 |     ESP32Can.writeFrame(obdFrame);
 43 | }
 44 | 
 45 | /**
 46 |       * @brief          电机使能
 47 |       */
 48 | void MI_Motor_::Motor_Enable(void)
 49 | {
 50 |     can_frame_t motor_con_frame;
 51 |     motor_con_frame.mode = 3;           //模式3，使能
 52 |     motor_con_frame.id = this->id;      //目标电机ID
 53 |     motor_con_frame.data = MASTER_ID;   //本机ID
 54 |     for (int i = 0; i < 8; i++)
 55 |     {
 56 |         motor_con_frame.tx_data[i] = 0;
 57 |     }
 58 |     CAN_Send_Frame(&motor_con_frame);
 59 | }
 60 | 
 61 | /**
 62 |       * @brief          电机停止
 63 |       */
 64 | void MI_Motor_::Motor_Reset(void)
 65 | {
 66 |     can_frame_t motor_con_frame;
 67 |     motor_con_frame.mode = 4;           //模式4，停止
 68 |     motor_con_frame.id = this->id;      //目标电机ID
 69 |     motor_con_frame.data = MASTER_ID;   //本机ID
 70 |     for (int i = 0; i < 8; i++)
 71 |     {
 72 |         motor_con_frame.tx_data[i] = 0;
 73 |     }
 74 |     CAN_Send_Frame(&motor_con_frame);
 75 | }
 76 | 
 77 | /**
 78 |       * @brief          设置电机机械零点
 79 |       */
 80 | void MI_Motor_::Motor_Set_Zero(void)
 81 | {
 82 |     can_frame_t motor_con_frame;
 83 |     motor_con_frame.mode = 6;           //模式6，设置零点
 84 |     motor_con_frame.id = this->id;      //目标电机ID
 85 |     motor_con_frame.data = MASTER_ID;   //本机ID
 86 |     for (int i = 0; i < 8; i++)
 87 |     {
 88 |         motor_con_frame.tx_data[i] = 0;
 89 |     }
 90 |     CAN_Send_Frame(&motor_con_frame);
 91 | }
 92 | 
 93 | /**
 94 |       * @brief          运动控制模式
 95 |       * @param[in]      torque 扭矩，N·m，-12到+12
 96 |       * @param[in]      MechPosition 角度，rad，-12.5到+12.5
 97 |       * @param[in]      speed 角速度，rad/s，-30到+30
 98 |       * @param[in]      kp 比例增益，0到500
 99 |       * @param[in]      kd 微分增益，0到5
100 |       */
101 | void MI_Motor_::Motor_ControlMode(float torque, float MechPosition, float speed, float kp, float kd)
102 | {
103 |     can_frame_t motor_con_frame;
104 |     motor_con_frame.mode = 1;           //模式1,运控模式
105 |     motor_con_frame.id = this->id;      //目标电机ID
106 |     motor_con_frame.data = float_to_uint(torque, T_MIN, T_MAX, 16);
107 | 
108 |     motor_con_frame.tx_data[0] = float_to_uint(MechPosition, P_MIN, P_MAX, 16) >> 8;
109 |     motor_con_frame.tx_data[1] = float_to_uint(MechPosition, P_MIN, P_MAX, 16);
110 |     motor_con_frame.tx_data[2] = float_to_uint(speed, V_MIN, V_MAX, 16) >> 8;
111 |     motor_con_frame.tx_data[3] = float_to_uint(speed, V_MIN, V_MAX, 16);
112 |     motor_con_frame.tx_data[4] = float_to_uint(kp, KP_MIN, KP_MAX, 16) >> 8;
113 |     motor_con_frame.tx_data[5] = float_to_uint(kp, KP_MIN, KP_MAX, 16);
114 |     motor_con_frame.tx_data[6] = float_to_uint(kd, KD_MIN, KD_MAX, 16) >> 8;
115 |     motor_con_frame.tx_data[7] = float_to_uint(kd, KD_MIN, KD_MAX, 16);
116 |     CAN_Send_Frame(&motor_con_frame);
117 | }
118 | 
119 | /**
120 |       * @brief          设置单参数
121 |       * @param[in]      parameter 参数数据
122 |       * @param[in]      index 参数列表，详见电机说明书的4.1.11
123 |       */
124 | void MI_Motor_::Set_SingleParameter(uint16_t parameter, float index)
125 | {
126 |     can_frame_t motor_con_frame;
127 |     motor_con_frame.mode = 18;          //模式18,单参数修改模式
128 |     motor_con_frame.id = this->id;      //目标电机ID
129 |     motor_con_frame.data = MASTER_ID;   //来标识主CAN_ID
130 | 
131 |     motor_con_frame.tx_data[2] = 0;
132 |     motor_con_frame.tx_data[3] = 0;     //Byte2~3: 00
133 | 
134 |     memcpy(&motor_con_frame.tx_data[0], &parameter, 2);   //Byte0~1: index，参数列表详见4.1.11
135 |     memcpy(&motor_con_frame.tx_data[4], &index, 4);       //Byte4~7: 参数数据，1字节数据在Byte4
136 |     CAN_Send_Frame(&motor_con_frame);
137 | }
138 | 
139 | /**
140 |       * @brief          解析电机发送数据包
141 |       * @param[in]      can_Frame_point CAN原始数据包的指针
142 |       * @param[in]      motor_frame_point 电机解析数据包的指针
143 |       */
144 | void MI_Motor_::Decoding_Motor_data(CanFrame* can_Frame_point, can_rx_frame_t* motor_frame_point)
145 | {
146 |     motor_frame_point->master_id = RX_29ID_DISASSEMBLE_MASTER_ID(can_Frame_point->identifier);
147 |     motor_frame_point->motor_id = RX_29ID_DISASSEMBLE_MOTOR_ID(can_Frame_point->identifier);
148 |     motor_frame_point->err_sta = RX_29ID_DISASSEMBLE_ERR_STA(can_Frame_point->identifier);
149 |     motor_frame_point->HALL_err = RX_29ID_DISASSEMBLE_HALL_ERR(can_Frame_point->identifier);
150 |     motor_frame_point->magnet_err = RX_29ID_DISASSEMBLE_MAGNET_ERR(can_Frame_point->identifier);
151 |     motor_frame_point->temp_err = RX_29ID_DISASSEMBLE_TEMP_ERR(can_Frame_point->identifier);
152 |     motor_frame_point->current_err = RX_29ID_DISASSEMBLE_CURRENT_ERR(can_Frame_point->identifier);
153 |     motor_frame_point->voltage_err = RX_29ID_DISASSEMBLE_VOLTAGE_ERR(can_Frame_point->identifier);
154 |     motor_frame_point->mode_sta = RX_29ID_DISASSEMBLE_MODE_STA(can_Frame_point->identifier);
155 | 
156 |     //弧度单位
157 |     motor_frame_point->cur_angle = RX_DATA_DISASSEMBLE_CUR_ANGLE(can_Frame_point->data) * INT2ANGLE - 4 * PI;
158 |     motor_frame_point->cur_speed = RX_DATA_DISASSEMBLE_CUR_SPEED(can_Frame_point->data) * INT2SPEED - 30;
159 |     motor_frame_point->cur_torque = RX_DATA_DISASSEMBLE_CUR_TORQUE(can_Frame_point->data) * INT2TORQUE - 12;
160 |     motor_frame_point->cur_temp = RX_DATA_DISASSEMBLE_CUR_TEMP(can_Frame_point->data) / 10.0f;
161 | }
162 | 
163 | /**
164 |       * @brief          设置速度模式的参数
165 |       * @param[in]      speed 角速度，rad/s，-30到+30
166 |       */
167 | void MI_Motor_::Set_SpeedMode(float speed)
168 | {
169 |     Set_SingleParameter(SPD_REF, speed);
170 | }
171 | 
172 | /**
173 |       * @brief          设置位置模式的参数
174 |       * @param[in]      position 角度，rad，-12.5到+12.5
175 |       * @param[in]      max_speed 角速度，rad/s，0到+30
176 |       */
177 | void MI_Motor_::Set_PosMode(float position, float max_speed)
178 | {
179 |     Set_SingleParameter(LIMIT_SPD, max_speed);
180 |     Set_SingleParameter(LOC_REF, position);
181 | }
182 | 
183 | /**
184 |       * @brief          设置电流模式的参数
185 |       * @param[in]      current 电流，A，-23到+23
186 |       */
187 | void MI_Motor_::Set_CurMode(float current)
188 | {
189 |     Set_SingleParameter(IQ_REF, current);
190 | }
191 | 
192 | /**
193 |       * @brief          设置运动模式
194 |       * @param[in]      mode 运控模式：CTRL_MODE 电流模式：CUR_MODE 速度模式：SPEED_MODE 位置模式：POS_MODE
195 |       */
196 | void MI_Motor_::Change_Mode(uint8_t mode)
197 | {
198 |     uint16_t parameter = RUN_MODE;
199 |     can_frame_t motor_con_frame;
200 |     motor_con_frame.mode = 18;  //模式18，单参数写入
201 |     motor_con_frame.data = MASTER_ID;
202 |     motor_con_frame.id = this->id;    //电机ID
203 |     for (int j = 0; j <= 7; j++) {
204 |         motor_con_frame.tx_data[j] = 0;
205 |     }
206 |     motor_con_frame.tx_data[4] = mode;
207 |     memcpy(&motor_con_frame.tx_data[0], &parameter, 2);
208 |     CAN_Send_Frame(&motor_con_frame);
209 | }
210 | 
211 | /**
212 |       * @brief          电机初始化，在上位机查看电机ID和主机ID，主机ID默认为0
213 |       * @param[in]      motor_id 电机ID
214 |       */
215 | void MI_Motor_::Motor_Con_Init(uint8_t motor_id)
216 | {
217 |     this->id = motor_id;
218 | }
219 | 
220 | /**
221 |       * @brief          CAN初始化
222 |       */
223 | void Motor_CAN_Init(void)
224 | {
225 |     ESP32Can.setPins(CAN_TX, CAN_RX);                //设置pin脚
226 |     ESP32Can.setRxQueueSize(50);                     //设置缓存区大小
227 |     ESP32Can.setTxQueueSize(50);                     //设置缓存区大小
228 |     ESP32Can.setSpeed(ESP32Can.convertSpeed(1000));  //设置速度，一定要1M，其它库都没有1M，就这个库有
229 |     ESP32Can.begin();
230 | }
231 | 
232 | /**
233 |       * @brief          更新电机数据
234 |       * @param[in]      timeout 超时时间
235 |       * @retval         err_sta 0：正常读取；1：读取到数据，但不是自己的；2：没有读取到数据
236 |       */
237 | uint8_t MI_Motor_::Motor_Data_Updata(uint32_t timeout)
238 | {
239 |     uint8_t err_sta = 0;
240 |     if (ESP32Can.readFrame(rxFrame, timeout)) {
241 |         if (RX_29ID_DISASSEMBLE_MOTOR_ID(rxFrame.identifier) == this->id) {
242 |             Decoding_Motor_data(&rxFrame, &motor_rx_data);
243 |         } else {
244 |             err_sta = 1;
245 |         }
246 |     } else {
247 |         err_sta = 2;
248 |     }
249 | 
250 |     return err_sta;
251 | }


--------------------------------------------------------------------------------
/arduino/advanced_motor_control/TWAI_CAN_MI_Motor.cpp:
--------------------------------------------------------------------------------
  1 | #include "Arduino.h"
  2 | #include "TWAI_CAN_MI_Motor.h"
  3 | 
  4 | //把浮点数转换成uint_16 用在位置 扭矩 上面
  5 | static int float_to_uint(float x, float x_min, float x_max, int bits)
  6 | {
  7 |   float span = x_max - x_min;
  8 |   float offset = x_min;
  9 |   if (x > x_max) x = x_max;
 10 |   else if (x < x_min) x = x_min;
 11 |   return (int)((x - offset) * ((float)((1 << bits) - 1)) / span);
 12 | }
 13 | 
 14 | //底层的CAN发送指令，小米电机采用扩展帧，数据帧的格式
 15 | static void CAN_Send_Frame(can_frame_t* frame)
 16 | {
 17 |     CanFrame obdFrame = { 0 };
 18 |     uint32_t id_val, data_val, mode_val;
 19 |     uint32_t combined_val;
 20 | 
 21 |     obdFrame.extd = 1;              //0-标准帧; 1-扩展帧
 22 |     obdFrame.rtr = 0;               //0-数据帧; 1-远程帧
 23 |     obdFrame.ss = 0;                //0-错误重发; 1-单次发送(仲裁或丢失时消息不会被重发)，对接收消息无效
 24 |     obdFrame.self = 0;              //0-不接收自己发送的消息，1-接收自己发送的消息，对接收消息无效
 25 |     obdFrame.dlc_non_comp = 0;      //0-数据长度不大于8(ISO 11898-1); 1-数据长度大于8(非标);
 26 | 
 27 |     //拼接ID
 28 |     id_val = frame->id;
 29 |     data_val = frame->data;
 30 |     mode_val = frame->mode;
 31 |     combined_val |= (mode_val << 24);
 32 |     combined_val |= (data_val << 8);
 33 |     combined_val |= id_val;
 34 | 
 35 |     obdFrame.identifier = combined_val; //普通帧直接写id，扩展帧需要计算。11/29位ID
 36 |     obdFrame.data_length_code = 8;      //要发送的字节数
 37 | 
 38 |     for (int i = 0; i < 8; i++)
 39 |     {
 40 |         obdFrame.data[i] = frame->tx_data[i];
 41 |     }
 42 |     ESP32Can.writeFrame(obdFrame);
 43 | }
 44 | 
 45 | /**
 46 |       * @brief          电机使能
 47 |       */
 48 | void MI_Motor_::Motor_Enable(void)
 49 | {
 50 |     can_frame_t motor_con_frame;
 51 |     motor_con_frame.mode = 3;           //模式3，使能
 52 |     motor_con_frame.id = this->id;      //目标电机ID
 53 |     motor_con_frame.data = MASTER_ID;   //本机ID
 54 |     for (int i = 0; i < 8; i++)
 55 |     {
 56 |         motor_con_frame.tx_data[i] = 0;
 57 |     }
 58 |     CAN_Send_Frame(&motor_con_frame);
 59 | }
 60 | 
 61 | /**
 62 |       * @brief          电机停止
 63 |       */
 64 | void MI_Motor_::Motor_Reset(void)
 65 | {
 66 |     can_frame_t motor_con_frame;
 67 |     motor_con_frame.mode = 4;           //模式4，停止
 68 |     motor_con_frame.id = this->id;      //目标电机ID
 69 |     motor_con_frame.data = MASTER_ID;   //本机ID
 70 |     for (int i = 0; i < 8; i++)
 71 |     {
 72 |         motor_con_frame.tx_data[i] = 0;
 73 |     }
 74 |     CAN_Send_Frame(&motor_con_frame);
 75 | }
 76 | 
 77 | /**
 78 |       * @brief          设置电机机械零点
 79 |       */
 80 | void MI_Motor_::Motor_Set_Zero(void)
 81 | {
 82 |     can_frame_t motor_con_frame;
 83 |     motor_con_frame.mode = 6;           //模式6，设置零点
 84 |     motor_con_frame.id = this->id;      //目标电机ID
 85 |     motor_con_frame.data = MASTER_ID;   //本机ID
 86 |     for (int i = 0; i < 8; i++)
 87 |     {
 88 |         motor_con_frame.tx_data[i] = 0;
 89 |     }
 90 |     CAN_Send_Frame(&motor_con_frame);
 91 | }
 92 | 
 93 | /**
 94 |       * @brief          运动控制模式
 95 |       * @param[in]      torque 扭矩，N·m，-12到+12
 96 |       * @param[in]      MechPosition 角度，rad，-12.5到+12.5
 97 |       * @param[in]      speed 角速度，rad/s，-30到+30
 98 |       * @param[in]      kp 比例增益，0到500
 99 |       * @param[in]      kd 微分增益，0到5
100 |       */
101 | void MI_Motor_::Motor_ControlMode(float torque, float MechPosition, float speed, float kp, float kd)
102 | {
103 |     can_frame_t motor_con_frame;
104 |     motor_con_frame.mode = 1;           //模式1,运控模式
105 |     motor_con_frame.id = this->id;      //目标电机ID
106 |     motor_con_frame.data = float_to_uint(torque, T_MIN, T_MAX, 16);
107 | 
108 |     motor_con_frame.tx_data[0] = float_to_uint(MechPosition, P_MIN, P_MAX, 16) >> 8;
109 |     motor_con_frame.tx_data[1] = float_to_uint(MechPosition, P_MIN, P_MAX, 16);
110 |     motor_con_frame.tx_data[2] = float_to_uint(speed, V_MIN, V_MAX, 16) >> 8;
111 |     motor_con_frame.tx_data[3] = float_to_uint(speed, V_MIN, V_MAX, 16);
112 |     motor_con_frame.tx_data[4] = float_to_uint(kp, KP_MIN, KP_MAX, 16) >> 8;
113 |     motor_con_frame.tx_data[5] = float_to_uint(kp, KP_MIN, KP_MAX, 16);
114 |     motor_con_frame.tx_data[6] = float_to_uint(kd, KD_MIN, KD_MAX, 16) >> 8;
115 |     motor_con_frame.tx_data[7] = float_to_uint(kd, KD_MIN, KD_MAX, 16);
116 |     CAN_Send_Frame(&motor_con_frame);
117 | }
118 | 
119 | /**
120 |       * @brief          设置单参数
121 |       * @param[in]      parameter 参数数据
122 |       * @param[in]      index 参数列表，详见电机说明书的4.1.11
123 |       */
124 | void MI_Motor_::Set_SingleParameter(uint16_t parameter, float index)
125 | {
126 |     can_frame_t motor_con_frame;
127 |     motor_con_frame.mode = 18;          //模式18,单参数修改模式
128 |     motor_con_frame.id = this->id;      //目标电机ID
129 |     motor_con_frame.data = MASTER_ID;   //来标识主CAN_ID
130 | 
131 |     motor_con_frame.tx_data[2] = 0;
132 |     motor_con_frame.tx_data[3] = 0;     //Byte2~3: 00
133 | 
134 |     memcpy(&motor_con_frame.tx_data[0], &parameter, 2);   //Byte0~1: index，参数列表详见4.1.11
135 |     memcpy(&motor_con_frame.tx_data[4], &index, 4);       //Byte4~7: 参数数据，1字节数据在Byte4
136 |     CAN_Send_Frame(&motor_con_frame);
137 | }
138 | 
139 | /**
140 |       * @brief          解析电机发送数据包
141 |       * @param[in]      can_Frame_point CAN原始数据包的指针
142 |       * @param[in]      motor_frame_point 电机解析数据包的指针
143 |       */
144 | void MI_Motor_::Decoding_Motor_data(CanFrame* can_Frame_point, can_rx_frame_t* motor_frame_point)
145 | {
146 |     motor_frame_point->master_id = RX_29ID_DISASSEMBLE_MASTER_ID(can_Frame_point->identifier);
147 |     motor_frame_point->motor_id = RX_29ID_DISASSEMBLE_MOTOR_ID(can_Frame_point->identifier);
148 |     motor_frame_point->err_sta = RX_29ID_DISASSEMBLE_ERR_STA(can_Frame_point->identifier);
149 |     motor_frame_point->HALL_err = RX_29ID_DISASSEMBLE_HALL_ERR(can_Frame_point->identifier);
150 |     motor_frame_point->magnet_err = RX_29ID_DISASSEMBLE_MAGNET_ERR(can_Frame_point->identifier);
151 |     motor_frame_point->temp_err = RX_29ID_DISASSEMBLE_TEMP_ERR(can_Frame_point->identifier);
152 |     motor_frame_point->current_err = RX_29ID_DISASSEMBLE_CURRENT_ERR(can_Frame_point->identifier);
153 |     motor_frame_point->voltage_err = RX_29ID_DISASSEMBLE_VOLTAGE_ERR(can_Frame_point->identifier);
154 |     motor_frame_point->mode_sta = RX_29ID_DISASSEMBLE_MODE_STA(can_Frame_point->identifier);
155 | 
156 |     //弧度单位
157 |     motor_frame_point->cur_angle = RX_DATA_DISASSEMBLE_CUR_ANGLE(can_Frame_point->data) * INT2ANGLE - 4 * PI;
158 |     motor_frame_point->cur_speed = RX_DATA_DISASSEMBLE_CUR_SPEED(can_Frame_point->data) * INT2SPEED - 30;
159 |     motor_frame_point->cur_torque = RX_DATA_DISASSEMBLE_CUR_TORQUE(can_Frame_point->data) * INT2TORQUE - 12;
160 |     motor_frame_point->cur_temp = RX_DATA_DISASSEMBLE_CUR_TEMP(can_Frame_point->data) / 10.0f;
161 | }
162 | 
163 | /**
164 |       * @brief          设置速度模式的参数
165 |       * @param[in]      speed 角速度，rad/s，-30到+30
166 |       */
167 | void MI_Motor_::Set_SpeedMode(float speed)
168 | {
169 |     Set_SingleParameter(SPD_REF, speed);
170 | }
171 | 
172 | /**
173 |       * @brief          设置位置模式的参数
174 |       * @param[in]      position 角度，rad，-12.5到+12.5
175 |       * @param[in]      max_speed 角速度，rad/s，0到+30
176 |       */
177 | void MI_Motor_::Set_PosMode(float position, float max_speed)
178 | {
179 |     Set_SingleParameter(LIMIT_SPD, max_speed);
180 |     Set_SingleParameter(LOC_REF, position);
181 | }
182 | 
183 | /**
184 |       * @brief          设置电流模式的参数
185 |       * @param[in]      current 电流，A，-23到+23
186 |       */
187 | void MI_Motor_::Set_CurMode(float current)
188 | {
189 |     Set_SingleParameter(IQ_REF, current);
190 | }
191 | 
192 | /**
193 |       * @brief          设置运动模式
194 |       * @param[in]      mode 运控模式：CTRL_MODE 电流模式：CUR_MODE 速度模式：SPEED_MODE 位置模式：POS_MODE
195 |       */
196 | void MI_Motor_::Change_Mode(uint8_t mode)
197 | {
198 |     uint16_t parameter = RUN_MODE;
199 |     can_frame_t motor_con_frame;
200 |     motor_con_frame.mode = 18;  //模式18，单参数写入
201 |     motor_con_frame.data = MASTER_ID;
202 |     motor_con_frame.id = this->id;    //电机ID
203 |     for (int j = 0; j <= 7; j++) {
204 |         motor_con_frame.tx_data[j] = 0;
205 |     }
206 |     motor_con_frame.tx_data[4] = mode;
207 |     memcpy(&motor_con_frame.tx_data[0], &parameter, 2);
208 |     CAN_Send_Frame(&motor_con_frame);
209 | }
210 | 
211 | /**
212 |       * @brief          电机初始化，在上位机查看电机ID和主机ID，主机ID默认为0
213 |       * @param[in]      motor_id 电机ID
214 |       */
215 | void MI_Motor_::Motor_Con_Init(uint8_t motor_id)
216 | {
217 |     this->id = motor_id;
218 | }
219 | 
220 | /**
221 |       * @brief          CAN初始化
222 |       */
223 | void Motor_CAN_Init(void)
224 | {
225 |     ESP32Can.setPins(CAN_TX, CAN_RX);                //设置pin脚
226 |     ESP32Can.setRxQueueSize(50);                     //设置缓存区大小
227 |     ESP32Can.setTxQueueSize(50);                     //设置缓存区大小
228 |     ESP32Can.setSpeed(ESP32Can.convertSpeed(1000));  //设置速度，一定要1M，其它库都没有1M，就这个库有
229 |     ESP32Can.begin();
230 | }
231 | 
232 | /**
233 |       * @brief          更新电机数据
234 |       * @param[in]      timeout 超时时间
235 |       * @retval         err_sta 0：正常读取；1：读取到数据，但不是自己的；2：没有读取到数据
236 |       */
237 | uint8_t MI_Motor_::Motor_Data_Updata(uint32_t timeout)
238 | {
239 |     uint8_t err_sta = 0;
240 |     if (ESP32Can.readFrame(rxFrame, timeout)) {
241 |         if (RX_29ID_DISASSEMBLE_MOTOR_ID(rxFrame.identifier) == this->id) {
242 |             Decoding_Motor_data(&rxFrame, &motor_rx_data);
243 |         } else {
244 |             err_sta = 1;
245 |         }
246 |     } else {
247 |         err_sta = 2;
248 |     }
249 | 
250 |     return err_sta;
251 | }


--------------------------------------------------------------------------------