File content as of revision 1:2c82c636c43e:

/*
 * 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.
 */


#define MBED_ENVIRONMENT

#ifdef MBED_ENVIRONMENT
    #include "mbed.h"
#else
    #include <iostream>
    #include <cstdint>
#endif

#ifndef MBED_ENVIRONMENT
    typedef uint16_t PinName;
    const PinName PA_9 = 9;
    const PinName PA_10 = 10;
#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
};

/**
 * @brief Dynamixel XM430サーボ制御用クラス
 * 
 */
class XM430
{
    private:

        // Checksum polynomial
        const static uint16_t CRC16_POLY = 0x8005;

        /********************* レジスタマップ ***********************/
        /***************** instruction *******************/
        const static uint8_t WRITE = 0x03;
        /*************************************************/

        /**************** Control table ******************/
        const static uint16_t TORQUE_ENABLE = 64;   
        const static uint16_t GOAL_POSITION = 116;
        /*************************************************/
        /***********************************************************/

        uint16_t crc_tbl[256];  // CRC16 計算用テーブル
        uint8_t tx_buf[128];    // シリアル送信バッファ
        uint8_t rx_buf[128];    // シリアル受信バッファ
        
        #ifdef MBED_ENVIRONMENT
        Serial uart;            // シリアル通信用クラス

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

        }
        #endif

    public:
        /**
         * @brief Construct a new XM430 object
         * 
         */
        #ifdef MBED_ENVIRONMENT
        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(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;
        }


    private:

        /**
         * @brief チェックサム計算( crc16 )
         * 
         * @param crc_init_val crc 初期値
         * @param data 検査対象のデータ列
         * @param data_length 
         * @return uint16_t チェックサム計算結果
         */
        uint16_t 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 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 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 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;
        }

    public:
        /**
         * @brief サーボのトルクをONにする
         * 
         * @param id サーボID
         * @return true 通信が正常に終了
         * @return false 通信が不正に終了
         */
        bool 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 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 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;
        }
};
};


int main()
{
    const uint8_t servo_id = 0x01;

    dynamixel_servo_v2::XM430 xm430(PA_9, PA_10);

    xm430.Torque_ON(servo_id);
    
    // 少し待つ処理が必要( サーボからの返答を受信する処理を入れたら不要になる )
    #ifdef MBED_ENVIRONMENT 
        wait(0.1); 
    #endif

    xm430.Set_Pos(servo_id, 999);
    
    
    #ifdef MBED_ENVIRONMENT
    while(1)
    {
        
    }
    #endif

    return 0;
}