Diff: main.cpp

Revision:

1:2c82c636c43e

Parent:

0:c9df5db1b525

Child:

2:92f3aa5245dc

--- a/main.cpp	Sat Jun 09 12:09:04 2018 +0000
+++ b/main.cpp	Wed Jun 13 03:50:30 2018 +0000
@@ -1,25 +1,72 @@
-/**
- * @brief Dynamixelサーボコントローラ
- * 
- * @file main.cpp
- * @author Yusuke Okino
- * @date 2018-06-09
+/*
+ * 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 <iostream>
-#include <cstdint>
+
+#define MBED_ENVIRONMENT
+
+#ifdef MBED_ENVIRONMENT
+    #include "mbed.h"
+#else
+    #include <iostream>
+    #include <cstdint>
+#endif
 
-// Checksum polynomial
-#define     CRC16POLY       0x8005
+#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
+{
 
-/***************** instruction *******************/
-#define     WRITE           0x03
-/*************************************************/
+namespace 
+{
+    // チェックサム計算クラス( 今後実装 )
+    #if 0
+    class CRC16 
+    {
+        private:
 
-/**************** Control table ******************/
-#define     TORQUE_ENABLE   64   
-#define     GOAL_POSITION   116
-/*************************************************/
+        public:
+            /**
+             * @brief Construct a new CRC16 object
+             * 
+             * @param polynomial 
+             */
+            CRC16(const uint16_t polynomial)
+            {
+                /******************** CRC16 用テーブル生成 *********************/
+
+                /**************************************************************/
+            }
+
+    };
+    #endif
+};
 
 /**
  * @brief Dynamixel XM430サーボ制御用クラス
@@ -29,75 +76,175 @@
 {
     private:
 
-        uint8_t tx_buf[128];
+        // 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 CRC16 IBM polynomial 0x8005 
+         * @brief UART 受信割り込み関数
+         * 
+         */
+        void UART_Rx_Callback()
+        {
+
+        }
+        #endif
+
+    public:
+        /**
+         * @brief Construct a new XM430 object
          * 
-         * @param crc_accum 
-         * @param data_blk_ptr 
-         * @param data_blk_size 
-         * @return uint16_t 
          */
-        uint16_t CRC16(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size)
+        #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)
         {
-            uint16_t i, j;
-            uint16_t crc_table[256] = {
-                0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
-                0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
-                0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
-                0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
-                0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
-                0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
-                0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
-                0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
-                0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
-                0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
-                0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
-                0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
-                0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
-                0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
-                0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
-                0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
-                0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
-                0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
-                0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
-                0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
-                0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
-                0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
-                0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
-                0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
-                0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
-                0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
-                0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
-                0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
-                0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
-                0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
-                0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
-                0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
-            };
+        #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;
+            }
+            /**************************************************************/
 
-            for(j = 0; j < data_blk_size; j++)
+            #if 0
+            for(uint32_t i = 0; i < 256; i ++)
             {
-                i = ((uint16_t)(crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF;
-                crc_accum = (crc_accum << 8) ^ crc_table[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 crc_accum;
+            return temp;
         }
 
         /**
-         * @brief シリアルバルク通信
+         * @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, uint16_t buf_length)
+        bool Send_Bulk_Char(uint8_t *buf, const uint32_t buf_length)
         {
-            #if 0
+            #ifdef MBED_ENVIRONMENT
                 // ユーザ関数
+
+                return true;
             #else
                 for(uint32_t i = 0; i < buf_length; i ++)
                 {
@@ -111,59 +258,73 @@
             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 Construct a new XM430 object
-         * 
-         */
-        XM430()
-        {
-            // Header
-            tx_buf[0] = 0xFF;
-            tx_buf[1] = 0xFF;
-            tx_buf[2] = 0xFD;
-
-            // Reserved
-            tx_buf[3] = 0x00;
-        }
-
-        /**
          * @brief サーボのトルクをONにする
          * 
          * @param id サーボID
          * @return true 通信が正常に終了
          * @return false 通信が不正に終了
          */
-        bool Torque_ON(const uint8_t id)
+        bool Torque_ON(uint8_t id)
         {
-            uint16_t checksum;
             uint16_t length = 6;
-            uint8_t data = 1;       // Enable torque
+            uint8_t data[1];       // トルクON/OFF
 
             bool ret_val;
 
-            tx_buf[4] = id;
+            data[0] = 1;            // トルクON
+
+            Create_Packet(id, length, WRITE, TORQUE_ENABLE, data);
 
-            // Length
-            tx_buf[5] = (uint8_t)(length & 0xFF);
-            tx_buf[6] = (uint8_t)(length >> 8);
-
-            // Instruction
-            tx_buf[7] = WRITE;
+            ret_val = Send_Bulk_Char(tx_buf, 13);
+            return ret_val;
+        }
 
-            // control register
-            tx_buf[8] = (uint8_t)(TORQUE_ENABLE & 0xFF);
-            tx_buf[9] = (uint8_t)(TORQUE_ENABLE >> 8);
+        /**
+         * @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
 
-            // Data
-            tx_buf[10] = (uint8_t)(data & 0xFF);
+            bool ret_val;
 
-            // Checksum
-            checksum = CRC16(0, tx_buf, ( 13-2 ));
-            tx_buf[11] = (uint8_t)(checksum & 0xFF);
-            tx_buf[12] = (uint8_t)(checksum >> 8);
+            data[0] = 0;            // トルクOFF
 
+            Create_Packet(id, length, WRITE, TORQUE_ENABLE, data);
 
             ret_val = Send_Bulk_Char(tx_buf, 13);
             return ret_val;
@@ -177,52 +338,52 @@
          * @return true 通信が正常に終了
          * @return false 通信が不正に終了
          */
-        bool Set_Pos(const uint8_t id, uint32_t pos)
+        bool Set_Pos(uint8_t id, uint32_t pos)
         {
-            uint16_t checksum;
             uint16_t length = 9;
-            uint32_t data = pos;       // Position ( 32 bit val )
+            uint8_t data[4];    // 位置データ
 
             bool ret_val;
 
-            tx_buf[4] = id;
-
-            // Length
-            tx_buf[5] = (uint8_t)(length & 0xFF);
-            tx_buf[6] = (uint8_t)(length >> 8);
-
-            // Instruction
-            tx_buf[7] = WRITE;
-
-            // control register
-            tx_buf[8] = (uint8_t)(GOAL_POSITION & 0xFF);
-            tx_buf[9] = (uint8_t)(GOAL_POSITION >> 8);
+            // 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);
 
-            // Data
-            tx_buf[10] = (uint8_t)(data & 0xFF);
-            tx_buf[11] = (uint8_t)((data >> 8) & 0xFF);
-            tx_buf[12] = (uint8_t)((data >> 16) & 0xFF);
-            tx_buf[13] = (uint8_t)((data >> 24) & 0xFF);
-
-            // Checksum
-            checksum = CRC16(0, tx_buf, ( 16-2 ));
-            tx_buf[14] = (uint8_t)(checksum & 0xFF);
-            tx_buf[15] = (uint8_t)(checksum >> 8);
-
-
+            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;
 
-    XM430 xm430;
+    dynamixel_servo_v2::XM430 xm430(PA_9, PA_10);
+
+    xm430.Torque_ON(servo_id);
+    
+    // 少し待つ処理が必要( サーボからの返答を受信する処理を入れたら不要になる )
+    #ifdef MBED_ENVIRONMENT 
+        wait(0.1); 
+    #endif
 
-    //xm430.Torque_ON(servo_id);
     xm430.Set_Pos(servo_id, 999);
+    
+    
+    #ifdef MBED_ENVIRONMENT
+    while(1)
+    {
+        
+    }
+    #endif
 
     return 0;
-}
\ No newline at end of file
+}
+
+