File content as of revision 2:92f3aa5245dc:

/*
 * Copyright (c) 2018 Yusuke Okino

 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:

 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "dynamixel_servo_controller.h"

#ifndef MBED_ENVIRONMENT
    #include <iostream>
    #include <cstdint>
#endif


/**
 * @brief Dynamixel サーボコントローラ protocol v2
 * 
 */
namespace dynamixel_servo_v2
{

namespace 
{
    // チェックサム計算クラス( 今後実装 )
    #if 0
    class CRC16 
    {
        private:

        public:
            /**
             * @brief Construct a new CRC16 object
             * 
             * @param polynomial 
             */
            CRC16(const uint16_t polynomial)
            {
                /******************** CRC16 用テーブル生成 *********************/

                /**************************************************************/
            }

    };
    #endif
};

    
        
#ifdef MBED_ENVIRONMENT

/**
 * @brief UART 受信割り込み関数
 * 
 */
void XM430::UART_Rx_Callback()
{

}
#endif


/**
 * @brief Construct a new XM430 object
 * 
 */
#ifdef MBED_ENVIRONMENT
XM430::XM430(PinName tx_pin, PinName rx_pin)
:   uart(tx_pin, rx_pin)
{
    uart.baud(57600);
    uart.format(8, Serial::None, 1);
    // UART受信割り込み
    uart.attach(callback(this, &XM430::UART_Rx_Callback), Serial::RxIrq);
    
#else
XM430::XM430(PinName tx_pin, PinName rx_pin)
{
#endif
    
    /******************** CRC16 用テーブル生成 *********************/
    uint16_t temp = 0;

    for(uint32_t i = 0; i < 256; i ++)
    {
        temp = (uint16_t)(i << 8);

        for(uint32_t j = 0; j < 8; j ++)
        {
            // tempの最上位ビットが0: ビットシフト
            // tempの最上位ビットが1: ビットシフト+XOR
            if((temp & 0x8000) == 0)
            {
                temp = (temp << 1);
            }
            else
            {
                temp = (temp << 1) ^ CRC16_POLY;
            }
        }
        crc_tbl[i] = temp;
    }
    /**************************************************************/

    #if 0
    for(uint32_t i = 0; i < 256; i ++)
    {
        std::cout << std::hex << crc_tbl[i] << std::endl;
    }
    #endif

    // Header
    tx_buf[0] = 0xFF;
    tx_buf[1] = 0xFF;
    tx_buf[2] = 0xFD;

    // Reserved
    tx_buf[3] = 0x00;
}

/**
 * @brief チェックサム計算( crc16 )
 * 
 * @param crc_init_val crc 初期値
 * @param data 検査対象のデータ列
 * @param data_length 
 * @return uint16_t チェックサム計算結果
 */
uint16_t XM430::CRC16(const uint16_t crc_init_val, uint8_t *data, const uint32_t data_length)
{
    uint32_t index;
    uint16_t temp = crc_init_val;

    for(uint32_t i = 0; i < data_length; i++)
    {
        index = ((uint16_t)(temp >> 8) ^ data[i]) & 0xFF;
        temp = (temp << 8) ^ crc_tbl[index];
    }

    return temp;
}

/**
 * @brief パケット生成
 * 
 * @param servo_id 
 * @param length 
 * @param instruction 
 * @param ctrl_reg control レジスタ
 * @param data 送信データ
 */
void XM430::Create_Packet(uint8_t servo_id, uint16_t length, const uint8_t instruction,
                    const uint16_t ctrl_reg, uint8_t *data)
{
    uint16_t checksum;

    // ID
    tx_buf[4] = servo_id;

    // Length
    tx_buf[5] = (uint8_t)(length & 0xFF);
    tx_buf[6] = (uint8_t)(length >> 8);

    // Instruction
    tx_buf[7] = instruction;

    // control register
    tx_buf[8] = (uint8_t)(ctrl_reg & 0xFF);
    tx_buf[9] = (uint8_t)(ctrl_reg >> 8);

    // Data
    for(uint32_t i = 0; i < ( length-5 ); i ++)
    {
        tx_buf[ i+10 ] = data[i];
    }
    
    // Checksum
    checksum = CRC16(0, tx_buf, ( length+5 ));
    tx_buf[ length+5 ] = (uint8_t)(checksum & 0xFF);
    tx_buf[ length+6 ] = (uint8_t)(checksum >> 8);
}

/**
 * @brief シリアルバルク送信
 * 
 * @param buf 送信バッファ
 * @param buf_length バッファ長
 * @return true 正常に通信が終了
 * @return false 通信が不正に終了
 */
bool XM430::Send_Bulk_Char(uint8_t *buf, const uint32_t buf_length)
{
    #ifdef MBED_ENVIRONMENT
        // ユーザ関数

        return true;
    #else
        for(uint32_t i = 0; i < buf_length; i ++)
        {
            //std::cout << tx_buf[i] << std::endl;
            std::cout << std::hex << std::showbase << std::uppercase << (uint32_t)tx_buf[i] << std::endl;
        }

        return true;
    #endif

    return false;
}

/**
 * @brief シリアルバルク受信
 * 
 * @param buf 受信バッファ
 * @param buf_length バッファ長
 * @param timeout 通信途絶を判断するまでの時間
 * @return true 正常に受信
 * @return false 受信失敗
 */
bool XM430::Read_Bulk_Char(uint8_t *buf, const uint32_t buf_length, const uint32_t timeout)
{
    #ifdef MBED_ENVIRONMENT
    uint32_t count = 0;

    count = 0;
    while(0 == uart.readable())
    {
        if(count > timeout)
        {
            return false;
        }
    }
    #endif

    return false;
}

/**
 * @brief サーボのトルクをONにする
 * 
 * @param id サーボID
 * @return true 通信が正常に終了
 * @return false 通信が不正に終了
 */
bool XM430::Torque_ON(uint8_t id)
{
    uint16_t length = 6;
    uint8_t data[1];       // トルクON/OFF

    bool ret_val;

    data[0] = 1;            // トルクON

    Create_Packet(id, length, WRITE, TORQUE_ENABLE, data);

    ret_val = Send_Bulk_Char(tx_buf, 13);
    return ret_val;
}

/**
 * @brief サーボのトルクをOFFにする
 * 
 * @param id サーボID
 * @return true 通信が正常に終了
 * @return false 通信が不正に終了
 */
bool XM430::Torque_OFF(uint8_t id)
{
    uint16_t length = 6;
    uint8_t data[1];       // トルクON/OFF

    bool ret_val;

    data[0] = 0;            // トルクOFF

    Create_Packet(id, length, WRITE, TORQUE_ENABLE, data);

    ret_val = Send_Bulk_Char(tx_buf, 13);
    return ret_val;
}

/**
 * @brief サーボホーン位置を設定
 * 
 * @param id サーボID
 * @param pos 目標回転位置
 * @return true 通信が正常に終了
 * @return false 通信が不正に終了
 */
bool XM430::Set_Pos(uint8_t id, uint32_t pos)
{
    uint16_t length = 9;
    uint8_t data[4];    // 位置データ

    bool ret_val;

    // Data
    data[0] = (uint8_t)(pos & 0xFF);
    data[1] = (uint8_t)((pos >> 8) & 0xFF);
    data[2] = (uint8_t)((pos >> 16) & 0xFF);
    data[3] = (uint8_t)((pos >> 24) & 0xFF);

    Create_Packet(id, length, WRITE, GOAL_POSITION, data);
    
    ret_val = Send_Bulk_Char(tx_buf, 16);
    return ret_val;
}

};