Diff: main.cpp

Revision:

2:92f3aa5245dc

Parent:

1:2c82c636c43e

--- a/main.cpp	Wed Jun 13 03:50:30 2018 +0000
+++ b/main.cpp	Wed Jun 13 08:31:51 2018 +0000
@@ -20,354 +20,16 @@
  * 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;
-        }
-};
-};
-
+#include "dynamixel_servo_controller.h"
 
 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);