servoJ를 사용한 관절 제어

인터페이스 설명

/**
 * @brief 관절 추적 모드 열기
 * @param vmax 속도 제한 단위 도/초
 * @param amax 가속도 제한 단위 도/초^2
 * @param jmax 가가속도 제한 단위 도/초^3
 */
Result open_servoJ(SOCKETFD socketFd, std::vector<double> vmax, std::vector<double> amax, std::vector<double> jmax);


/**
 * @brief 추적 관절 위치 전송
 * @param q 목표 위치 단위 도
 */
Result set_servoJ_pos(SOCKETFD socketFd, std::vector<double> q);


/**
 * @brief 관절 추적 모드 닫기
 */
Result stop_servoJ(SOCKETFD socketFd);

사용 예시

int main()
{
    // 6000 포트에 연결하여 전원 켜기 제어 및 로봇의 현재 위치 조회
    SOCKETFD fd_6000 = connect_robot("192.168.1.13", "6000");
    // 7000 포트에 연결
    SOCKETFD fd_7000 = connect_robot("192.168.1.13", "7000");
    if (fd_6000 < 0 || fd_7000 < 0) 
    {
        return 0; // 연결 실패, 종료.
    }
    // 로봇 전원 켜기 설정
    std::cout << "준비: " <<  set_servo_state(fd_6000, 1)<<std::endl;
    std::cout << "전원 켜기: " << set_servo_poweron(fd_6000)<<std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(30));
    
    // 운동 제한 설정
    std::vector<double> vMax = {10, 10, 10, 10, 10, 10, 10};  // 속도 10°/s
    std::vector<double> avMax = {1000, 1000, 1000, 1000, 1000, 1000, 1000}; // 가속도 1000°/s^2
    std::vector<double> jMax = {1000, 1000, 1000, 1000, 1000, 1000, 1000}; // 가가속도 1000°/s^3
    std::cout << "관절 추종 열기" << open_servoJ(fd_7000, vMax, avMax, jMax) << std::endl;


    int n = 100
    while(n > 0)
    {
        n--;
        std::cout << "현재 위치: " << get_current_position(fd_6000, 0, pos) << std::endl;
    
        // 첫 번째 요소 pos[0]에만 0.5를 더하며, 로봇의 1축에 해당
        pos[0] += 0.2;
    
        std::cout << "관절 추종 전송" << set_servoJ_pos(fd_7000, pos) << std::endl;
        // 10ms마다 새 위치를 전송
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }


    // servoJ 모드 중지
    stop_servoJ(fd_7000);
    return 0;
}