# hightorque_robot 库使用指南

## 安装库

### 1. 编译并安装库
```bash
mkdir build && cd build
cmake -DCMAKE_INSTALL_PREFIX=/usr/local ..
make -j8
sudo make install
```

### 2. 或者安装到自定义位置
```bash
mkdir build && cd build
cmake -DCMAKE_INSTALL_PREFIX=$HOME/local ..
make -j8
make install
```

## 在其他CMake项目中使用

### CMakeLists.txt 配置

```cmake
cmake_minimum_required(VERSION 3.0.2)
project(my_robot_project)

# 查找hightorque_robot库
find_package(hightorque_robot REQUIRED)

# 创建可执行文件
add_executable(my_robot_app main.cpp)

# 链接库（会自动链接所有依赖项）
target_link_libraries(my_robot_app hightorque_robot::hightorque_robot)
```

### 如果安装到自定义位置
```bash
# 设置CMAKE_PREFIX_PATH
export CMAKE_PREFIX_PATH=$HOME/local:$CMAKE_PREFIX_PATH

# 或者在cmake时指定
cmake -DCMAKE_PREFIX_PATH=$HOME/local ..
```

## C++ 代码示例

### 基本使用（类似example/motor_run.cpp）

```cpp
#include "robot.hpp"  // 主头文件
#include <iostream>

int main() {
    // 创建机器人实例（会自动加载robot_param/robot_config.yaml配置）
    hightorque_robot::robot rb;
    
    // 启用LCM消息发布
    rb.lcm_enable();
    
    const int motor_num = rb.Motors.size();
    std::cout << "发现 " << motor_num << " 个电机" << std::endl;
    
    float angle = 0.2;
    int count = 0;
    
    while(true) {
        // 控制所有电机
        for (int i = 0; i < motor_num; i++) {
            if(i < motor_num/2) {
                // 前半部分电机正向运动
                rb.Motors[i]->pos_vel_MAXtqe(angle, 0.1, 10);
            } else {
                // 后半部分电机反向运动  
                rb.Motors[i]->pos_vel_MAXtqe(-angle, 0.1, 10);
            }
        }
        
        // 发送命令
        rb.motor_send_cmd();
        
        // 读取并显示电机状态
        for (auto* m : rb.Motors) {
            motor_back_t motor = *m->get_current_motor_state();
            std::cout << "ID:" << motor.ID 
                     << ",位置:" << motor.position
                     << ",速度:" << motor.velocity  
                     << ",扭矩:" << motor.torque << std::endl;
        }
        
        // 每250次循环改变方向
        count++;
        if(count >= 250) {
            count = 0;
            angle *= -1;
        }
        
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
    
    return 0;
}
```

### 自定义配置文件路径

```cpp
#include "robot.hpp"
#include "parse_robot_params.hpp"

int main() {
    // 可以在创建robot之前修改配置文件路径
    // 默认配置文件位置：/usr/local/share/hightorque_robot/robot_param/
    
    hightorque_robot::robot rb;
    // ... 其他代码
}
```

## 电机控制方法

### 位置控制
```cpp
// 位置 + 速度 + 最大扭矩控制
motor->pos_vel_MAXtqe(position, velocity, max_torque);
```

### 获取电机状态
```cpp
motor_back_t state = *motor->get_current_motor_state();
std::cout << "位置: " << state.position << std::endl;
std::cout << "速度: " << state.velocity << std::endl; 
std::cout << "扭矩: " << state.torque << std::endl;
std::cout << "故障码: " << std::hex << state.fault << std::endl;
```

### 安全功能
```cpp
// 设置所有电机停止
rb.set_stop();

// 复位所有电机
rb.set_reset();

// 设置零点
rb.set_reset_zero();

// 只对特定电机设置零点
rb.set_reset_zero({1, 2, 3}); // 电机ID 1,2,3

// 设置超时时间（毫秒）
rb.set_timeout(1000); // 1秒超时
```

## 配置文件

安装后，默认配置文件位于：
- `/usr/local/share/hightorque_robot/robot_param/robot_config.yaml`
- 可以复制并修改这些配置文件来适配你的机器人配置

## 故障排除

### 找不到库
```bash
# 检查安装路径
find /usr/local -name "*hightorque_robot*"

# 检查PKG_CONFIG_PATH
echo $PKG_CONFIG_PATH
```

### 依赖项问题
确保已安装所有依赖项：
```bash
sudo apt-get install libserialport-dev libyaml-cpp-dev
```

### 串口权限
```bash
# 添加用户到dialout组
sudo usermod -a -G dialout $USER
# 需要重新登录生效
```